Fix: /tf and /static_tf topics' inconsistencies

README.md

@@ -222,7 +222,12 @@
   - For example: ```depth_qos:=SENSOR_DATA```
   - Reference: [ROS2 QoS profiles formal documentation](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles)
 - **Notice:** ***<stream_type>*_info_qos** refers to both camera_info topics and metadata topics.
-- **tf_publish_rate**: double, positive values mean dynamic transform publication with specified rate, all other values mean static transform publication. Defaults to 0 
+- **tf_publish_rate**: 
+  - double, rate (in Hz) at which dynamic transforms are published
+  - Default value is 0.0 Hz *(means no dynamic TF)*
+  - This param also depends on **publish_tf** param
+    - If **publish_tf:=false**, then no TFs will be published, even if **tf_publish_rate** is >0.0 Hz
+    - If **publish_tf:=true** and **tf_publish_rate** set to >0.0 Hz, then dynamic TFs will be published at the specified rate
 
 #### Parameters that cannot be changed in runtime:
 - **serial_no**:
@@ -277,8 +282,11 @@
 - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings.
 - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin.
 - **publish_tf**:
-  - boolean, publish or not TF at all.
-  - Defaults to True.
+  - boolean, enable/disable publishing static and dynamic TFs
+  - Defaults to True
+    - So, static TFs will be published by default
+    - If dynamic TFs are needed, user should set the param **tf_publish_rate** to >0.0 Hz
+  - If set to false, both static and dynamic TFs won't be published, even if the param **tf_publish_rate** is set to >0.0 Hz
 - **diagnostics_period**: 
   - double, positive values set the period between diagnostics updates on the `/diagnostics` topic.
   - 0 or negative values mean no diagnostics topic is published. Defaults to 0.</br>

realsense2_camera/include/base_realsense_node.h

@@ -165,6 +165,8 @@ namespace realsense2_camera
                                const tf2::Quaternion& q,
                                const std::string& from,
                                const std::string& to);
+        void unpublish_static_tf(const std::string& frame_id,
+                                 const std::string& child_frame_id);
         void setup();
 
     private:
@@ -200,7 +202,7 @@ namespace realsense2_camera
         void updateExtrinsicsCalibData(const rs2::video_stream_profile& left_video_profile, const rs2::video_stream_profile& right_video_profile);
         void updateStreamCalibData(const rs2::video_stream_profile& video_profile);
         void SetBaseStream();
-        void publishStaticTransforms(std::vector<rs2::stream_profile> profiles);
+        void publishStaticTransforms();
         void startDynamicTf();
         void publishDynamicTransforms();
         void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset);

realsense2_camera/launch/rs_intra_process_demo_launch.py

@@ -54,6 +54,8 @@
                            {'name': 'enable_gyro',                  'default': 'false', 'description': "enable gyro stream"},                           
                            {'name': 'enable_accel',                 'default': 'false', 'description': "enable accel stream"}, 
                            {'name': 'intra_process_comms',          'default': 'true', 'description': "enable intra-process communication"}, 
+                           {'name': 'publish_tf',                   'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'},
+                           {'name': 'tf_publish_rate',              'default': '0.0', 'description': '[double] rate in HZ for publishing dynamic TF'},
                           ]
 
 def declare_configurable_parameters(parameters):

realsense2_camera/launch/rs_launch.py

@@ -58,7 +58,8 @@
                            {'name': 'initial_reset',                'default': 'false', 'description': "''"},                           
                            {'name': 'allow_no_texture_points',      'default': 'false', 'description': "''"},                           
                            {'name': 'pointcloud.ordered_pc',        'default': 'false', 'description': ''},
-                           {'name': 'tf_publish_rate',              'default': '0.0', 'description': 'Rate of publishing static_tf'},
+                           {'name': 'publish_tf',                   'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'},
+                           {'name': 'tf_publish_rate',              'default': '0.0', 'description': '[double] rate in Hz for publishing dynamic TF'},
                            {'name': 'diagnostics_period',           'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'},
                            {'name': 'decimation_filter.enable',     'default': 'false', 'description': 'Rate of publishing static_tf'},
                            {'name': 'rosbag_filename',              'default': "''", 'description': 'A realsense bagfile to run from as a device'},

realsense2_camera/src/base_realsense_node.cpp

@@ -121,15 +121,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
         ROS_INFO("Intra-Process communication enabled");
     }
 
-    // intra-process do not support latched QoS, so we need to disable intra-process for this topic
-    rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
-    options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
-    #ifndef DASHING
-    _static_tf_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node, tf2_ros::StaticBroadcasterQoS(), std::move(options));
-    #else
-    _static_tf_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node, rclcpp::QoS(100), std::move(options));
-    #endif
-
     _image_format[1] = CV_8UC1;    // CVBridge type
     _image_format[2] = CV_16UC1;    // CVBridge type
     _image_format[3] = CV_8UC3;    // CVBridge type
@@ -855,6 +846,37 @@ tf2::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotati
     return tf2::Quaternion(q.x(), q.y(), q.z(), q.w());
 }
 
+void BaseRealSenseNode::unpublish_static_tf(const std::string& frame_id,
+                                            const std::string& child_frame_id)
+{
+    std::vector<geometry_msgs::msg::TransformStamped>::iterator it;
+    struct find_tf
+    {
+        const std::string& frame_id;
+        const std::string& child_frame_id;
+
+        // Struct Constructor
+        find_tf(const std::string& frame_id, const std::string& child_frame_id) :
+                                frame_id(frame_id), child_frame_id(child_frame_id) {}
+
+        bool operator()(const geometry_msgs::msg::TransformStamped &static_tf_msg) const
+        {
+            return (static_tf_msg.header.frame_id == frame_id &&
+                    static_tf_msg.child_frame_id == child_frame_id);
+        }
+    };
+
+    // Find whether there is any TF with given 'frame_id' and 'child_frame_id'
+    it = std::find_if(_static_tf_msgs.begin(), _static_tf_msgs.end(),
+                                        find_tf(frame_id, child_frame_id));
+
+    // If found, erase that specific TF
+    if (it != std::end(_static_tf_msgs))
+    {
+        _static_tf_msgs.erase(it);
+    }
+}
+
 void BaseRealSenseNode::publish_static_tf(const rclcpp::Time& t,
                                           const float3& trans,
                                           const tf2::Quaternion& q,
@@ -981,28 +1003,42 @@ void BaseRealSenseNode::SetBaseStream()
     _base_profile = available_profiles[*base_stream];
 }
 
-void BaseRealSenseNode::publishStaticTransforms(std::vector<rs2::stream_profile> profiles)
+void BaseRealSenseNode::publishStaticTransforms()
 {
-    // Publish static transforms
-    if (_publish_tf)
+    // Since the _static_tf_broadcaster is a latched topic, the old transforms will
+    // be alive even if the sensors are dynamically disabled. So, reset the
+    // broadcaster everytime and publish the transforms of enabled sensors alone.
+    if (_static_tf_broadcaster)
     {
-        for (auto &profile : profiles)
-        {
-            calcAndPublishStaticTransform(profile, _base_profile);
-        }
-        if (_static_tf_broadcaster)
-            _static_tf_broadcaster->sendTransform(_static_tf_msgs);
+        _static_tf_broadcaster.reset();
     }
+
+    // intra-process do not support latched QoS, so we need to disable intra-process for this topic
+    rclcpp::PublisherOptionsWithAllocator<std::allocator<void>> options;
+    options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
+
+    #ifndef DASHING
+    _static_tf_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(_node, tf2_ros::StaticBroadcasterQoS(), std::move(options));
+    #else
+    _static_tf_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(_node, rclcpp::QoS(100), std::move(options));
+    #endif
+
+    _static_tf_broadcaster->sendTransform(_static_tf_msgs);
 }
 
 void BaseRealSenseNode::startDynamicTf()
 {
+    if (!_publish_tf)
+    {
+        ROS_WARN("Since the param 'publish_tf' is set to 'false',"
+                "the value set on the param 'tf_publish_rate' won't have any effect");
+        return;
+    }
     if (_tf_publish_rate > 0)
     {
         ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate);
         if (!_tf_t)
         {
-            _dynamic_tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(_node);
             _tf_t = std::make_shared<std::thread>([this]()
             {
                 publishDynamicTransforms();
@@ -1016,12 +1052,22 @@ void BaseRealSenseNode::startDynamicTf()
             _tf_t->join();
             _tf_t.reset();
             _dynamic_tf_broadcaster.reset();
+            ROS_WARN("Stopped publishing dynamic camera transforms (/tf)");
+        }
+        else
+        {
+            ROS_WARN("Currently not publishing dynamic camera transforms (/tf)");
         }
     }
 }
 
 void BaseRealSenseNode::publishDynamicTransforms()
 {
+    if (!_dynamic_tf_broadcaster)
+    {
+        _dynamic_tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(_node);
+    }
+
     // Publish transforms for the cameras
     std::unique_lock<std::mutex> lock(_publish_dynamic_tf_mutex);
     while (rclcpp::ok() && _is_running && _tf_publish_rate > 0)

realsense2_camera/src/parameters.cpp

@@ -37,7 +37,6 @@ void BaseRealSenseNode::getParameters()
                 startDynamicTf();
             });
     _parameters_names.push_back(param_name);
-    startDynamicTf();
 
     param_name = std::string("diagnostics_period");
     _diagnostics_period = _parameters->setParam<double>(param_name, DIAGNOSTICS_PERIOD);

realsense2_camera/src/rs_node_setup.cpp

@@ -195,6 +195,26 @@ void BaseRealSenseNode::stopPublishers(const std::vector<stream_profile>& profil
         }
         _metadata_publishers.erase(sip);
         _extrinsics_publishers.erase(sip);
+
+        if (_publish_tf)
+        {
+            std::lock_guard<std::mutex> lock_guard(_publish_tf_mutex);
+
+            unpublish_static_tf(_base_frame_id, FRAME_ID(sip));
+            unpublish_static_tf(FRAME_ID(sip), OPTICAL_FRAME_ID(sip));
+
+            if (profile.is<rs2::video_stream_profile>() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1)
+            {
+                unpublish_static_tf(_base_frame_id, ALIGNED_DEPTH_TO_FRAME_ID(sip));
+                unpublish_static_tf(ALIGNED_DEPTH_TO_FRAME_ID(sip), OPTICAL_FRAME_ID(sip));
+            }
+
+            if ((_imu_sync_method > imu_sync_method::NONE) && (profile.stream_type() == RS2_STREAM_GYRO))
+            {
+                unpublish_static_tf(FRAME_ID(sip), IMU_FRAME_ID);
+                unpublish_static_tf(IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID);
+            }
+        }
     }
 }
 
@@ -330,10 +350,13 @@ void BaseRealSenseNode::updateSensors()
                 {
                     startPublishers(wanted_profiles, *sensor);
                     updateProfilesStreamCalibData(wanted_profiles);
+                    if (_publish_tf)
                     {
                         std::lock_guard<std::mutex> lock_guard(_publish_tf_mutex);
-                        _static_tf_msgs.clear();
-                        publishStaticTransforms(wanted_profiles);
+                        for (auto &profile : wanted_profiles)
+                        {
+                            calcAndPublishStaticTransform(profile, _base_profile);
+                        }
                     }
 
                     if(is_profile_changed)
@@ -351,6 +374,11 @@ void BaseRealSenseNode::updateSensors()
                 }
             }
         }
+        if (_publish_tf)
+        {
+            std::lock_guard<std::mutex> lock_guard(_publish_tf_mutex);
+            publishStaticTransforms();
+        }
     }
     catch(const std::exception& ex)
     {