Custom BT malfunctions

[anu120]

Hello,

I created my own BT using already available behavior nodes (updated the nav2_params.yaml file to give the right file)

Expected behavior: Gets a set of waypoints, follow the path until the goal is reached. If the system gets a new path (a new set of waypoints) it should be followed only after the current path is completed.
Actual behavior: The path updates immediately and the mobile base changes its route without completing the current path. Means it never reached the goal before updating the path.

I have been trying with various structures but they all update the path immediately. I am not sure whether its an actual bug or a fault in my BT. Kindly advise.

<ReactiveSequence name="MonitorAndFollowPath">
  <!-- Follow the current path -->
  <RecoveryNode number_of_retries="1" >
    <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
    <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap">
  </RecoveryNode>
  
  <!-- Only update the path when the robot has reached the current goal -->
  <GoalReached goal="{goal}" robot_base_frame="base_link"/>

  <!-- Proceed to the next goal after current path completion -->
  <Fallback name="GoalUpdateFallback">
    <GoalUpdated/>
  </Fallback>
  
</ReactiveSequence>

[SteveMacenski]

If new goals are added, those new goals are preempting the existing ones. You should wait to send the new goals to the on-going navigation task until they're completed. You can easily check this from your action client and have a return callback triggered when the current set of points are completed -- to then trigger the next batch.

If you want to handle this from the BT, you certainly can, but it will require that you create your own custom BT XML and handle the goal list management yourself. The system is designed to be responsive to changes so that you can dynamically adjust goals on the fly as you go, which is a common need / application.

[anu120]

"it will require that you create your own custom BT XML and handle the goal list management yourself." Could you kindly elaborate more on this? Any documents that I can refer to understand goal list management locally without using nav2 builtin management module?

[SteveMacenski]

You'd have to design a BT that stores and manages your goals in your blackboard and BT nodes -- create your own BT XML (which is a major part of the value of Nav2, being able to dynamically modify navigation behaviors)

[anu120]

Ok, so you mean creating a custom behavior node with a mini-blackboard that sort of acts as a goal storage and prevents overwriting in the blackboard itself, and passing the new goal once the blackboard is empty? It disrupts the whole base of BT as I understand, can I design my own mini blackboards like that? Or will a Delay on fallback branch work? Pardon in advance if my question is pretty basic but I really can use some expert knowledge here.

[SteveMacenski]

You could do that - there are many options! I leave it to your creative thinking skills for how you'd like to execute on it within your BT :-)

I dont think you need a new blackboard though, just keep the one that's there and add some new goal vector list that is being managed by your custom BT node looking at the current state of the blackboard goal and managing that list. How you choose to manage that list and BT node, is up to your creative logic and what you want your behavior to look like, but it should be pretty straight forward. Then insert it into your BT at the appropriate location (before planning) to adjust the goal blackboard key appropriately

[anu120]

Understood! Thanks Steve!