Enable goals to be sent in other frames than the pre-configured global_frame

[meyerj]

This patch was meant to address the issue described in #3894, at least partially: Some ROS and behavior tree nodes do not check the header frame_id of the goal and assume that goals are provided in their respective global frame.

I tested this patch with our own setup for an Ackermann-steered quad-like robotic platform, and also with the tb3_simulation_launch.py from the Getting Started guide. While this patch does solve the issue for the former, unfortunately I could not reproduce the issue with the latter though. We are using the nav2_smac_planner/SmacPlannerHybrid planner and the nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController with the navigate_w_replanning_only_if_path_becomes_invalid.xml behavior tree, so a different setup than the Turtlebot example setup.

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3894
Primary OS tested on	Ubuntu
Robotic platform tested on	- Custom Ackermann-steered quad-like robotic platform - tb3_simulation_launch.py from the Getting Started guide

---

Description of contribution in a few bullet points

• The stamped goal pose will be transformed to the configured global_frame of the bt_navigator node in NavigateToPoseNavigator::initializeGoalPose() before initialization of the BehaviorTree blackboard.

  The following behavior nodes maintained in this repository consume the goal input and may be affected:

  • ComputePathToPose
    • forwarded to the ComputePathToPose action server (planner_server) in the action request.
    • The planner_server transforms a goal to its own global frame, if that would differ from the global frame of the bt_navigator node. Otherwise the transform by the planner_server is a no-op.
  • NavigateToPose
    • forwarded to the NavigateToPose action server (bt_navigator) in the action request.
    • This action is not used by any behavior tree example in this repository. If it would be used in a behavior tree that implements the NavigateToPose action, that would be a recursion?
  • GoalReached
    • for comparison of the current pose in the goal pose.
    • Before this patch the header frame_id of the goal was not taken into account here. If this node is part of a behavior tree executed by the bt_navigator node, then the goal on the blackboard was already transformed to the global frame before now, and the change in GoalReachedCondition::isGoalReached() would not strictly be necessary because the goal.header.frame_id and the global frame do already match. However, because the input of this node is a stamped goal pose and it may also be used in other contexts, I suggest to do the comparison in the goal's frame in any case. The value read global_frame is redundant and only applied if the goal.pose.frame_id is empty.

Description of documentation updates required from your changes

• None. Implications of sending goal poses or paths in other than the respective configured global and local frame is not documented explicitly, or I have not found it yet.

---

Future work that may be required in bullet points

• Apply a similar patch to NavigateThroughPosesNavigator, for consistency?

• The planner_server does already transform the goal point(s) to the global frame here and here, since Fix #2186 #2222. However, the controller_server does not and it depends on the controller implementation whether the transform to the local frame is applied only once upon receiving a new plan or continuously, at each call of computeVelocityCommands(). It also transforms the goal pose to the local frame (the "global frame" of the local costmap) in ControllerServer::isGoalReached() (since Bugfix tf lookup of goal pose in nav2_controller #2780).

  In case of the controller_server not transform the goal path initially may be desirable, because updates of the transform between the global and local frame are supposed to have a direct effect on the controller for a path provided in map coordinates, without the need to replan. But maybe it would be good to document this implicit behavior better? Or even to add a parameter and optionally transform the received path only once, in ControllerServer::computeControl()?

• The configured goal checker used by the controller server may disgree with the behavior tree's GoalReachedCondition, which is only based on the Euclidian 2D distance. Depending on the concrete behavior tree this may have unwanted implications? However, I noticed that this condition node is not even used by any of the behavior trees in nav2_bt_navigator, so this concern cannot be the culprit of the behavior described in Goal frame_id is not respected consistently #3894.

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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

[SteveMacenski]

I generally approve of both of these actions - but is this exhaustive? It seems to me that potentially other updates wrt global_frame could be made to other BT nodes. I don't want to only partially solve the issue. From my eye quickly glancing at the BT nodes that may also directly benefit from this change:

• https://github.com/ros-planning/navigation2/blob/main/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp#L62

[SteveMacenski]

In case of the controller_server not transform the goal path initially may be desirable, because updates of the transform between the global and local frame are supposed to have a direct effect on the controller for a path provided in map coordinates, without the need to replan. But maybe it would be good to document this implicit behavior better? Or even to add a parameter and optionally transform the received path only once, in ControllerServer::computeControl()?

The transform between map and odom (the typical global and local frames) can change over time, so it is actually important to update the path each iteration to account for drift / odometric motion. Can you clarify what you think is implicit and might be desired to disable?

The configured goal checker used by the controller server may disgree with the behavior tree's GoalReachedCondition

Absolutely, yes. This is a known thing since they do analogous operations. They're meant to be used mutually exclusively or where the behavior tree has definitionally "more liberal" definitions of reached-ness than the controller server's (in whatever definition is sensible for the formulation of the Goal Checker).

That, at least, is my intent with it. If there's a better way to homogolate them (perhaps make the GoalReached condition send a service request to the controller server's goal checker?), I'm all ears. This is potentially to be used outside of the usual path-tracking modes though; for instance if doing some kind of control loop in the BT itself independent of the Controller/Planner servers

---

Also linter is unhappy

--- src/navigators/navigate_to_pose.cpp
+++ src/navigators/navigate_to_pose.cpp.uncrustify
@@ -214,2 +214,3 @@
-        goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
-        feedback_utils_.transform_tolerance)) {
+      goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
+      feedback_utils_.transform_tolerance))
+  {


-  if (goal.header.frame_id.empty()) goal.header.frame_id = global_frame_;
+  if (goal.header.frame_id.empty()) {goal.header.frame_id = global_frame_;}

[SteveMacenski]

@meyerj its been 2 weeks - can you please respond to the review so we can get this resolved?

[meyerj]

In case of the controller_server not transform the goal path initially may be desirable, because updates of the transform between the global and local frame are supposed to have a direct effect on the controller for a path provided in map coordinates, without the need to replan. But maybe it would be good to document this implicit behavior better? Or even to add a parameter and optionally transform the received path only once, in ControllerServer::computeControl()?

The transform between map and odom (the typical global and local frames) can change over time, so it is actually important to update the path each iteration to account for drift / odometric motion. Can you clarify what you think is implicit and might be desired to disable?

Maybe "implicit" is not the right word: What I meant is that some action servers transform the goals (or whatever input pose?) only once initially, and others keep looking up the transform continuously in each update cycle, like the controller. I think the behavior is fine as-is and no extra parameter is needed. But the navigator actions now transform the goal(s) only once initially, although multiple replanning steps may be triggered while running the behavior. Those decisions do have an effect on the behavior if the goals are not provided in the respective pre-configured frame, and hence would be worth to be mentioned in the documentation somewhere?

Or was that even by design that another trigger of the planner action (e.g. ComputePathToPose) may actually run with another transformed goal pose, if the transform to the global frame changed in between? That would be an indirect way to update the goal pose then, that is not covered by the GoalUpdated condition, for example, and this patch would be invalid.

[SteveMacenski]

But the navigator actions now transform the goal(s) only once initially, although multiple replanning steps may be triggered while running the behavior.

I think we've beaten this topic with a stick. You should navigate with respect to some external frame. This is standard practice but certainly an explicit remark about it does not hurt in documentation, I would agree. Use odom if you're doing local moves without desiring for global localization correction -- or map for longer term actions that require updates in positioning from localization. This is established in REP 105 for ROS and hasn't changed since the early days of robotics for a reason. Navigating w.r.t. an internal frame at an original time is incredibly fragile due to transformation buffer rolling sizes, among others.

[SteveMacenski]

With those small updates, this looks good to go to me! Please rebase or merge in main so that CI is green

[meyerj]

Navigating w.r.t. an internal frame at an original time is incredibly fragile due to transformation buffer rolling sizes, among others.

Not necessarily imho: It only implies that the transformation to the internal frame needs to happen only once early on, when the goals are received and before buffer timeouts could occur, as it is done with this patch now. rviz for example sends goal poses in whatever frame is selected as the fixed frame, and with a "now" timestamp. (Remeber to run rviz with the use_sim_time parameter set when running in simulation!). So strictly speaking that implies that the goal is meant to be transformed to the map frame (global_frame) only once, because if the goal's header stamp would be respected by the navigation stack and buffers would have infinite size, the transform would not change anymore while following the goal (with the exception of static transform updates). On the other hand, if the goal would be provided with a zero timestamp by the action client or publisher, that indicates that it is meant to be transformed repeatedly on each planner or behavior cycle using the latest available transform at the time, which is what actually happened in the navigation stack before this patch, just not consistently in all action nodes, and error-prone indeed because tf buffers are not infinite and the goal stamps are usually not zero.

[SteveMacenski]

It only implies that the transformation to the internal frame needs to happen only once early on, when the goals are received and before buffer timeouts could occur, as it is done with this patch now

That's also what you could do with the request; transform it into odom frame and send it. That's SOP. See things like the spin/backup behaviors.

On the other hand, if the goal would be provided with a zero timestamp by the action client or publisher, that indicates that it is meant to be transformed repeatedly on each planner or behavior cycle using the latest available transform at the time,

That is not a correct understanding of the intent, but may be able to be leveraged in the way you describe. A zero timestamp says to use the most recently available transformation in TF transformations when its transformed, not to do so every single time in all cases where TF is looked up. That is again, incredibly fragile and highly not recommended. So many of the things needed to make robotics highly accurate after you hit a POC level comes down to time synchronization and properly propagating timestamps.