From bac07361bd805f587600ee0f2f42d842e3bae1e5 Mon Sep 17 00:00:00 2001 From: Aarsh Thakker <53620577+aarsht7@users.noreply.github.com> Date: Fri, 16 Aug 2024 14:23:29 +0200 Subject: [PATCH] immt selection * individual marker type selection * update readme --- CMakeLists.txt | 3 +- README.md | 15 ++--- include/natnet_ros2/natnet_ros2.hpp | 4 +- launch/natnet_ros2.launch.py | 18 ++++-- scripts/helper_node_r2.py | 22 ++++++-- src/natnet_ros2.cpp | 55 ++++++++++++------ ui/helper.ui | 88 ++++++++++++++++++++++------- 7 files changed, 148 insertions(+), 57 deletions(-) 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..17b8afc 100644 --- a/README.md +++ b/README.md @@ -19,9 +19,9 @@ This package is only tested with the Natnet 4.0 and ROS 2 (Foxy and Humble) but ### Current Features: - 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) + - Rigid bodies are published as `geometry_msgs/PoseStamped` under name given in the Motive, i.e `//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 `//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 @@ -74,7 +74,7 @@ ros2 launch natnet_ros2 gui_natnet_ros2.launch.py #### Difficult way Using Non gui approach -`ros2 launch natnet_ros_cpp natnet_ros2.launch.py` +`ros2 launch natnet_ros2 natnet_ros2.launch.py` ##### Understanding the launch file Launch file `natnet_ros2.launch.py` contains the several configurable arguments. The details are mentioned in the launch file. Following are several important argument for the connection and the data transfer. Other connection arguments are for the advanced option. @@ -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 + +