Fix: /tf and /static_tf topics' inconsistencies #2676

Arun-Prasad-V · 2023-03-31T04:10:21Z

Issues Fixed:

/static_tf topic is getting listed even when publish_tf:=false
When the tf_publish_rate param is dynamically changed from 0.0 to >0.0, /tf topic is not getting published
/tf topic has only the TFs of last started sensor and not the TFs of previously started sensors

…'s callback

SamerKhshiboun

Nice catch @Arun-Prasad-V ! Working good also on my environment.
As we fix this, I think we can add a warning also when disabling the dynamic tf
so, as we have in line 989:
ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate);
We can add something similar at line ~1002, something like:
ROS_WARN("Publishing dynamic camera transforms to topic (/tf) has been stopped");

so, when user do
ros2 param set /camera/camera tf_publish_rate 0.0
he will get warning about that

Nir-Az · 2023-04-03T11:50:08Z

realsense2_camera/src/parameters.cpp

@@ -37,7 +37,6 @@ void BaseRealSenseNode::getParameters()
                startDynamicTf();
            });
    _parameters_names.push_back(param_name);
-    startDynamicTf();


Does this change the behavior from default start publishing to not?

When the node is started, "startDynamicTf()" is getting called twice. So, the prints are also coming twice as shown below:

With this change, it will be called only once.

Basic testing is done. Will test for corner cases as well.

SamerKhshiboun · 2023-04-12T00:12:20Z

As I tested it now, if we put pulish_tf:=false,
and in another terminal runros2 topic list, we will still see /tf_static as topic (it is an empty one, not publishing anything)
I would prefer to not starting this topic at all.
We can do this by moving _static_tf_broadcaster init to be inside the publishStaticTransform
It will look something like this:

void BaseRealSenseNode::publishStaticTransforms(std::vector<rs2::stream_profile> profiles)
{
    // Publish static transforms
    if (_publish_tf)
    {
        if(!_static_tf_broadcaster)
        {
            // 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
        }
        
        for (auto &profile : profiles)
        {
            calcAndPublishStaticTransform(profile, _base_profile);
        }
.
.
.

This way, we will not see any of /tf_static and /tf topics if publish_tf:=false

SamerKhshiboun · 2023-04-12T00:21:04Z

realsense2_camera/launch/rs_launch.py

@@ -58,6 +58,7 @@
                           {'name': 'initial_reset',                'default': 'false', 'description': "''"},                           
                           {'name': 'allow_no_texture_points',      'default': 'false', 'description': "''"},                           
                           {'name': 'pointcloud.ordered_pc',        'default': 'false', 'description': ''},
+                           {'name': 'publish_tf',                   'default': 'true', 'description': 'enable/disable publishing tf'},


I would prefer to explain more in the description field, specially for these two parameters since they can confuse users.

publish_tf: [bool] enable/disable publishing static or dynamic TF

tf_publish_rate: [double] rate in HZ for publishing dynamic TF

Also, I would prefer to explanation and elaborate more in the README.md file, where we explain about publish_rate and tf_publish_rate
1- to explain that publish_tf:=false will not publish any static/dynamic TFs, even if tf_publish_rate is bigger than 0.0
2- to explain that publish_tf:=true, will automatically publish static TFs. If dynamic TFs are needed, user should set the tf_publish_rate for it, and it should be > 0.0
3- to mention that the default value is publish_tf:=true with tf_publish_rate:=0.0 which means publishing static TFs as default behavior.

SamerKhshiboun · 2023-04-12T00:34:36Z

Take into mind that this fix is not covering the whole problem from issue #2522
@Tristis116 rightly claims that not all transformations are transferred when publish Dynamic TFs (/tf topic), since we clear the TF messages. Reference: look for _static_tf_msgs.clear();
Every time we restarts a sensor, we clear these messages.
If we put some debug messages, it looks like something like this:

The static broadcaster (tf2_ros::StaticTransformBroadcaster), as a Latched Topic, works like it accumulates inside him all the messages. So, clear of messages is not harming it. Every new sensor we turn on during runtime, the broadcaster will add it's TF to the rest of TFs already in his hand.
Turning off Sensors, will not change anything, and will not remove any TF msg from the published info, and this is okay.

But the dynamic broadcaster (tf2_ros::TransformBroadcaster )is not Latched topic, and it works like normal topics. It looks every time on the _static_tf_msgs we have, and tries to broadcast it. So usually, you will see in /tf topic only the last started sensor TFs, and that is a wrong behavior.

We need to figure out how to solve it, maybe to use a HashMap instead of vector of tf_msgs, (see std::vector<geometry_msgs::msg::TransformStamped> _static_tf_msgs;) where the KEY is the wanted profile or something like a string TF_FROM_TO, and then we clear/fill only by this key.

P.S.
I don't know why we publish dynamic TFs at all, since all the camera parts are statically connected.
So, in our Camera modules, dynamic TFs are the same as static TFs, but they are being published in a specific rate, while the static TFs are being published only when needed (usually once..)

…o false

Arun-Prasad-V · 2023-04-21T10:29:51Z

Added fix for the issue "/tf topic has only the TFs of last started sensors"

Root cause:

The TFs are stored in static_tf_msgs vector
This vector is getting cleared everytime a sensor is started - that's why /tf has only TFs of last started sensor

Fix details:

Not clearing static_tf_msgs vector everytime
When a sensor is started, add its corresponding TF frames to that vector
When a sensor is stopped, erase its corresponding TF frames alone in that vector. So that the TFs of other sensors will be alive
- The corresponding TFs to be erased are searched and identified in the vector using frame_id & child_frame_id

SamerKhshiboun · 2023-04-24T16:24:39Z

Great job @Arun-Prasad-V.
I think now we cover all flows and it is working good.
I gave you some comments, and I think we can move all base_realsense::<TFs functions> to a new cpp file tfs.cpp to be dedicated to all TF methods (no need for header file. see parameters.cpp for example).
If you do that, remember to add this cpp to the cmake file

SamerKhshiboun · 2023-04-24T16:04:52Z

realsense2_camera/src/base_realsense_node.cpp

+                                            const std::string& child_frame_id)
+{
+    std::vector<geometry_msgs::msg::TransformStamped>::iterator it;
+    struct find_tf


I think it is better to move this struct outside this method.
and maybe name it with another name since using find_tf with find_if might be confusing while reading the code.

SamerKhshiboun · 2023-04-24T16:07:16Z

realsense2_camera/src/rs_node_setup.cpp

+        if (_publish_tf)
+        {
+            std::lock_guard<std::mutex> lock_guard(_publish_tf_mutex);
+            publishStaticTransforms();


if we publish static transforms only at this point, then I think we should change calcAndPublishStaticTransforms name to be just calculcateStaticTransforms
or even calcualteTransforms since we use it for static and dynamic TFs
This is affecting line 358

SamerKhshiboun · 2023-04-24T16:13:39Z

realsense2_camera/src/rs_node_setup.cpp

+
+        if (_publish_tf)
+        {
+            std::lock_guard<std::mutex> lock_guard(_publish_tf_mutex);


I think this part from line 201 to line 217 should be in a separate function.
Like we have PublishStaticTransforms() which uses publish_static_tf inside it, we can have UnpublishStaticTransforms(<params>) which will use unpublish_static_tf inside it.

SamerKhshiboun · 2023-04-24T16:15:30Z

realsense2_camera/src/base_realsense_node.cpp

+        }
+        else
+        {
+            ROS_WARN("Currently not publishing dynamic camera transforms (/tf)");


Just a question, can you remind me when we will get to here ? In which scenario ?

SamerKhshiboun · 2023-04-24T16:22:12Z

realsense2_camera/src/base_realsense_node.cpp

    }
+
+    // intra-process do not support latched QoS, so we need to disable intra-process for this topic


I would move this part from line 1008 to line 1024 into a seperate function
resetStaticTransformBroadcaster(_static_tf_broadcaster)

SamerKhshiboun

LGTM

Arun-Prasad-V added 2 commits March 30, 2023 19:19

Avoid configuring dynamic_tf_broadcaster within tf_publish_rate param…

ec8023a

…'s callback

Avoid configuring dynamic_tf_broadcaster within tf_publish_rate's cb

b092f35

Arun-Prasad-V marked this pull request as ready for review March 31, 2023 04:11

Arun-Prasad-V marked this pull request as draft March 31, 2023 05:55

Nir-Az requested a review from SamerKhshiboun March 31, 2023 06:57

SamerKhshiboun reviewed Mar 31, 2023

View reviewed changes

Nir-Az reviewed Apr 3, 2023

View reviewed changes

Arun-Prasad-V added 3 commits April 4, 2023 00:06

Fix: Don't publish /tf when publish_tf is false

f032edb

Indentation corrections

1e38de7

Added 'publish_tf' param in rs launch files

ce50027

Arun-Prasad-V marked this pull request as ready for review April 10, 2023 06:10

SamerKhshiboun reviewed Apr 12, 2023

View reviewed changes

Arun-Prasad-V marked this pull request as draft April 12, 2023 04:16

Arun-Prasad-V and others added 3 commits April 13, 2023 00:25

Fix: Avoid /tf_static being broadcasted when 'publish_tf' param set t…

f36c390

…o false

Fixing /tf topic has only TFs of last started sensor

31f3cac

Merge branch 'IntelRealSense:ros2-development' into dynamic_tf_broadcast

dce6b74

Arun-Prasad-V marked this pull request as ready for review April 21, 2023 11:14

Updated TF descriptions in launch py and readme

7ebd9ce

Arun-Prasad-V requested a review from SamerKhshiboun April 24, 2023 04:34

Arun-Prasad-V changed the title ~~Avoid configuring dynamic_tf_broadcaster within tf_publish_rate's cb~~ Fix: /tf and /static_tf topics' inconsistencies Apr 25, 2023

SamerKhshiboun reviewed Apr 25, 2023

View reviewed changes

Arun-Prasad-V added 2 commits April 25, 2023 18:26

Moved all TF related functions to tfs.cpp

82eb970

Revamped the TF related code

3e87a6c

SamerKhshiboun approved these changes Apr 27, 2023

View reviewed changes

Nir-Az merged commit eb1458f into IntelRealSense:ros2-development Apr 27, 2023

SamerKhshiboun mentioned this pull request Jun 1, 2023

[ros2-beta] Missing TFs in "/tf" topic when tf_publish_rate > 0 #2522

Closed

Arun-Prasad-V deleted the dynamic_tf_broadcast branch August 10, 2023 05:34

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix: /tf and /static_tf topics' inconsistencies #2676

Fix: /tf and /static_tf topics' inconsistencies #2676

Arun-Prasad-V commented Mar 31, 2023 •

edited

Loading

SamerKhshiboun left a comment •

edited

Loading

Nir-Az Apr 3, 2023

Arun-Prasad-V Apr 3, 2023

SamerKhshiboun commented Apr 12, 2023

SamerKhshiboun Apr 12, 2023 •

edited

Loading

SamerKhshiboun commented Apr 12, 2023 •

edited

Loading

Arun-Prasad-V commented Apr 21, 2023

SamerKhshiboun commented Apr 24, 2023 •

edited

Loading

SamerKhshiboun Apr 24, 2023

SamerKhshiboun Apr 24, 2023

SamerKhshiboun Apr 24, 2023

SamerKhshiboun Apr 24, 2023

SamerKhshiboun Apr 24, 2023

SamerKhshiboun left a comment

		}

		// intra-process do not support latched QoS, so we need to disable intra-process for this topic

Fix: /tf and /static_tf topics' inconsistencies #2676

Fix: /tf and /static_tf topics' inconsistencies #2676

Conversation

Arun-Prasad-V commented Mar 31, 2023 • edited Loading

SamerKhshiboun left a comment • edited Loading

Choose a reason for hiding this comment

Nir-Az Apr 3, 2023

Choose a reason for hiding this comment

Arun-Prasad-V Apr 3, 2023

Choose a reason for hiding this comment

SamerKhshiboun commented Apr 12, 2023

SamerKhshiboun Apr 12, 2023 • edited Loading

Choose a reason for hiding this comment

SamerKhshiboun commented Apr 12, 2023 • edited Loading

Arun-Prasad-V commented Apr 21, 2023

SamerKhshiboun commented Apr 24, 2023 • edited Loading

SamerKhshiboun Apr 24, 2023

Choose a reason for hiding this comment

SamerKhshiboun Apr 24, 2023

Choose a reason for hiding this comment

SamerKhshiboun Apr 24, 2023

Choose a reason for hiding this comment

SamerKhshiboun Apr 24, 2023

Choose a reason for hiding this comment

SamerKhshiboun Apr 24, 2023

Choose a reason for hiding this comment

SamerKhshiboun left a comment

Choose a reason for hiding this comment

Arun-Prasad-V commented Mar 31, 2023 •

edited

Loading

SamerKhshiboun left a comment •

edited

Loading

SamerKhshiboun Apr 12, 2023 •

edited

Loading

SamerKhshiboun commented Apr 12, 2023 •

edited

Loading

SamerKhshiboun commented Apr 24, 2023 •

edited

Loading