immt selection

* individual marker type selection

* update readme

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

@@ -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/<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)
+ - Rigid bodies are published as `geometry_msgs/PoseStamped` under name given in the Motive, i.e `/<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 `/<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
 
@@ -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
-```
-
 <!-- ## 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();
         }