individual marker type selection

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()
+#ament_package()

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/<body-name>/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/<body-name>/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/<name-in-config-file>/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 `/<body-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 `/<name-in-config-file>/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
-```
-
 <!-- ## Citation
 If you use this software, please consider citing it [from here](https://hal.science/hal-04150950) -->

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<int32_t,std::string> ListRigidBodies; 
     std::map<std::string, rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseStamped>::SharedPtr> RigidbodyPub;
     std::map<std::string, rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>::SharedPtr> RigidbodyMarkerPub;
-    std::map<std::string, rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseStamped>::SharedPtr> IndividualMarkerPub;
+    std::map<std::string, rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseStamped>::SharedPtr> IndividualMarkerPosePub;
+    std::map<std::string, rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>::SharedPtr> IndividualMarkerPointPub;
     rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::PointCloud>::SharedPtr PointcloudPub;
     sensor_msgs::msg::PointCloud msgPointcloud;
     std::unique_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster;

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)  
-        ])
+        ])

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()
+    main()

src/natnet_ros2.cpp

@@ -32,6 +32,7 @@ NatNetNode::NatNetNode(rclcpp::NodeOptions& node_options) : LifecycleNode("natne
     declare_parameter<bool>("pub_rigid_body_marker", false);
     declare_parameter<bool>("pub_individual_marker", false);
     declare_parameter<bool>("pub_pointcloud", false);
+    declare_parameter<std::string>("individual_marker_msg_type","PoseStamped");
     declare_parameter<std::string>("global_frame", "world");
     declare_parameter<bool>("remove_latency",false);
     declare_parameter<std::string>("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<std::vector<std::string>>("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<geometry_msgs::msg::PoseStamped>(object_list[i].name+"/pose", rclcpp::QoS(1000));
+            	if (immt=="PoseStamped")
+                    {IndividualMarkerPosePub[object_list[i].name] = create_publisher<geometry_msgs::msg::PoseStamped>(object_list[i].name+"/pose", rclcpp::QoS(1000));};
+                if (immt=="PointStamped")
+                    {IndividualMarkerPointPub[object_list[i].name] = create_publisher<geometry_msgs::msg::PointStamped>(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();
         }