ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

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

asked 2022-08-01 03:23:10 -0600

Per Edwardsson gravatar image

updated 2022-08-08 03:52:49 -0600

ljaniec gravatar image

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">
        <!-- Allow PathManager to get load its path into the path variable -->
        <ComputePathToPose goal="{goal}" path="{path}" planner_id="PathManager"/>
            <RobotReadyForPathCondition path="{path}" max_distance="0.5" max_angle="3.0"/>
                <!-- 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">
                        <Wait wait_duration="1.0"/>
                        <ComputePathToPose goal="{first_goal}" path="{path_to_first}" planner_id="navfn"/>
                    <FollowPath path="{path_to_first}" controller_id="RPPController"/>
        <Wait wait_duration="5.0"/>
        <ComputePathToPose goal="{first_goal}" path="{path}" planner_id="PathManager"/>
        <FollowPath path="{path_to_first}" controller_id="RPPController"/>

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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2022-08-08 04:19:13 -0600

ljaniec gravatar image

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

For the ROS2 Galactic installation:

   cd ~/dev_ws/src
   git clone
   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.

edit flag offensive delete link more

Question Tools



Asked: 2022-08-01 03:23:10 -0600

Seen: 310 times

Last updated: Aug 08 '22