Robotics StackExchange | Archived questions

[nav2] How do I make a custom behavior tree?

I am having great trouble making my own behavior tree. It seems like a good system, but I can't figure out how to debug and construct in the way I intend.

I want the robot to navigate in stages. I ask the user to give me a path, and load it into the blackboard via a custom global planner. Then, I use a custom condition to check if I am close to the start of that path. If not, I navigate to the start of the path, then I navigate the original path. If I am close to the start to begin with, I navigate the original path.

<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
    <Sequence>
        <!-- Allow PathManager to get load its path into the path variable -->
        <ComputePathToPose goal="{goal}" path="{path}" planner_id="PathManager"/>
        <Fallback>
            <RobotReadyForPathCondition path="{path}" max_distance="0.5" max_angle="3.0"/>
            <Sequence>
                <!-- Get the first pose of the path -->
                <ExtractFirstPose path="{path}" extracted_pose="{first_goal}"/>
                <ComputePathToPose goal="{first_goal}" path="{path_to_first}" planner_id="navfn"/>
                <RecoveryNode number_of_retries="10">
                    <Sequence>
                        <Wait wait_duration="1.0"/>
                        <ComputePathToPose goal="{first_goal}" path="{path_to_first}" planner_id="navfn"/>
                    </Sequence>
                    <FollowPath path="{path_to_first}" controller_id="RPPController"/>
                </RecoveryNode>
            </Sequence>
        </Fallback>
        <Wait wait_duration="5.0"/>
        <ComputePathToPose goal="{first_goal}" path="{path}" planner_id="PathManager"/>
        <FollowPath path="{path_to_first}" controller_id="RPPController"/>
    </Sequence>
</BehaviorTree>
</root>

The result of this behavior tree is that it follows the original path to the goal. It's like all my custom logic does literally nothing, and I have no debug tools (that I know of, happy to be proven wrong). How do I go about finishing this tree? Have I made a fatal error in my tree? What tools can I use to continue to debug and read out values that I am interested in?

Asked by Per Edwardsson on 2022-08-01 03:23:10 UTC

Comments

Answers

Did you try to use Groot for modifying a BT? I recommend it, it makes everything easier.

https://navigation.ros.org/tutorials/docs/using_groot.html

For the ROS2 Galactic installation:

   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/your package and modify it.

Right now there is an open issue in the Nav2 repository about probing the blackboard parameters for debugging:

You can try to check this forum too:

As well as the documentation or this PDF.

Asked by ljaniec on 2022-08-08 04:19:13 UTC

Comments