EDIT: This isn't enough, you need to modify Behavior Tree too. Sorry for misleading before.
Did you try with something like:
recoveries_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
recovery_plugins: ["wait"]
wait:
plugin: "nav2_recoveries/Wait"
global_frame: odom
robot_base_frame: base_link
transform_timeout: 0.1
use_sim_time: true
based on:
https://navigation.ros.org/configurat... ?
Modification of the Behavior Tree - you can use the Groot. For the ROS2 Galactic use:
cd ~/dev_ws/src
git clone https://github.com/BehaviorTree/Groot.git
cd ..
rosdep install --from-paths src --ignore-src
colcon build --symlink-install --packages-select groot
I have sudo apt install ros-galactic-behaviortree-cpp-v3
before too.
You have to source install/setup.bash
and ./build/groot/Groot
.
You can load BT XMLs from the Nav2 repository and modify it, e.g. like this:
root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Control ID="RecoveryNode" name="NavigateRecovery" number_of_retries="6">
<Control ID="PipelineSequence" name="NavigateWithReplanning">
<Decorator ID="RateController" hz="0.333">
<Control ID="RecoveryNode" name="ComputePathThroughPoses" number_of_retries="1">
<ReactiveSequence>
<Action ID="RemovePassedGoals" input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
<Action ID="ComputePathThroughPoses" goals="{goals}" path="{path}" planner_id="GridBased" start=""/>
</ReactiveSequence>
<ReactiveFallback name="ComputePathThroughPosesRecoveryFallback">
<Condition ID="GoalUpdated"/>
<Action ID="ClearEntireCostmap" name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</ReactiveFallback>
</Control>
</Decorator>
<Control ID="RecoveryNode" name="FollowPath" number_of_retries="1">
<Action ID="FollowPath" controller_id="FollowPath" goal_checker_id="GoalChecker" path="{path}"/>
<ReactiveFallback name="FollowPathRecoveryFallback">
<Condition ID="GoalUpdated"/>
<Action ID="ClearEntireCostmap" name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</ReactiveFallback>
</Control>
</Control>
<ReactiveFallback name="RecoveryFallback">
<Condition ID="GoalUpdated"/>
<Control ID="RoundRobin" name="RecoveryActions">
<Sequence name="ClearingActions">
<Action ID="ClearEntireCostmap" name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<Action ID="ClearEntireCostmap" name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<Action ID="Wait" wait_duration="5"/>
</Control>
</ReactiveFallback>
</Control>
</BehaviorTree>
</root>