[NavThroughPoses] issue after canceling the goal

[albertkiskaroly]

Bug report

Required Info:

• Operating System:
  • Ubunutu 22.04
• ROS2 Version:
  • Humble
• Version or commit hash:
  • nav2 version: 1.1.12

Steps to reproduce issue

Step 1: I added a block to the default navigation_through_poses behavior tree to prevent computing a new path every time, only if there is a new goal or the planned path is not valid. Therefore the tree should be modified to:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="0.333">
          <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
            <ReactiveFallback name="FallbackComputePathThroughPoses">
                <ReactiveSequence name="CheckIfNewPathNeeded">
                    <Inverter>
                        <GlobalUpdatedGoal />
                    </Inverter>
                    <IsPathValid path="{path}" />
                    <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.5"/>
                </ReactiveSequence>
                <ReactiveSequence>
                    <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.5"/>
                    <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
                </ReactiveSequence>
            </ReactiveFallback>
            <ReactiveFallback name="ComputePathThroughPosesRecoveryFallback">
              <GoalUpdated/>
              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </ReactiveFallback>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ReactiveFallback name="FollowPathRecoveryFallback">
            <GoalUpdated/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </ReactiveFallback>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <RoundRobin name="RecoveryActions">
          <Sequence name="ClearingActions">
            <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          </Sequence>
          <Wait wait_duration="5"/>
          <BackUp backup_dist="0.3" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

Step 2: launch the tb3_simulation_launch.py as described here, with the modified behavior tree

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

Expected behavior

I give the robot a goal at the beginning, it will plan a path and follow it until it reaches the goal. If I cancel the goal, the robot stops. After that, if I give the robot a new goal, it will plan a new path and move along it towards the new destination.

Actual behavior

After canceling a goal, if I specify a new goal, it will plan the new path, but it will not follow it, but will go to the old destination.

Additional information

https://github.com/ros-planning/navigation2/assets/55767637/8f9caff9-3a25-4083-b4b1-94adacddf6b6

[SteveMacenski]

Ask on Robotics Stack Exchange or I recommend reading Behavior Trees for Robotics and AI. The issue is clearly in your BT modifications -- likely in that its not setup so that we always replan first-thing so the old plan on the blackboard is sent to Follow Path because you don't enter the planning part of the tree with your changes. Unfortunately, I don't have cycles to debug every users custom BT configurations and its worth learning how to write BTs and how the nodes you're using work to make sure you understand the logic you're asking it to perform.

I'm going to guess it has to do with the RateController or GlobalUpdatedGoal BT nodes -- might be useful to throw some logs in there and see what's going on. If you find there's actually a bug, happy to discuss that, but I can't debug every complete application BT that comes my way :-). If you have something in particular you think is wrong about a node that I can reproduce, that's something I can help wtih!