ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.58k stars 1.3k forks source link

How to make Planner selector work #3112

Closed Lannister-Xiaolin closed 2 years ago

Lannister-Xiaolin commented 2 years ago

Required Info:

Operating System: Ubuntu 20.04 ROS2 Version: Galatic

Detail

Any one can share a xml configure that works for galactic version? I setted as below, it's not working!!!

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <RecoveryNode number_of_retries="6" name="NavigateRecovery">
            <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased"
                             topic_name="planner_selector">
                <PipelineSequence name="NavigateWithReplanning">
                    <RateController hz="1.0">
                        <RecoveryNode number_of_retries="1" name="ComputePathToPose">
                            <ComputePathToPose goal="{goal}" path="{path}"/>
                            <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
                                <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>
            </PlannerSelector>
            <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>
                    <Spin spin_dist="1.57"/>
                    <Wait wait_duration="5"/>
                    <BackUp backup_dist="0.15" backup_speed="0.025"/>
                </RoundRobin>
            </ReactiveFallback>
        </RecoveryNode>
    </BehaviorTree>
</root>

image

SteveMacenski commented 2 years ago

This is an action node, not a control flow node, you've wrapped a bunch of stuff around the planner selector as having children when it itself is a leaf node only. You might want to try using Groot to modify the behavior trees to build BTs, I believe it should prevent you from making mistakes things like that https://navigation.ros.org/tutorials/docs/using_groot.html

<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector">

Should include the end tag /> as a leaf, not later wrapping some stuff as you have some lines down with </PlannerSelector> (and then remove indenting). You can see example usage in the unit tests: https://github.com/ros-planning/navigation2/blob/main/nav2_behavior_tree/test/plugins/action/test_planner_selector_node.cpp