Construct TF listeners passing nodes, spinning on separate thread

[roncapat]

See #1182 - this is a re-assessment after 6 years.
Advantages: easier auditing of TF2 subscriptions across the ROS graph. The nav2 node(s) name(s) will appear for example when issuing ros2 topic info -v /tf.

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1182
Primary OS tested on	Ubuntu
Robotic platform tested on	proprietary simulation & HW
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

Construct tf2_ros::TransformListener instances passing a node pointer, so that no additional nodes with randomized names are spawned on the ROS graph (less pollution, better auditing), but enabling the spin_thread flag, so that we ensure TF subscriptions are not interleaved with other nav2-related callbacks in the same executor.

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[mergify]

@roncapat, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

Pull in / rebase on main once #5409 is merged to get CI to turn over. Sorry about that, we hit the wall with circleci limits and needing to load balance the builds.

Also makes sure to sign off with DCO

[SteveMacenski]

... huh. Alot of tests failed. I'm rerunning but if they fail again, I think this introduces a regression

[roncapat]

Also noticed right now... I will be able to investigate as early as next week, sorry. Will be interesting to see what is the cause, since in my real testing scenario it works impressively well. Hope to learn something and have a fix!

[SteveMacenski]

impressively well

How so? big perf boost? I wouldn't have expected that

[roncapat]

Nah I mean more like "without surprises" - but I am 99% sure that by using the node, it will benefit from enabled IPC on the /tf subscribers.

It has been maybe two years or so, I have pushed in the past some patches for IPC in the TransformListener, need to check again in which conditions it gets enabled or not.

[SteveMacenski]

yeah this is still failing completely - I think there's something awry here. I sampled 2 of the 16 tests and the lifecycle transition never completes while its waiting for a transform to be available (which seems awfully related, so I don't think its a CI fluke)

[roncapat]

I began to study deeper the tf2_ros::TransformListener.

What I assessed, basically, is that current nav2 code uses the constructor:
TransformListener(tf2::BufferCore & buffer, bool spin_thread = true, bool static_only = false)
Notice the default spin_thread = true.

This is to say, the "only" difference introduced in this PR is the node used by TransformListerer implementation, not the spinning logic, that is exactly kept the same.

Of course, here we are passing a LifecycleNode, from which the TransformListener costructor will extract a set of NodeInterfaces. I will probably focus in the upcoming days on possible subtle implications of this - for example, whether the LifecycleNode current state could influence the correct working of TransformListener.

Moreover, it seems the only place where we have problems in the tests is the costmap_2d_ros. Reverting modifications only for that node makes all the tests pass. This may be an hint on specific way of using the TransformListener that can cause such issue w.r.t. other use cases, restricting the "search area" for the issue.

Will update you if I discover something more in the upcoming days.

[roncapat]

Ok, I may have undestood the issue.
costmap_2d_ros expect TF to be received during the on_activate call.

Per https://design.ros2.org/articles/node_lifecycle.html, in the inactive mode ...the node will not receive any execution time to read topics, perform processing of data, respond to functional service requests, etc.

Two options:

• explicitly create a non-lifecycle-node and explicitly pass to the TransformListener -> node name can be customized***
• defer the canTransform call to the "active" state of costmap_2d_ros

*** https://github.com/ros2/geometry2/blob/2b1742c80a4e91a411e5798eec78573928391a7c/tf2_ros/src/transform_listener.cpp#L46-L56

---

My personal take (a bit phylosophic, take this with a grain of salt):

I fully understand why canTransform is called there, but I think that it reveals a (minor) flaw in the choice of adopting a LifecycleNode-based architecture - basically by expecting to receive something during the on_activate transition -this currently works because this responsibility is deferred to a "classic" node, hidden in TransformListener.
This also reveals that, when the costmap node is inactive, the listener is still receiving /tf messages - while being 100% strict, in principle, it should be "disabled" (not receiving) too.

[SteveMacenski]

Mhm, I don't think we can activate until we have all the inputs required to actually be able to process something. Unlike other things like having services available from other nodes that we can make sure are available intrinsically by the ordering of lifecycle transitions, the setting of the robot's initial pose is a user-application defined task (or SLAM if running SLAM) that we cannot know is completed without checking.

defer the canTransform call to the "active" state of costmap_2d_ros

We could move it to the already actived state, but then requests are able to be submitted without actually being processable. At the moment, I think we should leave this as-is but can be reopened designwise. I suppose we could have a timer or possibly in the update map thread check for this transform and have the similar delay after activation. That would complicate the implementation a bit, but nothing terrible. My biggest concern there is that we have a timeout feature for waiting for that transform. If we cannot return a failure on a state transition when that timeout is exceeded, then the server becomes in an unrecoverable state. If we have some ideas around that, I wouldn't object to a redesign of this handling.

This also reveals that, when the costmap node is inactive, the listener is still receiving /tf messages - while being 100% strict, in principle, it should be "disabled" (not receiving) too.

TF is not lifecycle enabled, so that's no surprise. This isn't doing 'work' or given 'execution time' on the application though so I think that's fine. The lifecycle transition quote you gave from the design document I think is talking about the work in the transition function to block the completion of transition. While perhaps TF could technically do some work given a message, the transition isn't dependent on it, so that's fine. How we use TF to block for the available transform however does break that principle. But your point is understood. If we wanted to be aggressively pure on Lifecycle Nodes, there are many ROS libraries that would need to have activate/deactivate functions enabled.

Anyway, but why does change with using the spinning thread and node not work? I'm a little unclear as to that, since there is no lifecycle subscription for the subscription within TF to not be processing. The spin thread should be creating its own executor spun in its own thread as well so that should be all working independently, from first glance.

[roncapat]

Anyway, but why does change with using the spinning thread and node not work? I'm a little unclear as to that, since there is no lifecycle subscription for the subscription within TF to not be processing. The spin thread should be creating its own executor spun in its own thread as well so that should be all working independently, from first glance.

I may have misunderstood the design document (at least the part I quoted), but it seems to me that since we are using the Lifecycle Node interfaces to create the subscription inside TransformListener, also that subscription will not receive any execution time to read topics. I don't understand how, will try to study more the rclcpp_lifecycle code.

[SteveMacenski]

I think this has more to with the TF code spinning w.r.t. the main node. Maybe some print statements would help clarify. I think we should understand the 'why' before we merge, but once we do I'm happy to merge assuming we don't find its just hiding something buggy (or we find that this change is actually buggy and costmap2D is the only place showing the problem to us immediately)

[roncapat]

I agree!
I think I have found the issue. Took this screenshot while running some failing tests:

/tf and /tf_static get namespaced!

Will check a simple way to avoid this.

---

EDIT 1:
I tried to add remappings like

    rclcpp::NodeOptions().arguments({
    "--ros-args", "-r", std::string("__ns:=") + nav2_util::add_namespaces(parent_namespace, local_namespace),
    "--ros-args", "-r", nav2_util::add_namespaces(parent_namespace, local_namespace) + "/tf:=/tf",
    "--ros-args", "-r", nav2_util::add_namespaces(parent_namespace, local_namespace) + "/tf_static:=/tf_static",
    "--ros-args", "-r", "tf:=/tf",
    "--ros-args", "-r", "tf_static:=/tf_static",
    "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"),

in costmap_2d_ros.cpp but they won't work.
Apparently the issue lies in the nav2_system_test launchfile test_error_codes_launch.py, where:

    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]

is found, like in many nav2_bringup files.
Emptying that list will do the trick. Of course, it is not the right solution.
It seems that remapping from CLI forcefully override any hardcoded override.

[mergify]

@roncapat, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@roncapat, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@roncapat, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[roncapat]

(temporarily applied #5381 to solve CircleCI faliures, but seems the cache kicks in, how do I solve?)

[roncapat]

Managed to run all tests locally (it has been hard to set up everything because normally I skip building stuff like slam_toolbox, has troubles with yaml-cpp as its dependency and many more issues...).

I can confirm that the issue lies in the typical remap setup present in many navigation2 and nav2_minimal_turtlebot_simulation launch files.

Currently, we find this snippet:

    # Map fully qualified names to relative ones so the node's namespace can be prepended.
    # In case of the transforms (tf), currently, there doesn't seem to be a better alternative
    # https://github.com/ros/geometry2/issues/32
    # https://github.com/ros/robot_state_publisher/pull/30
    # TODO(orduno) Substitute with `PushNodeRemapping`
    #              https://github.com/ros2/launch_ros/issues/56
    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]

which basically forces /tf and /tf_static to be relative topic names. There is no way to block this from happening inside the code. CLI args takes the precedence. Since we are now piggy-backing TransformListener on existing nodes, this means that the node namespace gets forcefully composed to /tf and /tf_static.

Locally, to pass all tests, I temporarily emptied the remappings list.

Now, IMHO we need to find a proper way to:

• retain the overall launch file goal of "namespacing a robot" alongside its /tfs.
• avoid that such rules apply to "nested" nodes

My feeling is that we should change all occurencies of the above with more specific rules that keeps the topics absolute, but namespaced as the user desires:

   namespace = LaunchConfiguration('namespace')
   ...
   remappings = [
     ('/tf', `/` + namespace + '/tf'),
     ('/tf_static', `/` + namespace + '/tf_static')
   ]

NOTE: this won't work / is not valid code (take it as pseudocode), since namespace is a LaunchConfiguration object, hence the string is not already valid. Something like the workings of PathJoinSubstitution should be implemented to join the namespace to the topic name, prepending the "/" if the namespace is non-empty.

@SteveMacenski what do you think?

[SteveMacenski]

Pull in main and you should be good to go on the TF headers now.

I think maybe we should merge in the non-costmap ones first to not let the enemy of perfection be forward progress. I'm OK with partial solutions that gets us 80%+ of the way there (as long as the 20% isn't disruptive, which this isn't).

Something to consider is that we have joinWithParentNamespace in Costmap 2D for similar reasons used in the layers for interpreting the topic to use for a relative sensor topic. This strips out the node's namespace but applies the global system namespace so that relative topics can be used (its relatively new). That seems to me to be very similar to what you want here, though I'm not sure if you can apply it to the NodeOptions for the costmap node without the CLI taking over. I think (?) the only way to use that would be to:

• Get the node options from the parent costmap node (which we do in main), find the TF remapping from the CLI (should be there?), and manually modify it using that function
• Change the TF Listener API to accept a topic name to use for the TF topic as an optional argument.

This is largely only required for costmap 2D due to legacy reasons of having the costmap node in its own node name's namespace. Unfortunately this is just debt we need to incur for users to have a smooth ROS 1 -> ROS 2 transition. In a few years we can strip that out, but by that time I'm hoping that Costmap 2D is depreciated for something else ( 🤞 )

[roncapat]

Pull in main and you should be good to go on the TF headers now.

Just pushed! Thanks.

I think maybe we should merge in the non-costmap ones first to not let the enemy of perfection be forward progress. I'm OK with partial solutions that gets us 80%+ of the way there (as long as the 20% isn't disruptive, which this isn't).

Good for me, I kept only non-costmap-related changes.

Something to consider is that we have joinWithParentNamespace in Costmap 2D for similar reasons used in the layers for interpreting the topic to use for a relative sensor topic. This strips out the node's namespace but applies the global system namespace so that relative topics can be used (its relatively new). That seems to me to be very similar to what you want here, though I'm not sure if you can apply it to the NodeOptions for the costmap node without the CLI taking over. I think (?) the only way to use that would be to:

• Get the node options from the parent costmap node (which we do in main), find the TF remapping from the CLI (should be there?), and manually modify it using that function

CLI remaps are taken from the rclcpp::context -> you can't see them in the NodeOptions unfortunately

• Change the TF Listener API to accept a topic name to use for the TF topic as an optional argument.

Not sure this is the right approach. External CLI remaps would always kick in + I don't see added values as those names are already re-mappable via NodeOptions. The issue here lies in the CLI remaps, it's not a geometry2 issue per-se.

This is largely only required for costmap 2D due to legacy reasons of having the costmap node in its own node name's namespace. Unfortunately this is just debt we need to incur for users to have a smooth ROS 1 -> ROS 2 transition. In a few years we can strip that out, but by that time I'm hoping that Costmap 2D is depreciated for something else ( 🤞 )

I believe that the costmap sub-namespace is ok to keep, in fact I was not proposing to remove it. Instead, my proposal would have just required to write possibly better (?) remap rules in launch file to let everything work as usual. But we can merge this and maybe track down the proposal / residual TransformListener to address in a separate Issue I will open after merge :)

[roncapat]

CI failure due to (unrelated?) code coverage failure.
As I have just changed a bunch of params towards an external library, I think it is not my fault this time 🥲

[SteveMacenski]

OK! Merged!

[mini-1235]

Can we backport this to Jazzy?

[SteveMacenski]

Sure -- Tags and Mergify, do yo thing!