Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Writing a simple BT for Move Base Flex

Description: In this tutorial you learn how to set up a very simple behavior tree (BT from now on) using the amazing py_trees_ros library to interact with Move Base Flex. We just replicate the hard-coded move_base FSM (with some limitations), but py_trees_ros lets you easily increase the sophistication of your navigation strategy!.

Keywords: py_trees_ros, move_base_flex, navigation, planning, behavior tree

Tutorial Level: ADVANCED

Description

In this tutorial we address the actions GetPath, ExePath and Recovery provided by Move Base Flex. While GetPath runs the global planner searching for a path to the target pose, ExePath runs the local planner executing the planned path. The Recovery action can be used to execute various behaviors for error handling during planning and controlling. We connect these actions by setting up a py_trees_ros BT using ActionClient Behaviors. In addition to the actions described above, the implementation of a state that receives a navigation goal by the user is required. The target pose can be easily set via the visualization tool RViz and published on a specific topic.

Behavior Tree on RQT

The implemented BT visualized on rqt

To learn about BTs and the particular library used here I encourage you to read py-trees documentation, and follow py_trees_ros tutorials to learn how to control ROS-based robots with BTs.

As a minimum requirement to understand what's coming next, be sure you understand the following concepts:

action behavior
check behavior
composite
blackboard

Behaviors

Our tree requires five action behaviors: NewGoal, ClearGoal, GetPath, ExePath and Recovery and one check behavior: HaveGoal.

NewGoal

geometry_msgs/PoseStamped on the topic /move_base_simple/goal. For creating a NewGoal action behavior, add the following lines to your code:

   1 new_goal = py_trees_ros.subscribers.ToBlackboard(name="NewGoal",
   2                                                  topic_name="/move_base_simple/goal",
   3                                                  topic_type=geometry_msgs.PoseStamped,
   4                                                  blackboard_variables = {'target_pose': None})

GetPath

The following lines declare the class GetPath extending ActionClient and create an action behavior we can later add to the tree:

   1 class GetPath(py_trees_ros.actions.ActionClient):
   2 
   3     def initialise(self):
   4         """
   5         Get target pose from the blackboard to create an action goal
   6         """
   7         self.action_goal = mbf_msgs.GetPathGoal(target_pose=py_trees.blackboard.Blackboard().get("target_pose"))
   8         super(GetPath, self).initialise()
   9 
  10     def update(self):
  11         """
  12         On success, set the resulting path on the blackboard, so ExePath can use it
  13         """
  14         status = super(GetPath, self).update()
  15         if status == py_trees.Status.SUCCESS:
  16             py_trees.blackboard.Blackboard().set("path", self.action_client.get_result().path)
  17         return status
  18 
  19 get_path = GetPath(name="GetPath",
  20                    action_namespace="/move_base_flex/get_path",
  21                    action_spec=mbf_msgs.GetPathAction)

Ensure that you declare the correct namespace for the action (or remap appropriately on the launch file). On this simple demo we need to add pretty little additional code to the base ActionClient class, just gather from blackboard the data required to create the goal s and add the result back to the blackboard, so other actions can use it.

ExePath

The ExePath action is very similar to GetPath:

   1 class ExePath(py_trees_ros.actions.ActionClient):
   2 
   3     def initialise(self):
   4         """
   5         Get path from the blackboard to create an action goal
   6         """
   7         self.action_goal = mbf_msgs.ExePathGoal(path=py_trees.blackboard.Blackboard().get("path"))
   8         super(ExePath, self).initialise()
   9 
  10 exe_path = ExePath(name="ExePath",
  11                    action_namespace="/move_base_flex/exe_path",
  12                    action_spec=mbf_msgs.ExePathAction)

Here, we only have a goal path but no results. Do not forget to change the namespace.

Recovery

The Recovery action is slightly more complicated because we need to manage the list of available recovery behaviors, read from ROS parameters server. Every time the action is executed, we try the next recovery behavior, dropping it from the list. Once exhausted, we fail to abort navigation, but also restore the list for the next goal.

   1 class Recovery(py_trees_ros.actions.ActionClient):
   2     def setup(self, timeout):
   3         """
   4         Read the list of available recovery behaviors so we can try them in sequence
   5         """
   6         self._behaviors = rospy.get_param("/move_base_flex/recovery_behaviors")
   7         return super(Recovery, self).setup(timeout)
   8 
   9     def update(self):
  10         """
  11         Try the next recovery behavior, dropping it from the list
  12         """
  13         try:
  14             self.action_goal = mbf_msgs.RecoveryGoal(behavior=self._behaviors.pop(0)["name"])
  15             return super(Recovery, self).update()
  16         except IndexError:
  17             # recovery behaviors exhausted; fail to abort navigation but restore the list for the next goal
  18             # TODO: this means that we won't reset the list after a successful recovery, so the list keeps shrinking
  19             # until fully exhausted; that's clearly not the expected operation, so I need to find a better solution
  20             self._behaviors = rospy.get_param("/move_base_flex/recovery_behaviors")
  21             return py_trees.Status.FAILURE
  22 
  23 recovery = Recovery(name="Recovery",
  24                     action_namespace="/move_base_flex/recovery",
  25                     action_spec=mbf_msgs.RecoveryAction)

Other action and check behaviors

The remaining action and check behaviors are much simpler and require a single line of code each. HaveGoal simple checks if "target_pose" variable is on the blackboard, while ClearGoal (used twice) removes the same variable from the blackboard:

   1     have_goal = py_trees.blackboard.CheckBlackboardVariable(name="HaveGoal", variable_name="target_pose")
   2     clr_goal1 = py_trees.blackboard.ClearBlackboardVariable(name="ClearGoal", variable_name="target_pose")
   3     clr_goal2 = py_trees.blackboard.ClearBlackboardVariable(name="ClearGoal", variable_name="target_pose")

Composites

For the simple behavior implemented on this tutorial we just need four composites, two sequences and two selectors:

   1     bt_root = py_trees.composites.Sequence("MBF BT Demo")
   2     get_goal = py_trees.composites.Selector("GetGoal")
   3     fallback = py_trees.composites.Selector("Fallback")
   4     navigate = py_trees.composites.Sequence("Navigate")

Composing the Behavior Tree

And finally here comes the tree itself:

   1     # Compose tree
   2     bt_root.add_children([get_goal, fallback])
   3     get_goal.add_children([have_goal, new_goal])
   4     navigate.add_children([get_path, exe_path, clr_goal1])
   5     fallback.add_children([navigate, recovery, clr_goal2])

To display the BT on rqt, run:

rosrun rqt_py_trees rqt_py_trees

This a very good idea first to verify that your code actually composes the tree you have designed. And will help you a lot for debugging, as nodes are highlighted with a color scheme to show the current execution state:

GREEN for SUCCESS
RED for FAILURE
BLUE for RUNNING
GREY for unvisited.

The GUI also provides:

Tooltips : hover over a behavior to catch name, type, status and feedback message information
Timeline : rewind as you wish, note the bars indicating where important events occurred

Full code

I repeat the full code here in the original order to facilitate copy pasting:

   1 #!/usr/bin/env python
   2 
   3 """
   4 MBF BT Demo: Behavior tree implementing a really basic navigation strategy,
   5 even simpler than the move_base hardcoded FSM, as it lacks:
   6 
   7 * continuous replanning
   8 * oscillation detection
   9 
  10 We create on the first place action client behaviors for MBF's planner, controller and recovery action servers
  11 On this simple demo we need to add pretty little additional code to the base ActionClient class
  12 """
  13 
  14 ##############################################################################
  15 # Imports
  16 ##############################################################################
  17 
  18 import functools
  19 import py_trees
  20 import py_trees_ros
  21 import py_trees.console as console
  22 import rospy
  23 import sys
  24 
  25 import geometry_msgs.msg as geometry_msgs
  26 import mbf_msgs.msg as mbf_msgs
  27 
  28 
  29 ##############################################################################
  30 # Actions
  31 ##############################################################################
  32 
  33 class GetPath(py_trees_ros.actions.ActionClient):
  34 
  35     def initialise(self):
  36         """
  37         Get target pose from the blackboard to create an action goal
  38         """
  39         self.action_goal = mbf_msgs.GetPathGoal(target_pose=py_trees.blackboard.Blackboard().get("target_pose"))
  40         super(GetPath, self).initialise()
  41 
  42     def update(self):
  43         """
  44         On success, set the resulting path on the blackboard, so ExePath can use it
  45         """
  46         status = super(GetPath, self).update()
  47         if status == py_trees.Status.SUCCESS:
  48             py_trees.blackboard.Blackboard().set("path", self.action_client.get_result().path)
  49         return status
  50 
  51 class ExePath(py_trees_ros.actions.ActionClient):
  52 
  53     def initialise(self):
  54         """
  55         Get path from the blackboard to create an action goal
  56         """
  57         self.action_goal = mbf_msgs.ExePathGoal(path=py_trees.blackboard.Blackboard().get("path"))
  58         super(ExePath, self).initialise()
  59 
  60 class Recovery(py_trees_ros.actions.ActionClient):
  61     def setup(self, timeout):
  62         """
  63         Read the list of available recovery behaviors so we can try them in sequence
  64         """
  65         self._behaviors = rospy.get_param("/move_base_flex/recovery_behaviors")
  66         return super(Recovery, self).setup(timeout)
  67 
  68     def update(self):
  69         """
  70         Try the next recovery behavior, dropping it from the list
  71         """
  72         try:
  73             self.action_goal = mbf_msgs.RecoveryGoal(behavior=self._behaviors.pop(0)["name"])
  74             return super(Recovery, self).update()
  75         except IndexError:
  76             # recovery behaviors exhausted; fail to abort navigation but restore the list for the next goal
  77             # TODO: this means that we won't reset the list after a successful recovery, so the list keeps shrinking
  78             # until fully exhausted; that's clearly not the expected operation, so I need to find a better solution
  79             self._behaviors = rospy.get_param("/move_base_flex/recovery_behaviors")
  80             return py_trees.Status.FAILURE
  81 
  82 
  83 ##############################################################################
  84 # Behaviours
  85 ##############################################################################
  86 
  87 def create_root():
  88     # Create all behaviours
  89     bt_root = py_trees.composites.Sequence("MBF BT Demo")
  90     get_goal = py_trees.composites.Selector("GetGoal")
  91     fallback = py_trees.composites.Selector("Fallback")
  92     navigate = py_trees.composites.Sequence("Navigate")
  93     new_goal = py_trees_ros.subscribers.ToBlackboard(name="NewGoal",
  94                                                      topic_name="/move_base_simple/goal",
  95                                                      topic_type=geometry_msgs.PoseStamped,
  96                                                      blackboard_variables = {'target_pose': None})
  97     have_goal = py_trees.blackboard.CheckBlackboardVariable(name="HaveGoal", variable_name="target_pose")
  98     clr_goal1 = py_trees.blackboard.ClearBlackboardVariable(name="ClearGoal", variable_name="target_pose")
  99     clr_goal2 = py_trees.blackboard.ClearBlackboardVariable(name="ClearGoal", variable_name="target_pose")
 100     get_path = GetPath(name="GetPath",
 101                        action_namespace="/move_base_flex/get_path",
 102                        action_spec=mbf_msgs.GetPathAction)
 103     exe_path = ExePath(name="ExePath",
 104                        action_namespace="/move_base_flex/exe_path",
 105                        action_spec=mbf_msgs.ExePathAction)
 106     recovery = Recovery(name="Recovery",
 107                         action_namespace="/move_base_flex/recovery",
 108                         action_spec=mbf_msgs.RecoveryAction)
 109 
 110     # Compose tree
 111     bt_root.add_children([get_goal, fallback])
 112     get_goal.add_children([have_goal, new_goal])
 113     navigate.add_children([get_path, exe_path, clr_goal1])
 114     fallback.add_children([navigate, recovery, clr_goal2])
 115     return bt_root
 116 
 117 
 118 def shutdown(behaviour_tree):
 119     behaviour_tree.interrupt()
 120 
 121 
 122 ##############################################################################
 123 # Main
 124 ##############################################################################
 125 
 126 if __name__ == '__main__':
 127     """
 128     Entry point for the demo script.
 129     """
 130     rospy.init_node("mbf_bt_demo")
 131     root = create_root()
 132     behaviour_tree = py_trees_ros.trees.BehaviourTree(root)
 133     rospy.on_shutdown(functools.partial(shutdown, behaviour_tree))
 134     if not behaviour_tree.setup(timeout=15):
 135         console.logerror("failed to setup the tree, aborting.")
 136         sys.exit(1)
 137     behaviour_tree.tick_tock(500)

ROS 2 Documentation

Wiki

Page

User