Navigation and Behaviour trees for task execution
This blog is continuation to the main blog (Robots at Work: A story of Decentralized Collaboration), which you can read here.
by Rushikesh Halle and Divye Singh.
Navigation is the core function of any mobile robot. A successful navigation requires coordination between a lot of functionalities, such as planning, controlling, localization, mapping, obstacle avoidance, etc.
Navigation2 is a set of packages that provide a framework for designing the above components along with some ready-to-use concrete implementations.
Navigation functionality can be very easily added to the control system of your bot using the nav2_bringup package. The package contains a properly organized set of launch files that launch the common nodes required to achieve the navigation functionalities, along with sensible defaults for parameter values. The user can simply set the parameter values suitable for their use case by changing values in a single YAML file.
The ready to use planners and controllers are robust and flexible enough to satisfy common navigation needs.
Using the framework provided, not only navigation but also any general tasks can be implemented. The nav2_behavior_tree is one such package in the Navigation2 stack, using which we can define a sequence of activities for completing any task. We will discuss how we achieved the box-arranging task using the nav2_behavior_tree package in the next session.
Behavior trees
Even the simple action of navigating from one point to another requires a complex sequence of actions. For example, a path planner should keep updating the path periodically, and the controller should always consider the latest updated path for sending control signals. If it is impossible to update the path, a proper backup action should be in place for recovery. Every such action has an independent action-server node. They should get triggered in the proper sequence for successful execution of tasks. Manually keeping track of sequences through code manipulation is tedious. For this purpose, behavior trees come in handy.
Behavior trees are used in robotics for designing and tracking control sequences. A user just needs to write down a simple XML file containing control nodes, action nodes, condition nodes, and decorator nodes, linked in a certain order. For example, the Fig 2 shows the behavior tree for the simple task of finding and picking coloured boxes on the floor. All the actions are to be performed one after another in sequence; hence, we linked them using the sequence control node. The actions in blue are not actual action servers but are action clients. Action servers are supposed to be launched independently, hence following the Single responsibility principle.
For more information on how behavior trees work, please refer to the following tutorial:
https://www.behaviortree.dev/docs/tutorial-basics/tutorial_01_first_tree/
The behavior tree comes with some predefined control nodes but also provides interfaces to define our own. The nav2_behavior_tree package comes with a few extended control nodes using which we can implement very complex tasks. The Fig 3 shows the actual behavior tree used in the task of arranging boxes in warehouses:
The xml equivalent of the above tree:
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<PipelineSequence name="NavigateWithReplanning">
<RateController hz="10.0">
<RecoveryNode number_of_retries="1" name="ApproachBox">
<GetNearestBoxPickupPose name="get_nearest_box_pickup_pose" box_to_pick_colour="{box_to_pick_colour}" box_pickup_pose="{box_pickup_pose}"/>
<GoToCorner />
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="7" name="ApproachBox">
<ApproachBox name="approach_box" box_pickup_pose="{box_pickup_pose}"/>
<Wait wait_duration="2"/>
</RecoveryNode>
</PipelineSequence>
<PickBox name="pick_box" box_to_pick_colour="{box_to_pick_colour}" picked_shape_type="{picked_shape_type}"/>
<RecoveryNode number_of_retries="7" name="ApproachWareHouse">
<ApproachWareHouse name="approach_ware_house" picked_shape_type="{picked_shape_type}" />
<Wait wait_duration="2"/>
</RecoveryNode>
<DropBox name="drop_box" picked_shape_type="{picked_shape_type}" />
</Sequence>
</BehaviorTree>
</root>So the overall process of creating a task using a behavior tree is as follows:
- Create the desired behavior tree in diagrammatic format using tools such as Groot.Most of the control nodes required for achieving tasks are available in nav2_behavior_tree; in very rare cases, you will need to create new ones. But, even in that case they can be easily created using the provided interfaces.
- Create an xml file equivalent to the above tree.
- Create plugins for leaf nodes in the above behavior tree. i.e., action client nodes and register the plugins: https://navigation.ros.org/plugin_tutorials/docs/writing_new_bt_plugin.html
- Create action servers for each action node mentioned in the above tree.
- You can run the behavior tree using the behavior tree engine provided by the nav2 package. https://github.com/ros-planning/navigation2/blob/humble/nav2_behavior_tree/src/behavior_tree_engine.cpp
Conclusion:
The Navigation2 package eases the development of task planning and execution algorithms by providing a simple framework and a few ready-to-use packages that can be easily configured depending on the use-case requirements. We have demonstrated its usage for executing the box-arranging task.
References:
Disclaimer: The statements and opinions expressed in this blog are those of the author(s) and do not necessarily reflect the positions of Thoughtworks.
