/navigate_to_pose mishandles request pointers when not replanning

[relffok]

Bug report

Required Info:

• Operating System:
  Ubuntu 20.04
• ROS2 Version:
  Galactic binaries
• Version or commit hash:
  ros-galactic-nav2-planner 1.0.8-1focal.20220211.070855

Hello, I noticed something interesting when using the navigate_to_pose action client when using in a unusual manner, which was to try not to replan.

Steps to reproduce issue

I used the following BT:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithoutReplanning">
      <RateController hz="0.0">
        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
      </RateController>
      <FollowPath path="{path}"  controller_id="FollowPath"/>
    </PipelineSequence>
  </BehaviorTree>
</root>

Its a very easy setup, I have two goals that can be completed with a straight movement:

goal1 with x1 = 1.0 and
goal2 with x2 = 5.0

and robot starting from the origin, so they only differ in the length of the movement. The values here don't matter. It's just nice to visualize it in a simple way.

Tested:

1. short goal, then long goal
   I bring up the navigation nodes and send goal1 to navigate_to pose (short path), which is accepted and starts to execute. Before the robot can reach it though, I send goal2 (long path), which is accepted, (goal1 is aborted immediately). So far so good. Looking at rviz, I realize that the plan is not updated (makes sense with a 0.0 update rate). But when the robot reaches x1, goal2 is the request that receives the finished status SUCCEEDED.

2. long goal then short goal
   I bring up the navigation nodes and send goal2 to navigate_to pose (long path), which is accepted and starts to execute. I send goal1 (short path), which is accepted, (goal2 is aborted immediately). Again: so far so good. Looking at rviz, I realize again that the plan is not updated (it really makes sense with 0.0 update rate).The robot shoots over goal1 and continues until goal2 is reached. I again accept that this is reasonable behavior but in the end goal1 receives the finished status SUCCEEDED when the robot actually reaches goal2.

Expected behavior

I understand that setting the update rate to such a low rate might not be intended purpose of the stack, but to be fair: at the moment there is nothing stopping me to do so. In both scenarios, the goal executed should must have matched the requested goal and the resulting response.

Implementation considerations

If this is a feature that could be kept (imo it is actually quite useful to use nav2 for simple research questions where replanning is not intended), I see 2 possible scenarios:

1. New goals won't be accepted while another is being executed.
2. New goals replace old goals, but if so, the response must match the outcome. The initial goal is being ABORTED, the goal must be internally updated, and the new goal is being executed resulting in the respective SUCCEEDED/ABORTED status.

[SteveMacenski]

I'd start off by saying 0.0 is not a valid rate, you'll get a divide by zero which is not defined behavior https://github.com/ros-planning/navigation2/blob/main/nav2_behavior_tree/plugins/decorator/rate_controller.cpp#L31. I'm sure it probably works, but why not simply use the single trigger BT node decorator https://navigation.ros.org/plugins/index.html#behavior-tree-nodes or create something more semantically meaningful to your needs? You do not need to use the rate controller at all and it is clearly not the right tool for the job that you're trying to accomplish. It could be event based on a new goal, it could be a single trigger instance, it could be anything you choose.

But more specifically to your commentary, yes, if you make poorly designed behavior trees, you're going to have problems. There's no bug as far as I can tell in Nav2 or the BT Navigator from your situation. Your issue is that you have created an invalid behavior tree since it does not either (1) handle preemptions of new goals or (2) reject them in a way that makes for valid behavior. Both of the situations you mention are at fault of the BT XML you linked to above either not replanning it should due to the preemption it received or didn't outright reject a preemption request because you are intentionally not processing them in this BT by your user design.

You could create additional BT nodes to check if the goal is changed and throw an exception that this tree will not handle preemptions, but is up to you as the responsible party designing behavior trees to make sure you do so to handle the basic action API. If you don't design in dynamic behavior (or intentionally remove it) then you can't treat your behavior tree as if there will be dynamic behavior. A more simplistic analog would be if another user created a BT XML that drove a robot straight into a wall at full speed - there's really nothing we can do to protect users at the navigation logic level from creating logic that results in bad situations, as much as we try.

Your other option for use (but you should still do above) is to cancel and reissue a new goal, not preempt goals so the original action is ceased when a new action is requested.

But for best practices, my recommended change would be to make the replanning decorator dependent on changes in goals. So that if the goal changes, it will allow for a single replan. You could accomplish that with a basic control flow node and the goal updated condition node (though semantically, to replace the rate controller you could create a goal controller that would trigger the replanning child, but that would involve you creating a BT node versus just rearranging your XML). If your goal is to handle preemptions, that is. If you don't want preemptions, you should throw an exception and it will fail the task back to your navigation client.

If you did have interest in the GoalUpdatedController, I would gladly merge that into Nav2's BT node library, I think that has alot of great generalized use - if you're open to contributing it!

[SteveMacenski]

Oh whoops, wrong button

[SteveMacenski]

Update thoughts?

[SteveMacenski]

Closing since no response and thrust of the ticket is a poor BT design by the user. Happy to reopen or continue the discussion if there is a follow up.

[relffok]

Hi there! I am so sorry for the late reply, covid itself struck me down. Thank you so much for your detailed and great feedback. It is much appreciated how much time you take to reply to everybody and provide and maintain such a cool ROS 2 tool!

Now back to the discussion:

I'd start off by saying 0.0 is not a valid rate, you'll get a divide by zero which is not defined behavior

I figured, but imo the crucial thing here is that if a rate selected here (can also be 0.1) is a problem once the goal is reached within that timeframe.

A more simplistic analog would be if another user created a BT XML that drove a robot straight into a wall at full speed - there's really nothing we can do to protect users at the navigation logic level from creating logic that results in bad situations, as much as we try.

Thanks a lot for the laugh, I felt caught!

I definitely agree on the statement of the poor BT design and that GoalUpdateController is the key to that problem and I am happy something came out of this issue!

I'd love to chip in on this one but for now I have to stick with the simple BT change. If the issue is still around once I have more time on my hands to read into it, I'm happy to take it on.

[relffok]

update: changed my mind and opened a PR (#3044). I am not quite sure if this is how it was intended to be implemented but it helped me to understand the issue you pointed out! 👍