Bug report

Required Info:

• Operating System:
  • Ubuntu 18.04
• ROS2 Version:
  • Foxy source
• Version or commit hash:
  • master

Steps to reproduce issue

Use the following behavior tree which implements the most basic navigation logic; compute path to pose and follow path:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="NavigateWithReplanning">
      <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
      <FollowPath path="{path}"  controller_id="FollowPath"/>
    </Sequence>
  </BehaviorTree>
</root>

Send a navigation goal through rviz and the bot should start executing a path to this goal. Send another goal before the previous goal is reached.

Expected behavior

The previous goal should be preempted and the bot should start following a path to the new goal.

Actual behavior

The bot continues to move towards the previous goal and the new navigation goal is lost.