Goal frame_id is not respected consistently

[meyerj]

Bug report

Required Info:

• Operating System:
  • Ubuntu Focal 20.04 (ROS and navigation2 built from source)
• ROS2 Version:
  • Humble (built from source)
• Version or commit hash:
  • 9876fe1
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

• Robot spawned at position (10, 0), or drove close to position (10, 0) before.
• Empty costmap, no obstacle detection involved.
• Request the navigate_to_pose action:

  ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "
  pose:
    header:
      frame_id: base_link
    pose:
      position:
        x: 10.0
        y: 0.0
      orientation:
        w: 1.0
  "

Expected behavior

The robot navigates to a point 10 meters ahead of the current pose, then the action succeeds. With the patch from #2222 (for #2186), which addressed the issue for the planner_server, I would have expected that it is possible to send goals in any frame, including base_link or whatever the robot's base frame is. But apparently the goal's header frame_id is still ignored in various other places, for example in NavigateToPoseNavigator::initializeGoalPose() and GoalReachedCondition::isGoalReached().

Like the original reporter of #2186, I would expect the header frame_id to be respected in each consumer of the current goal if it is represented as geometry_msgs/msg/PoseStamped, independent of whether it has been already transformed to the global navigation frame before or not. Otherwise, if the frame_id is expected to match the global frame by design and the header stamp also does not play a role, then the goal could have been represented by a geometry_msgs/msg/Pose message only internally and all action requests.

In this specific case of my example, where a goal is provided with respect to the robot frame with a zero header stamp (or any other frame local to the robot) then each attempt to transform it to the global fixed frame would return a pose relative to the latest available position, which cannot be reached by definition. So it would make sense to transform the goal only once in NavigateToPoseNavigator::initializeGoalPose(). But in other cases, where the goal is provided in any world-fixed frame other than the configured global frame for navigation, it may be desirable to transform the goal repeatedly using the latest available information from localization (e.g. odom or any earth-fixed frame when navigating using a global reference like GNSS), almost like the goal would have been updated each time the relevent tf transforms were updated. What would be the expected and/or preferred behavior here for submitting a pull request?

Actual behavior

Output (immediately after the goal was accepted):

Waiting for an action server to become available...
Sending goal:
     pose:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: base_link
  pose:
    position:
      x: 10.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
behavior_tree: ''

Goal accepted with ID: a97c3b328a784cfeaa4a653481b676eb

Result:
    result: {}

Goal finished with status: SUCCEEDED

Navigation stack output:

[bt_navigator-5] [INFO] [1698000807.702693163] [bt_navigator]: Begin navigating from current location (10.29, -0.00) to (10.00, 0.00)
[controller_server-1] [INFO] [1698000807.705938492] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1698000807.708074050] [controller_server]: Reached the goal!
[bt_navigator-5] [INFO] [1698000807.742920182] [bt_navigator]: Goal succeeded

The robot does not start to move and the action succeeds immediately. Likely because the start pose in the global map frame is already close to the goal's position coordinates, and therefore the controller's goal checker is happy early on and reports success.

Additional information

I am using the behavior tree navigate_w_replanning_only_if_path_becomes_invalid.xml and the nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController with the nav2_controller::SimpleGoalChecker.

[SteveMacenski]

base_link is not a consistent reference frame like map or odom. I don't see that either of the locations your point to ignore the frame -

blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose);
...
  if (!nav2_util::getCurrentPose(
      current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_))

But consider either consider the stamp in setting or a respective global_frame_ of the BT node that you can set appropriately for that node's context. You do need to carefully look at the frames being used in all the parts of your system to make sure its doing what you'd like it to do. Especially in the BT nodes, where they are basic primitives that can be used for any number of things so they are purposely generic to reconfigure to your particular needs. By default, they're set to the most common needs, but are setup to accommodate others.

I think the idea of trying to navigate relative to the base_link frame is an error as its not an external reference frame. If you want relative moves, use the odometry frame. If you want your robot to simple move forward some Nm, consider using the drive on heading BT node / behavior plugin.

I won't say that we can't be better about navigating in handling alternative reference frames, but I think the particular issue you're running into is due to using an invalid frame for navigation. It seems very strange to think of moving in the frame of the base link circa t=N. You'd need to consider your TF buffer sizes since that transformation at t=N might not be available forever to check status. If you use an external reference frame, then it would be. Also if you move in base_link at some older time t, then you still need some kind of external frame to measure off of for the purposes of costmaps and the initial pose of base_link that you're measuring relative to at the start time.

When measuring the difference in positions between base_link at t=0 and t=M, that difference is literally computed using odometry against the odometry frame. You're making a choice to actually go more round-about for something that is pretty standard and should work well. Perhaps you're not aware of this or didn't think about that?

There are alot of technical reasons why what you're trying to do is -- perhaps not wrong -- but probably a bad idea regardless of if we magically updated the stack to do what you propose. Costmap, TF buffer sizes, and other technical concerns remain which would all be resolved even just by using the odometry frame which is especially designed for exactly this kind of purpose - see REP 105.

[meyerj]

base_link is not a consistent reference frame like map or odom. I don't see that either of the locations your point to ignore the frame -

blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose);
...

They may not completely "ignore" it, in the sense that the goal->pose set on the blackboard still carries the original information including the header stamp and frame_id. It is only that some consumers (behavior nodes) then ignore it and have undefined behavior if the assumption that the goal->pose.header.frame_id matches feedback_utils_.global_frame does not hold. In NavigateToPoseNavigator::initializeGoalPose() itself it is only the log message that may be confusing then, if the current pose and the goal are expressed in different frames:

https://github.com/ros-planning/navigation2/blob/57f55c4dca45d93b61adbf7bcd24ad83106192bd/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp#L213-L216

  if (!nav2_util::getCurrentPose(
      current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_))

I don't see how goal->pose.header.frame_id or goal->pose.header.stamp play any role here? This function call returns the current pose of the robot with respect to the pre-configured global_frame_, typically "map", which may not be the same as the frame the goal is expressed in.

If a goal expressed in any other frame than the preconfigured ones is passed in an action request is considered an invalid use case, then I think the action servers must check that early on and return an error immediately. Otherwise, if the goal->header.frame_id is allowed to deviate from global_frame_ in some cases, whatever it may be, then either the pose must be transformed once early on when the goal is accepted, or any behavior node must not use the goal->pose.pose input without checking goal->pose.header and transforming it to whatever target frame it refers to. Or alternatively vice-versa, to lookup the current robot pose in the goal's frame and disregard the pre-configured global frame.

But consider either consider the stamp in setting or a respective global_frame_ of the BT node that you can set appropriately for that node's context. You do need to carefully look at the frames being used in all the parts of your system to make sure its doing what you'd like it to do. Especially in the BT nodes, where they are basic primitives that can be used for any number of things so they are purposely generic to reconfigure to your particular needs. By default, they're set to the most common needs, but are setup to accommodate others.

I was not aware that the global frame is something that is meant to be configurable at run-time? What is the added value of having it as an input port of behavior tree nodes? Is there any existing node that updates that at run-time, for example after a new goal was accepted, or is this just a way to passthrough a ROS parameter to a behavior node? I think that is exactly the issue I tried to describe: The fact that the bt_navigator node has such a ROS parameter is in my opinion already a design flaw: The frame_id of the goal, whether that is base_link or a "consistent reference frame like map or odom", is a property of the goal and may change with every new request, no? And if some other behavior nodes require the name of a fixed frame independent of the goal, for example the DistanceTraveledCondition, then that may be specific to that node and not necessarily a property of the behavior server.

I think the idea of trying to navigate relative to the base_link frame is an error as its not an external reference frame. If you want relative moves, use the odometry frame. If you want your robot to simple move forward some Nm, consider using the drive on heading BT node / behavior plugin.

Yes, I agree that navigating in a frame that moves with the robot may in general not be a good idea. In my case it was just for testing different planner and controller parameters in a completely empty map in simulation, and I simply did not care about where exactly the start pose and target pose is, only their relative pose. So I wanted to send a goal request relative to the robot pose without having to lookup that pose using tf first.

Another, more common scenario that does not work right now, maybe without users being aware of the issue, is sending goals with rviz or Foxglove Studio while any other frame than map (the pre-configured global frame) has been selected as the fixed frame. Those UI tools always send the goal in their respective fixed frame as far as I know (e.g. here or here). There is no way to select the goal's frame in the tool properties explicitly to transform it before publishing the topic or sending the action request. And selecting the base_link for a robot-centric view is a perfectly valid choice.

I won't say that we can't be better about navigating in handling alternative reference frames, but I think the particular issue you're running into is due to using an invalid frame for navigation. It seems very strange to think of moving in the frame of the base link circa t=N. You'd need to consider your TF buffer sizes since that transformation at t=N might not be available forever to check status. If you use an external reference frame, then it would be. Also if you move in base_link at some older time t, then you still need some kind of external frame to measure off of for the purposes of costmaps and the initial pose of base_link that you're measuring relative to at the start time.

TF buffer sizes would not be an issue if the goal (or sequence of waypoints) was transformed only once at the time it is accepted by the action server, which was my expectation initially before I took a closer look at the documentation and implementation details. If not, and the goal's original frame is kept as-is while the action is running, then any frame_id other than the pre-configured one, external or not, may result in either undefined behavior if the goal's frame is simply ignored, or tf lookup failures if not and the timestamp of the goal becomes too old. Also the map to odom transform is typically non-static...

But simply ignoring the goal frame_id and treating a goal with header frame odom as if it was with respect to the map frame and not do any tf lookups is undefined behavior, and hence a bug?

When measuring the difference in positions between base_link at t=0 and t=M, that difference is literally computed using odometry against the odometry frame. You're making a choice to actually go more round-about for something that is pretty standard and should work well. Perhaps you're not aware of this or didn't think about that?

There are alot of technical reasons why what you're trying to do is -- perhaps not wrong -- but probably a bad idea regardless of if we magically updated the stack to do what you propose. Costmap, TF buffer sizes, and other technical concerns remain which would all be resolved even just by using the odometry frame which is especially designed for exactly this kind of purpose - see REP 105.

Thanks, I think I am well aware of the ROS frame conventions and REP-105, and the tf design in general. I have not found a specific reference, but my assumption until now was that any consumer of ROS message with a header is responsible for checking that and to transform the data to "its own reference frame", if needed, and not make any assumption of which frame is chosen by the publisher or requester? In case of actions that may run for an extended period of time, the only question is then whether that is supposed to happen only once initially, or continuously while the goal is active, especially if the header stamp of the request is zero which typically means "the latest available".

But okay, let's ignore the maybe unusual case of sending goals in a frame somehow attached to the robot. Unless I missed something plain obvious, also sending goals in the odometry frame (odom) as you suggested would not work at the moment. The header.frame_id of the goal is ignored, at least by the behavior_server and any behavior node provided by package nav2_behavior_tree. There is not a single hit when searching for goal.header.frame_id or similar, execpt when it is set in unit tests. The only class that does "the right thing" in my opinion is TruncatePathLocal, which does not have a global_frame_id input and retrieves the robot pose in whatever frame the input path is expressed in to do its job:

https://github.com/ros-planning/navigation2/blob/a137347f6b7bc56f64dff8145ec3f5113cbb14d6/nav2_behavior_tree/plugins/action/truncate_path_local_action.cpp#L64

In contrast, the PlannerServer does transform the provided goal and the optional start pose to the global frame of the costmap initially here, and even has a helper function transformPosesToGlobalFrame() for that purpose (since #2222 mentioned before). Also the ControllerServer seems to lookup the transform of whatever frame the path and end goal is expressed in to the local costmap's global frame if needed at first glance (a related issue was apparently fixed in #2780). So maybe it is an issue of the behavior server only, or even only the GoalReachedCondition::tick(), which definitely compares the current_pose with respect to the global_frame_ with the goal.pose without another tf lookup:

https://github.com/ros-planning/navigation2/blob/a137347f6b7bc56f64dff8145ec3f5113cbb14d6/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp#L78-L88

I fixed this particular one by replacing global_frame_ by goal.header.frame_id in the call of nav2_util::getCurrentPose(), which solves at least some issues. The "global_frame" input port could be removed then... But the GoalReachedCondition behavior node and the planner could still disagree on what the actual goal pose is, because the planner transformed it at the time the plan was computed, while the GoalReachedCondition checks it much later, when the robot hopefully arrives without the need to replan in between. Or should the behavior tree always enfore periodic replanning if the goal's frame is not the global costmap frame?!? What if the value of the global_frame parameters of the behavior_server and the global_costmap nodes would differ? So the easiest and most consistent solution is probably to transform the goal pose(s) to the global frame initially at the time the action goal is accepted by the BT Navigator node, and optionally only store the result as a Pose instead of a PoseStamped?

Note: I found a related ROS answers post from >10 years ago: https://answers.ros.org/question/34684/header-frame_id/
It also suggest that sending goals in the base_link frame or even the kinect frame is a perfectly valid use case, and apparently that did work in the old days of move_base.

[SteveMacenski]

Note that lets try not to have these responses turn into walls of text or we're going to need to split out the discussion into one issue at a time.

I think is worth starting at the base_link navigation. I pointed out that odometry frame would be a better choice for relative moves. It sounds like you agree with that, so I'm going to not respond to items related to the TF buffer sizes because they're incorrect assumptions to make, but moot if you're using an actual reference frame.

It is only that some consumers (behavior nodes) then ignore it and have undefined behavior if the assumption that the goal->pose.header.frame_id matches feedback_utils_.global_frame does not hold.

Can you enumerate these BT nodes or behaviors so I can look at them? I just need the names and I'll look into them.

initializeGoalPose

Sure, transforming the goal's pose into the global frame would be acceptable here, but the planner server takes care of that for us so once the goal is sent to the planner server, its transformed from whatever frame its in to the planner's global frame. Its really just the logging that's a bit off. I think adding the reference frames to the info log would meet the need, no? Except for the request above for BT nodes / behaviors that aren't working as expected. Let me look into those once you give me a list.

sending goals with rviz or Foxglove Studio while any other frame than map

Keep in mind that's a 'my first robot demo' tool. That's not supposed to be a fully featured system with complete flexibility. But absolutely, adding generic any-frame requests would be unquestionably nice and a PR to add that would be accepted and merged.

What is the added value of having it as an input port of behavior tree nodes?

Some behaviors you're interesting in having performed might be desirable in either the local or global frames (e.g. map or odom or other). So the BT nodes are units that can be used generically in many situations, but by default map is set since thats the most common. But rather than specifying it always be some frame, different instances of the BT nodes can be used in different contexts for varying behaviors within the same tree. But that's not to say that perhaps we didn't overcorrect and add frame optionality in places that it didn't make sense. Again, with a list I can address a specific response and perhaps there's things to fix :-)

I found a related ROS answers post from >10 years ago

I think Dimitri was just trying to prove a point to a new user with that response on the basics of TF and frames. But, like I said, I think it should technically be possible to use base_link but there are alot of subtle details you need to carefully consider if you do like TF buffer size, choice of costmap reference frames, etc. Is a complexity that hardly seems worth it

[SteveMacenski]

Can you enumerate these BT nodes or behaviors so I can look at them?

Hi, any update? I think this is something we can address better, but some help would be great as our maintenance time is limited 😄. Helping get some of the leg work done can let us get this done faster on your behalf by having a starting point to getting the more complex code changes made

[meyerj]

Can you enumerate these BT nodes or behaviors so I can look at them?

Hi, any update? I think this is something we can address better, but some help would be great as our maintenance time is limited 😄. Helping get some of the leg work done can let us get this done faster on your behalf by having a starting point to getting the more complex code changes made

I submitted a pull request (#3917) that solves the issue for us, but unfortunately I could not reproduce the same problem with the Turtlebot navigation example from the Getting Started guide.

I also found that the GoalReachedCondition node is not even used in any of the provided behavior trees for the NavigateToPose behavior, so it cannot have been the culprit, which was my first thought. After some more testing today I am quite sure that it has been the GloballyUpdatedGoalCondition in combination with the navigate_w_replanning_only_if_path_becomes_invalid.xml behavior tree: If the new goal message is the same as the previous goal, in the base_link frame, the GlobalUpdatedGoal condition evaluates to false and the old path is directly forwarded to the FollowPath action, without another planner invocation. In fact the goal is not the same because the robot moved in between. This issue could be resolved by transforming the current and previous goals to the global frame first before the comparison, or by somehow making sure that the first_pose condition in GloballyUpdatedGoalCondition::tick() gets reset when a new goal is activated? But it is a rather artificial case, and providing goals in a moving frame is not a typical use case either, so no strong reason to change anything here.

The patch proposed in #3917 also solves the issue from the description, because the goal pose is transformed early in NavigateToPoseNavigator::initializeGoalPose() before passing it on to the behavior tree. I would still consider it valid, in particular the update of the GoalReachedCondition. But the actual root cause here was the GloballyUpdatedGoalCondition and the unusual case of requesting the exact same goal message (not exactly the same goal!) twice. Many of the other behavior node implementations have a boolean member variable first_time or first_time_ (search in GitHub), so there may be other similar issues and side effects caused by not re-instantiating the behavior tree for a new goal if the tree is not reloaded because the xml filename did not change? Should this shortcut be removed?

[SteveMacenski]

Lets discuss in the PR, but...

But the actual root cause here was the GloballyUpdatedGoalCondition and the unusual case of requesting the exact same goal message (not exactly the same goal!) twice.

Another reason to shake my fist at the removal of seq in std_msgs/Header in ROS 2 ✊ That would make it trivial to disambiguate (though timestamp would help too)

[SteveMacenski]

Merged!