Getting Started
This tutorial goes through creating a solution from scratch with ROS Trees. The documentation in the README will be helpful in following this tutorial.
The task we would like to solve is placing visible bottles into a bin. The task is defined as:
A tabletop manipulator needs to place all bottles in its visible workspace into the adjacent bin. The manipulator has the following ROS capabilities available for completing the task:
- A ROS Service for getting a synced pair of RGB and depth images (service name:
/service/get_synced_rgbd); - A ROS Service to detect bottles, returning a list of detected bottles (service name:
/service/detect_bottles); - A ROS Action Server for actuating the gripper (action namespace:
/action/actuate_gripper); and - ROS Action Servers to move the gripper to either a pose (available:
/action/move_gripper/pose) and or named locactionworkspaceandbin(available:/action/move_gripper/location).
The full working example can obtained by cloning our ros_trees_examples repo:
git clone https://github.com/qcr/ros_trees_examples
The example is then started by:
-
running the dummy backend:
rosrun ros_trees_examples bottle_binner_backend -
running the behaviour tree for binning bottles:
rosrun ros_trees_examples bottle_binner_tree
This tutorial will go through the process of creating the bottle_binner_tree file from scratch.
This example can be run on a real robot; you simply need a robot with the capabilities defined in the task definition above. This natural transfer across machines and robots is one of the big strengths of using trees to detach behaviour from capability.
Here we go through three phases in creating powerful behaviour trees from scratch:
- Declaring a robot capability by writing leaves
- Creating re-usable behaviours with branches
- Combining it all together into a tree for your task
Leaves are implemented in ros_trees, leaving you to focus on providing new robot capabilities by instantiating, overriding, and expanding existing leaves. Full documentation of the existing base leaves is at the bottom of the README.
In most cases, a custom leaf is created by a parameterised call to a base leaf's constructor. For example, a leaf for actuating the gripper is created with:
from ros_trees.leaves_ros import ActionLeaf
actuate_gripper_leaf = ActionLeaf("Actuate Gripper", action_namespace='/action/actuate_gripper')All that's needed is a leaf name, and the namespace of ROS Action Server. The ros_trees package makes a lot of assumptions under the hood for you, which contributes to the code brevity. But it is important to understand the assumptions for cases where they don't apply.
Declaring inputs and outputs
Leaves typically need an input (e.g. a goal pose is needed for a leaf moving a robot to a pose), provide an output (e.g. the e-stop state is returned by a leaf the robot's e-stop), or both (e.g. a leaf detecting grasp poses takes in RGB-D images and outputs the found grasp poses). A number of constructors arguments in the Leaf class exist to control how a leaf handles input and output.
Although described in detail here, leaf inputs and outputs are broadly controlled by the load_* and save* parameters respectively. The default behaviour of leaves is to try to form input data from whatever was last saved from a previous leaf, and save output data if the save argument is set.
Let's see how these assumptions can be configured to handle the gripper opening and closing use case. Static input data is used to tell the gripper to open; i.e. a leaf for opening the gripper will always have the same input data commanding the ROS Action Server. To provide static input data instead of attempting to dynamically load input at runtime, we use the load_value constructor argument:
from ros_trees.leaves_ros import ActionLeaf
from ros_trees_examples.msg import ActuateGripperGoal
open_gripper_leaf = ActionLeaf("Actuate Gripper",
action_namespace='/action/actuate_gripper',
load_value=ActuateGripperGoal(
state=ActuateGripperGoal.STATE_OPEN))The static data is of type ActuateGripperGoal, which is defined in the ros_trees_examples package.
Repeating and extending leaves
Up to this point leaves have been created as a single class instance of a base leaf from ros_trees. This works for leaves only used once, but falls down when leaves need to be used multiple times in a tree, exist across different trees, be expanded upon, override default leaf behaviour, provide custom functions, etc.
A polymorphic approach with class definitions makes a lot more sense for these use cases. For example, re-usable leaves for opening and closing the gripper can be created with:
from ros_trees.leaves_ros import ActionLeaf
from ros_trees_examples.msg import ActuateGripperGoal
class _ActuateGripper(ActionLeaf):
def __init__(self, *args, **kwargs):
super(_ActuateGripper,
self).__init__(action_namespace='/action/actuate_gripper',
*args,
**kwargs)
class OpenGripper(_ActuateGripper):
OPEN_GOAL = ActuateGripperGoal(mode=ActuateGripperGoal.MODE_STATIC,
state=ActuateGripperGoal.STATE_OPEN)
def __init__(self, *args, **kwargs):
super(OpenGripper, self).__init__("Open Gripper",
load_value=OpenGripper.OPEN_GOAL,
*args,
**kwargs)
class CloseGripper(_ActuateGripper):
CLOSE_GOAL = ActuateGripperGoal(mode=ActuateGripperGoal.MODE_STATIC,
state=ActuateGripperGoal.STATE_CLOSED)
def __init__(self, *args, **kwargs):
super(CloseGripper, self).__init__("Close Gripper",
load_value=CloseGripper.CLOSE_GOAL,
*args,
**kwargs)Adding leaves for moving to poses and named locations is the same process as above:
from ros_trees.leaves_ros import ActionLeaf
class MoveGripperToPose(ActionLeaf):
def __init__(self, *args, **kwargs):
super(MoveGripperToPose,
self).__init__("Move gripper to pose",
action_namespace='/action/move_gripper/pose',
*args,
**kwargs)
class MoveGripperToLocation(ActionLeaf):
def __init__(self, *args, **kwargs):
super(MoveGripperToLocation,
self).__init__("Move gripper to location",
action_namespace='/action/move_gripper/location',
*args,
**kwargs)Saving leaf results
Now that we have some re-usable leaves, next up let's add leaves to detect bottles from the robot's camera images. This requires first calling the ROS service for getting synced RGB-D images, then passing the result onto the bottle detection service, and finally storing all detections in a persistent location for use later. The leaves are very similar to our leaves above, we just use a Service Leaf instead:
from ros_trees.leaves_ros import ServiceLeaf
class GetSyncedRgbd(ServiceLeaf):
def __init__(self, *args, **kwargs):
super(GetSyncedRgbd,
self).__init__("Get Synced RGBD",
service_name='/service/get_synced_rgbd',
save=True,
*args,
**kwargs)
class DetectBottles(ServiceLeaf):
def __init__(self, *args, **kwargs):
super(DetectBottles,
self).__init__("Detect bottles",
service_name='/service/detect_bottles',
save=True,
*args,
**kwargs)The save flag tells both leaves to save their results for the next leaf to get when loading the "last result".
For this task, we don't want to save the list of detected bottles in last result, but instead save them to a key so they don't get overwritten when another leaf saves to "last result". We could hardcode the key name in the DetectBottles() class here, but then everyone who ever uses the detector leaf will have no control over where the data is saved. Instead, we will set the key name at the instance level in our task when we create our tree (i.e. call DetectBottles(save_key=<MY_KEY_NAME>) when instantiating the leaf in our tree).
Customising leaves with the *_fn settings
Lastly, let's add some basic ROS-free leaves that print a detected objects list and pop (i.e. consume) an item from a list. We have already looked at input and output to a leaf, but these tasks will need us to adjust the processing part of the lifecycle; i.e. defining how a leaf gets a result from the input by providing a result_fn to the constructor. For the ActionLeaf and ServiceLeaf classes used above, their result_fn is already written for us which calls the Action Server / Service with the leaf input data and return the response as the leaf output data.
import ros_trees.data_management as dm
from ros_trees.leaves import Leaf
class PrintObjects(Leaf):
def __init__(self, *args, **kwargs):
super(PrintObjects, self).__init__("Print Objects",
result_fn=self._print_objects,
*args,
**kwargs)
def _print_objects(self):
if self.loaded_data is None or not self.loaded_data:
print("The detector found no objects!")
else:
print(
"The detector found %d objects at the following coordinates:" %
len(self.loaded_data))
for o in self.loaded_data:
print(
"\t'%s' of pixel dimensions %dx%d @ top left coordinates:"
" (%d,%d)" %
(o.class_label, o.width, o.height, o.x_left, o.y_top))
return True
class PopFromList(Leaf):
def __init__(self, pop_position=0, *args, **kwargs):
super(PopFromList, self).__init__("Pop from list",
result_fn=self._pop_item,
*args,
**kwargs)
self.pop_position = pop_position
def _pop_item(self):
if not self.loaded_data:
return None
item = self.loaded_data.pop(self.pop_position)
if self.load_key is not None:
dm.set_value(self.load_key, self.loaded_data)
else:
dm.set_last_value(self, self.loaded_data)
return itemPrintObjects prints from loaded_data (which is guarantee to be loaded for result_fn implementations), and PopFromList uses a new pop_position parameter to pop from a list (which is assumed to be available).
We now have all the leaves needed to complete this task. While this may have seemed a long process, remember that we have done this from scratch with no shared leaves available. As you create more re-usable leaves, implementing complex behaviours will become easier and easier.
The behaviour of placing a bottle in the bin will be repeated many times in our task. Here's a perfect situation for declaring a sub-behaviour, or branch. Our branch will be a series of sequential items housed in a Sequence composite (see resources at bottom of page for details on Sequence composites):
from py_trees.composites import Sequence
class BinItem(Sequence):
def __init__(self, load_pose_key, load_pose_fn=None, *args, **kwargs):
super(BinItem, self).__init__("Bin Item", [
OpenGripper(),
MoveGripperToLocation(load_value='workspace'),
MoveGripperToPose(load_key=load_pose_key, load_fn=load_pose_fn),
CloseGripper(),
MoveGripperToLocation(load_value='bin')
])A couple of conscious choices have been made about hardcoding data vs "passing it up" as branch arguments. The arm will be moved between the workspace and bin locations whenever binning an item, so they have been hardcoded in the branch example.
In contrast, the key where the object pose is saved and method for extracting a pose from the object data could change depending on the task using the BinItem branch. Therefore, we "pass them up" as parameters so the tree using the branch can control that functionality. Being frugal with what values you lock down is crucial if you want your leaves and branches to be useful for other people; if in doubt, "pass it up"!
We could also group getting a synced RGB-D image, detecting an object, and printing the objects into a branch that lets you choose which detector you would like to use. But let's leave it at that for this example.
Developing trees can be an art, an art which is explained by the suggested resources at the bottom of the page. Knowing about leaf status, composites, decorators, and blackboards is strongly recommended as a minimum for creating trees (the py_trees demo tutorials are great for this).
From this knowledge, we decide we want to create the following tree to complete the bottle binning task:
The tree shown above is then realised with ros_trees and our custom leaves via:
from py_trees.composites import Sequence
from ros_trees.trees import BehaviourTree
BehaviourTree(
"Bottle Binner",
Sequence("Bin Bottles", [
GetSyncedRgbd(load_value='camera_wrist'),
DetectBottles(save_key='bottles',
save_fn=object_list_from_response),
PrintObjects(load_key='bottles'),
FailureIsSuccess(
name="F=S",
child=SuccessIsRunning(
name="S=R",
child=Sequence("Bin bottle", [
PopFromList(load_key='bottles', save_key='bottle'),
Print(load_value="Binning bottle..."),
BinItem(load_pose_fn=pose_from_object,
load_pose_key='bottle'),
Print(load_value="Successfully binned bottle!")
]))),
WaitForEnterKey(
load_value="All bottles binned! Press enter to restart ... ")
])).run(hz=30, push_to_start=True, log_level='WARN')Some extra leaves and functions have been used in the tree (Print, WaitForEnterKey, object_list_from_response, and pose_from_object) which we won't discuss here for the sake of brevity (see example code for details). And that's it, a robot that can pick up after you in only one block of code; pretty cool!
