diff --git a/CMakeLists.txt b/CMakeLists.txt index d7e56c0..350f3b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} execute_process( COMMAND chmod "+x" "${CMAKE_CURRENT_SOURCE_DIR}/install_sdk.sh" + COMMAND chmod "+x" "${CMAKE_CURRENT_SOURCE_DIR}/scripts/helper_node_r2.py" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} ) @@ -148,4 +149,4 @@ if(BUILD_TESTING) endif() ament_auto_package() -#ament_package() \ No newline at end of file +#ament_package() diff --git a/README.md b/README.md index aa99bc4..a452725 100644 --- a/README.md +++ b/README.md @@ -20,8 +20,8 @@ This package is only tested with the Natnet 4.0 and ROS 2 (Foxy and Humble) but - Easy gui interface to control the node. - Rigid bodies are published as `geometry_msgs/PoseStamped` under name given in the Motive, i.e `/natnet_ros//pose`. Plus those are also broadcasting as `tf` frame for rviz - - Markers of the rigid bodies are published ad `geometry_msgs/PointStamped` unuder the name `/natnet_ros//marker#/pose` - - Unlabeled markers with the initial position and the name mentione in the `/config/initiate.yaml`are published as `geometry_msgs/PoseStamped` unuder the name `/natnet_ros//pose`. Plus those are also broadcasting as `tf` frame for rviz. The marker position is updated based on Iterative closest point (nearest neighbour) + - Markers of the rigid bodies are published ad `geometry_msgs/PointStamped` unuder the name `//marker#/pose` + - Unlabeled markers with the initial position and the name mentione in the `/config/initiate.yaml`are published as `geometry_msgs/PoseStamped` or `geometry_msgs/PointStamped` unuder the name `//pose`. Plus those are also broadcasting as `tf` frame for rviz. The marker position is updated based on Iterative closest point (nearest neighbour) - Unlabled markers can be also published as `sensor_msgs/PointCloud` - Different options for publishing and logging the data @@ -91,12 +91,5 @@ The question might arise on how to check the position of the single marker. For After configuring the `initiate.yaml`, in the launch file, enable the `pub_individual_marker`. Change the name of the config file in the argument `conf_file` if needed and launch the file. -##### Replacing existing package -You can easily replace the current package with this package. In the `natnet_ros2.launch.py` use the name of node to the node you currently using. For an example, -If you are using the `vrpn_client_node`, you can launch the node as follows. -``` -ros2 launch natnet_ros_cpp natnet_ros2.launch.py name:=vrpn_client_node -``` - diff --git a/include/natnet_ros2/natnet_ros2.hpp b/include/natnet_ros2/natnet_ros2.hpp index 1d5e782..11dafa6 100644 --- a/include/natnet_ros2/natnet_ros2.hpp +++ b/include/natnet_ros2/natnet_ros2.hpp @@ -97,6 +97,7 @@ class NatNetNode : public rclcpp_lifecycle::LifecycleNode bool pub_rigid_body = false; // To enable publishing the rigidbody as ros msg bool pub_rigid_body_marker = false; // To enable publishing individual markers of rigidbodies bool pub_individual_marker = false; // To publish the position of individual markers + std::string immt; bool pub_pointcloud = false; // To publish all the marker as pointcloud bool remove_latency = false; // To remove latency from the frame data published on ros publisher bool use_helper_node = false; // flag to chnage parameter while using helper node @@ -126,7 +127,8 @@ class NatNetNode : public rclcpp_lifecycle::LifecycleNode std::map ListRigidBodies; std::map::SharedPtr> RigidbodyPub; std::map::SharedPtr> RigidbodyMarkerPub; - std::map::SharedPtr> IndividualMarkerPub; + std::map::SharedPtr> IndividualMarkerPosePub; + std::map::SharedPtr> IndividualMarkerPointPub; rclcpp_lifecycle::LifecyclePublisher::SharedPtr PointcloudPub; sensor_msgs::msg::PointCloud msgPointcloud; std::unique_ptr tfBroadcaster; diff --git a/launch/natnet_ros2.launch.py b/launch/natnet_ros2.launch.py index c612781..f298523 100644 --- a/launch/natnet_ros2.launch.py +++ b/launch/natnet_ros2.launch.py @@ -2,9 +2,11 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch_ros.actions import SetParametersFromFile from launch_ros.actions import LifecycleNode from launch.actions import OpaqueFunction from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction from launch.substitutions import LaunchConfiguration import lifecycle_msgs.msg from launch.actions import EmitEvent @@ -30,6 +32,7 @@ def node_fn(context,*args, **kwargs): conf_file = LaunchConfiguration('conf_file') node_name = LaunchConfiguration('node_name') activate = LaunchConfiguration('activate') + immt = LaunchConfiguration('immt') params = [ {"serverIP": serverIP}, @@ -47,31 +50,34 @@ def node_fn(context,*args, **kwargs): {"log_internals": log_internals}, {"log_frames": log_frames}, {"log_latencies": log_latencies}, + {"individual_marker_msg_type" : immt}, ] - + conf_file_path = None if pub_individual_marker.perform(context): if len(conf_file.perform(context))==0 or conf_file.perform(context).split('.')[-1]!="yaml": raise RuntimeError("Provide yaml file for initial configuration") conf_file_path = os.path.join(get_package_share_directory( 'natnet_ros2'), 'config', conf_file.perform(context)) params.append(conf_file_path) + ld=[] node = LifecycleNode( package="natnet_ros2", executable="natnet_ros2_node", output="screen", name=node_name.perform(context), - namespace=node_name.perform(context), + namespace='', parameters=params, #arguments=[ # "--ros-args", # "--disable-external-lib-logs"] ) ld.append(node) + driver_configure = EmitEvent( event=ChangeState( lifecycle_node_matcher=matches_action(node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, + transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, ) ) ld.append(driver_configure) @@ -83,6 +89,7 @@ def node_fn(context,*args, **kwargs): ) ) ld.append(driver_activate) + return ld @@ -106,7 +113,8 @@ def generate_launch_description(): DeclareLaunchArgument('log_frames', default_value="False"), DeclareLaunchArgument('log_latencies', default_value="False"), DeclareLaunchArgument('conf_file', default_value="initiate.yaml"), - DeclareLaunchArgument('node_name', default_value="natnet_ros2"), + DeclareLaunchArgument('node_name', default_value="natnet_ros"), DeclareLaunchArgument('activate', default_value="false"), + DeclareLaunchArgument('immt', default_value="PoseStamped",description="publish type of individual markers PoseStampted or PointStamped"), OpaqueFunction(function=node_fn) - ]) \ No newline at end of file + ]) diff --git a/scripts/helper_node_r2.py b/scripts/helper_node_r2.py index 538bb63..349a4e2 100755 --- a/scripts/helper_node_r2.py +++ b/scripts/helper_node_r2.py @@ -82,6 +82,8 @@ def __init__(self,node:Node): self.ros_dist = os.environ['ROS_DISTRO'] self.pwd = os.environ['PWD'] + self.msg_types = ["PoseStamped","PointStamped"] + self.im_msg_type = self.msg_types[0] if os.path.exists(os.path.join(PKG_PATH, 'config','conf_autogen.yaml')): self.config_file = os.path.join(PKG_PATH, 'config','conf_autogen.yaml') self.name = 'natnet_ros2' @@ -90,6 +92,7 @@ def __init__(self,node:Node): "pub_rigid_body_marker": False, "pub_pointcloud": False, "pub_rigid_body_wmc": False, + "individual_marker_msg_type": "PoseStamped", } self.log_params = {"log_internals": False, "log_frames": False, @@ -144,6 +147,7 @@ def __init__(self,node:Node): self.pub_rbm.setEnabled(False) self.domain_id_spin.valueChanged.connect(self.select_network) self.client_ip_spin.valueChanged.connect(self.spin_clientIP) + self.msg_type_spin.valueChanged.connect(self.spin_msg_type) self.multicast_radio.clicked.connect(self.clicked_multicast) self.unicast_radio.clicked.connect(self.clicked_unicast) self.start_node.clicked.connect(self.start) @@ -188,14 +192,15 @@ def start(self): serverCommandPort:={scp} serverDataPort:={sdp} global_frame:={gf} \ remove_latency:={rl} pub_rigid_body:={prb} pub_rigid_body_marker:={prbm} \ pub_individual_marker:={pim} pub_pointcloud:={ppc} log_internals:={li} \ - log_frames:={lf} log_latencies:={ll} conf_file:={cf} activate:={activate} + log_frames:={lf} log_latencies:={ll} conf_file:={cf} activate:={activate} \ + immt:={immt} '''.format(dist=self.ros_dist,id=self.id, pwd=self.pwd, name=self.name, sip=self.conn_params["serverIP"], cip=self.conn_params["clientIP"],st=self.conn_params["serverType"],mca=self.conn_params["multicastAddress"], scp=self.conn_params["serverCommandPort"],sdp=self.conn_params["serverDataPort"],gf=self.conn_params["globalFrame"], rl=False,prb=self.pub_params["pub_rigid_body"],prbm=self.pub_params["pub_rigid_body_marker"], pim=self.pub_params["pub_individual_marker"],ppc=self.pub_params["pub_pointcloud"], li=self.log_params["log_internals"],lf=self.log_params["log_frames"],ll=self.log_params["log_latencies"], - cf='conf_autogen.yaml',activate=True) + cf='conf_autogen.yaml',activate=True,immt=self.pub_params["individual_marker_msg_type"]) self.start_node_thread = WorkerThread(command) self.start_node_thread.start() @@ -204,7 +209,7 @@ def stop(self): command=''' source /opt/ros/{dist}/setup.bash; export ROS_DOMAIN_ID={id}; - ros2 lifecycle set /{name}/{name} shutdown + ros2 lifecycle set /{name} shutdown '''.format(dist=self.ros_dist,id=self.id,name=self.name) #stop_node_thread = WorkerThread(command) #stop_node_thread.start() @@ -268,6 +273,9 @@ def pub_pc_setting(self): def pub_rbwmc_setting(self): self.Log('info','This functionality is not supported yet.') + + def pub_immt_setting(self): + self.pub_params["individual_marker_msg_type"] = self.im_msg_type #---------------------------------------------------------------------------------------- # NETWORK SELECTION THINGS @@ -335,6 +343,10 @@ def spin_clientIP(self): self.textServerIP.setText('.'.join(str(self.IP_LIST[self.client_ip_spin.value()-1]).split('.')[:-1],)+'.') self.Log('info','setting client ip '+str(self.conn_params["clientIP"])) + def spin_msg_type(self): + self.textMsgType.setText(self.msg_types[self.msg_type_spin.value()-1]) + self.im_msg_type = self.msg_types[self.msg_type_spin.value()-1] + def get_node_name(self): self.name = self.textNodeName.text() if self.name=='': @@ -377,6 +389,7 @@ def check_all_params(self): self.pub_rb_setting() self.pub_rbm_setting() self.pub_pc_setting() + self.pub_immt_setting() self.pub_params["pub_rigid_body_wmc"] = False self.log_frames_setting() self.log_internal_setting() @@ -400,6 +413,7 @@ def set_lcds(self,num_of_markers,x_position,y_position,z_position): self.z_position = z_position def yaml_dump(self): + self.im_msg_type = self.msg_types[self.msg_type_spin.value()-1] empty=0 object_names={'object_names':[]} if self.num_of_markers!=0: @@ -475,4 +489,4 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/src/natnet_ros2.cpp b/src/natnet_ros2.cpp index e325468..17ae833 100644 --- a/src/natnet_ros2.cpp +++ b/src/natnet_ros2.cpp @@ -32,6 +32,7 @@ NatNetNode::NatNetNode(rclcpp::NodeOptions& node_options) : LifecycleNode("natne declare_parameter("pub_rigid_body_marker", false); declare_parameter("pub_individual_marker", false); declare_parameter("pub_pointcloud", false); + declare_parameter("individual_marker_msg_type","PoseStamped"); declare_parameter("global_frame", "world"); declare_parameter("remove_latency",false); declare_parameter("serverIP", "192.168.0.100"); @@ -62,11 +63,11 @@ void NatNetNode::get_node_params() pub_rigid_body_marker = get_parameter("pub_rigid_body_marker").as_bool(); pub_individual_marker = get_parameter("pub_individual_marker").as_bool(); pub_pointcloud = get_parameter("pub_pointcloud").as_bool(); + immt = get_parameter("individual_marker_msg_type").as_string(); global_frame = get_parameter("global_frame").as_string(); remove_latency = get_parameter("remove_latency").as_bool(); if (pub_individual_marker) { - declare_parameter>("object_names", {}); object_names = get_parameter("object_names").as_string_array(); if(object_names.size()>0) @@ -330,7 +331,11 @@ void NatNetNode::get_info() { for (int i=0; i<(int) object_list.size(); i++) { - IndividualMarkerPub[object_list[i].name] = create_publisher(object_list[i].name+"/pose", rclcpp::QoS(1000)); + if (immt=="PoseStamped") + {IndividualMarkerPosePub[object_list[i].name] = create_publisher(object_list[i].name+"/pose", rclcpp::QoS(1000));}; + if (immt=="PointStamped") + {IndividualMarkerPointPub[object_list[i].name] = create_publisher(object_list[i].name+"/pose", rclcpp::QoS(1000));}; + } } } @@ -447,18 +452,31 @@ void NatNetNode::process_individual_marker(sMarker &data) object_list[update].x = data.x; object_list[update].y = data.y; object_list[update].z = data.z; - - geometry_msgs::msg::PoseStamped msgMarkerPose; - msgMarkerPose.header.frame_id = global_frame; - msgMarkerPose.header.stamp = remove_latency ? this->get_clock()->now()-frame_delay : this->get_clock()->now(); - msgMarkerPose.pose.position.x = data.x; - msgMarkerPose.pose.position.y = data.y; - msgMarkerPose.pose.position.z = data.z; - msgMarkerPose.pose.orientation.x = 0.0; - msgMarkerPose.pose.orientation.y = 0.0; - msgMarkerPose.pose.orientation.z = 0.0; - msgMarkerPose.pose.orientation.w = 1.0; - IndividualMarkerPub[object_list[update].name]->publish(msgMarkerPose); + if (immt=="PoseStamped") + { + geometry_msgs::msg::PoseStamped msgMarkerPose; + msgMarkerPose.header.frame_id = global_frame; + msgMarkerPose.header.stamp = remove_latency ? this->get_clock()->now()-frame_delay : this->get_clock()->now(); + msgMarkerPose.pose.position.x = data.x; + msgMarkerPose.pose.position.y = data.y; + msgMarkerPose.pose.position.z = data.z; + msgMarkerPose.pose.orientation.x = 0.0; + msgMarkerPose.pose.orientation.y = 0.0; + msgMarkerPose.pose.orientation.z = 0.0; + msgMarkerPose.pose.orientation.w = 1.0; + IndividualMarkerPosePub[object_list[update].name]->publish(msgMarkerPose); + } + + if (immt=="PointStamped") + { + geometry_msgs::msg::PointStamped msgMarkerPose; + msgMarkerPose.header.frame_id = global_frame; + msgMarkerPose.header.stamp = remove_latency ? this->get_clock()->now()-frame_delay : this->get_clock()->now(); + msgMarkerPose.point.x = data.x; + msgMarkerPose.point.y = data.y; + msgMarkerPose.point.z = data.z; + IndividualMarkerPointPub[object_list[update].name]->publish(msgMarkerPose); + } // creating tf frame to visualize in the rviz geometry_msgs::msg::TransformStamped msgTFMarker; @@ -506,7 +524,8 @@ void NatNetNode::del_info() ListRigidBodies.clear(); RigidbodyPub.clear(); RigidbodyMarkerPub.clear(); - IndividualMarkerPub.clear(); + IndividualMarkerPosePub.clear(); + IndividualMarkerPointPub.clear(); } CallbackReturnT NatNetNode::on_configure(const rclcpp_lifecycle::State & state) @@ -539,7 +558,11 @@ CallbackReturnT NatNetNode::on_activate(const rclcpp_lifecycle::State & state) } if(pub_individual_marker) { - for(auto const& i: IndividualMarkerPub) + for(auto const& i: IndividualMarkerPosePub) + { + i.second->on_activate(); + } + for (auto const& i: IndividualMarkerPointPub) { i.second->on_activate(); } diff --git a/ui/helper.ui b/ui/helper.ui index 018c100..480f14b 100644 --- a/ui/helper.ui +++ b/ui/helper.ui @@ -1,23 +1,4 @@ - - natnet_helper @@ -5406,6 +5387,75 @@ + + + + 10 + 630 + 111 + 21 + + + + + 50 + false + + + + Message Type + + + + + true + + + + 310 + 630 + 41 + 26 + + + + + 50 + false + + + + false + + + 1 + + + 2 + + + + + + 110 + 630 + 201 + 25 + + + + + 50 + false + + + + PoseStamped + + + true + +