Tutorials

Dependencies

The tutorials require installation of a few graphical dependencies that would otherwise unnecessarily explode the requirements of the core package and modules. e.g. for kinetic:

sudo apt install ros-kinetic-rqt-py-trees ros-kinetic-rqt-reconfigure

The Mock Robot

The tutorials here all run atop a very simple mock robot that encapsulates the following list of mocked components:

• Battery
• LED Strip
• Docking Action Server
• Move Base Action Server
• Rotation Action Server
• Safety Sensors Pipeline

This mock robot could just as easily be replaced by a gazebo simulated robot or even real robot with the same ROS API abstraction layer.

The tutorials take care of launching the mock robot, but it can be launched on its own with:

py_trees_ros/launch/mock_robot.launch

<launch>
  <node pkg="py_trees_ros" name="move_base" type="mock_component" args="move_base.MoveBase">
      <param name="duration" value="5" />
  </node>
  <node pkg="py_trees_ros" name="dock"      type="mock_component" args="dock.Dock"/>
  <node pkg="py_trees_ros" name="rotate"    type="mock_component" args="rotate.Rotate"/>
  <node pkg="py_trees_ros" name="battery"   type="mock_battery"/>
  <node pkg="py_trees_ros" name="led_strip" type="mock_led_strip"/>
  <node pkg="py_trees_ros" name="safety_sensors"   type="mock_safety_sensors"/>
</launch>

Tutorial 1 - Data Gathering

About

In this, the first of the tutorials, we start out by using a behaviour to collect battery data from a ros subscriber and cache the result on the blackboard for other behaviours to utilise.

Data gathering up front via subscribers is a useful practice that can by justified by one of a variety of reasons. In order of priority:

• Lock incoming data for the remainder of the tick so that decision making is consistent across the entire tree
• Avoid invoking the rospy threading model when it is not necessary
• Python access to the blackboard is easier than ROS api handling

In short, it takes the asynchronicity out of the subscriber callbacks and when it comes to sharing the data, it is far simpler to access the blackboard than to manage multiple subscribers spread across the entire behaviour tree.

Usually you will end up with a collection of data gatherers at the front end of your behaviour tree which will always run and will happen before any decision branching can occur in the tree.

Tree

py_trees_ros/tutorials/one.py#create_root

def create_root():
    """
    Create a basic tree and start a 'Topics2BB' work sequence that
    takes the asynchronicity out of subscription.

    Returns:
        :class:`~py_trees.behaviour.Behaviour`: the root of the tree
    """
    root = py_trees.composites.Parallel("Tutorial")

    topics2bb = py_trees.composites.Sequence("Topics2BB")
    battery2bb = py_trees_ros.battery.ToBlackboard(name="Battery2BB",
                                                   topic_name="/battery/state",
                                                   threshold=30.0
                                                   )
    priorities = py_trees.composites.Selector("Priorities")
    idle = py_trees.behaviours.Running(name="Idle")

    root.add_child(topics2bb)
    topics2bb.add_child(battery2bb)
    root.add_child(priorities)
    priorities.add_child(idle)
    return root

Along with the data gathering side, you’ll also notice the dummy branch for priority jobs (complete with idle behaviour that is always RUNNING). This configuration is typical of the data gathering pattern.

Behaviours

The tree makes use of the py_trees_ros.battery.ToBlackboard behaviour.

py_trees_ros/battery.py

class ToBlackboard(subscribers.ToBlackboard):
    """
    Subscribes to the battery message and writes battery data to the blackboard.
    Also adds a warning flag to the blackboard if the battery
    is low - note that it does some buffering against ping-pong problems so the warning
    doesn't trigger on/off rapidly when close to the threshold.

    When ticking, updates with :attr:`~py_trees.common.Status.RUNNING` if it got no data,
    :attr:`~py_trees.common.Status.SUCCESS` otherwise.

    Blackboard Variables:
        * battery (:class:`sensor_msgs.msg.BatteryState`)[w]: the raw battery message
        * battery_low_warning (:obj:`bool`)[w]: False if battery is ok, True if critically low

    Args:
        name (:obj:`str`): name of the behaviour
        topic_name (:obj:`str`) : name of the battery state topic
        threshold (:obj:`float`) : percentage level threshold for flagging as low (0-100)
    """
    def __init__(self, name, topic_name="/battery/state", threshold=30.0):
        super(ToBlackboard, self).__init__(name=name,
                                           topic_name=topic_name,
                                           topic_type=sensor_msgs.BatteryState,
                                           blackboard_variables={"battery": None},
                                           clearing_policy=py_trees.common.ClearingPolicy.NEVER
                                           )
        self.blackboard = py_trees.blackboard.Blackboard()
        self.blackboard.battery = sensor_msgs.BatteryState()
        self.blackboard.battery.percentage = 0.0
        self.blackboard.battery.power_supply_status = sensor_msgs.BatteryState.POWER_SUPPLY_STATUS_UNKNOWN
        self.blackboard.battery_low_warning = False   # decision making
        self.threshold = threshold

    def update(self):
        """
        Call the parent to write the raw data to the blackboard and then check against the
        threshold to determine if the low warning flag should also be updated.
        """
        self.logger.debug("%s.update()" % self.__class__.__name__)
        status = super(ToBlackboard, self).update()
        if status != py_trees.common.Status.RUNNING:
            # we got something
            if self.blackboard.battery.percentage > self.threshold + 5.0:
                self.blackboard.battery_low_warning = False
            elif self.blackboard.battery.percentage < self.threshold:
                    self.blackboard.battery_low_warning = True
                    rospy.logwarn_throttle(60, "%s: battery level is low!" % self.name)
            # else don't do anything in between - i.e. avoid the ping pong problems

            self.feedback_message = "Battery level is low" if self.blackboard.battery_low_warning else "Battery level is ok"
        return status

This behaviour will cause the entire tree will tick over with SUCCESS so long as there is data incoming. If there is no data incoming, it will simply block and prevent the rest of the tree from acting.

Running

$ roslaunch py_trees_ros tutorial_one.launch --screen

A glimpse of the blackboard with battery updates:

Tutorial 2 - Adding a Battery Check

About

Here we add the first decision. What to do if the battery is low? For this, we’ll get the robot to flash it’s led strip.

Tree

The white coloured ellipse shown here is a decorated behaviour (in this case a Selector with the success_is_failure() decorator (looking forward to better visualisations for this in the future).

py_trees_ros/tutorials/two.py

def create_root():
    # behaviours
    root = py_trees.composites.Parallel("Tutorial")
    topics2bb = py_trees.composites.Sequence("Topics2BB")
    battery2bb = py_trees_ros.battery.ToBlackboard(name="Battery2BB",
                                                   topic_name="/battery/state",
                                                   threshold=30.0
                                                   )
    priorities = py_trees.composites.Selector("Priorities")
    battery_check = py_trees.meta.success_is_failure(py_trees.composites.Selector)(name="Battery Emergency")
    is_battery_ok = py_trees.blackboard.CheckBlackboardVariable(
        name="Battery Ok?",
        variable_name='battery_low_warning',
        expected_value=False
    )
    flash_led_strip = py_trees_ros.tutorials.behaviours.FlashLedStrip(
        name="FlashLEDs",
        colour="red")
    idle = py_trees.behaviours.Running(name="Idle")

    # tree
    root.add_children([topics2bb, priorities])
    topics2bb.add_child(battery2bb)
    priorities.add_children([battery_check, idle])
    battery_check.add_children([is_battery_ok, flash_led_strip])
    return root

Here we’ve added a high priority branch for dealing with a low battery that causes the hardware strip to flash.

An important point here is to make sure that the flashing behaviour gets invalidated as soon as the battery becomes ok again. This will trigger the flashing behaviour’s terminate method (see below) to send off a command to clear the request. To do this we’ve made use of a higher priority ‘Is Battery Ok?’ check underneath the selector, but also had to decorate the selector with success_is_failure() to make sure the priority branch is chosen appropriately.

You could have also designed this particular subtree with sequences and parallels instead of the selector and decorator here.

Tip

When designing, it’s very useful to get a visual on what you are doing, even before you actually execute or implement anything more than a tree of skeleton behaviours. For this tutorial, you can render with:

py-trees-render py_trees_ros.tutorials.two.create_root

Behaviours

Introducing the flashing behaviour!

py_trees_ros/tutorials/behaviours.py#Flashing

class FlashLedStrip(py_trees.behaviour.Behaviour):
    """
    This behavoiur simply shoots a command off to the LEDStrip to flash
    a certain colour and returns :attr:`~py_trees.common.Status.RUNNING`.
    Note that this behaviour will never return with
    :attr:`~py_trees.common.Status.SUCCESS` but will send a clearing
    command to the LEDStrip if it is cancelled or interrupted by a higher
    priority behaviour.

    Args:
        name (:obj:`str`): name of the behaviour
        topic_name (:obj:`str`) : name of the battery state topic
        colour (:obj:`str`) : colour to flash ['red', 'green', blue']
    """
    def __init__(self, name, topic_name="/led_strip/command", colour="red"):
        super(FlashLedStrip, self).__init__(name=name)
        self.topic_name = topic_name
        self.colour = colour

    def setup(self, timeout):
        """
        Args:
            timeout (:obj:`float`): time to wait (0.0 is blocking forever)

        Returns:
            :obj:`bool`: whether it timed out trying to setup
        """
        self.publisher = rospy.Publisher(self.topic_name, std_msgs.String, queue_size=10, latch=True)
        self.feedback_message = "setup"
        return True

    def update(self):
        """
        Annoy the led strip to keep firing every time it ticks over (the led strip will clear itself
        if no command is forthcoming within a certain period of time). This behaviour will only finish if it
        is terminated or interrupted from above.
        """
        self.logger.debug("%s.update()" % self.__class__.__name__)
        self.publisher.publish(std_msgs.String(self.colour))
        self.feedback_message = "flashing {0}".format(self.colour)
        return py_trees.common.Status.RUNNING

    def terminate(self, new_status):
        """
        Shoot off a clearing command to the led strip.

        Args:
            new_status (:class:`~py_trees.common.Status`): the behaviour is transitioning to this new status
        """
        self.publisher.publish(std_msgs.String(""))

A new feature here is the way it uses the terminate method to put a ‘fullstop’ to the commands sent when ticking. Note also that it is permanently in the RUNNING state while ticking. Behaviours do not have to return SUCCESS or FAILURE, they can be just as involved in the decision making via the way they behave when cancelled or interrupted.

Running

$ roslaunch py_trees_ros tutorial_two.launch --screen

Then play around with the battery level in dynamic reconfigure to trigger the decision branching:

Tutorial 3 - Blackboards!

About

Tutorial three is a repeat of Tutorial 2 - Adding a Battery Check. The purpose of this tutorial however is to introduce the publishers and services provided to allow introspection of the blackboard from ROS. Publishers and services are provided by the Blackboard Exchange embedded in the ROS Behaviour Tree and interaction via the py-trees-blackboard-watcher command line utility.

Running

Launch the tutorial:

$ roslaunch py_trees_ros tutorial_three.launch --screen

In another shell:

# check the entire board
$ py-trees-blackboard-watcher
# determine what you may stream
$ py-trees-blackboard-watcher --list-variables
# pull a simple variable
$ py-trees-blackboard-watcher battery_low_warning
# drill down to get a variable
$ py-trees-blackboard-watcher battery/percentage

Tutorial 4 - Introspecting the Tree

About

Again, this is a repeat of Tutorial 2 - Adding a Battery Check. The ROS Behaviour Tree used in these tutorials provide several ROS connections for tree introspection. We will walkthrough several of these here:

Running

Launch

$ roslaunch py_trees_ros tutorial_four.launch --screen

Shell Introspection

# identify the tree publishers
$ rostopic list | grep tree
# a static ascii view of the tree (lacks formatting)
$ rostopic echo /tree/ascii/tree
# a static ascii view of the tree (with formatting)
$ py-trees-tree-watcher --tree
# a dynamic view of the tree (lacks formatting), with current state and feedback messages
$ rostopic echo /tree/ascii/snaphots
# a dynamic view of the tree (with formatting)
$ py-trees-tree-watcher --snapshot
# at what behaviour did decision making turn around and why? often useful for other programs to know
$ rostopic echo /tree/tip

and of course the blackboard topics, but they were covered in tutorial three.

RQT Visualisation

There is one more topic that provides the complete set of data for a tree state, that is ~/log/tree. This is not very human readable (typically very large), but can be used for other tools such as rqt-py-trees.

While not in this package itself, it’s worth taking some time out to discover what it can do here.

• Coloured States : GREEN for SUCCESS, RED for FAILURE, BLUE for RUNNING and GREY for unvisited.
• Tooltips : hover over a behaviour to catch name, type, status and feedback message information
• Timeline : rewind as you wish, note the bars indicating where important events occured
• Fit : disable auto-resizing by unchecking the ‘Fit’ button to handle large trees which become unreadable in a small window

Tutorial 5 - Action Clients

About

A few new items arriving in tantalising bowls of flying spaghetti here:

• A gui for manually triggering events
• A gui (same one) for visualising the led strip status
• A lower priority work branch triggered from the gui
• A first action client behaviour
• A kind of pre-emption, via behaviour tree decision logic

Tree

py_trees_ros/tutorials/five.py#create_root

def create_root():
    # behaviours
    root = py_trees.composites.Parallel("Tutorial")
    topics2bb = py_trees.composites.Sequence("Topics2BB")
    scan2bb = py_trees_ros.subscribers.EventToBlackboard(
        name="Scan2BB",
        topic_name="/dashboard/scan",
        variable_name="event_scan_button"
    )
    battery2bb = py_trees_ros.battery.ToBlackboard(name="Battery2BB",
                                                   topic_name="/battery/state",
                                                   threshold=30.0
                                                   )
    priorities = py_trees.composites.Selector("Priorities")
    battery_check = py_trees.meta.success_is_failure(py_trees.composites.Selector)(name="Battery Emergency")
    is_battery_ok = py_trees.blackboard.CheckBlackboardVariable(
        name="Battery Ok?",
        variable_name='battery_low_warning',
        expected_value=False
    )
    flash_led_strip = py_trees_ros.tutorials.behaviours.FlashLedStrip(
        name="Flash Red",
        colour="red")

    scan = py_trees.composites.Sequence(name="Scan")
    is_scan_requested = py_trees.blackboard.CheckBlackboardVariable(
        name="Scan?",
        variable_name='event_scan_button',
        expected_value=True
    )
    scan_preempt = py_trees.composites.Selector(name="Preempt?")
    is_scan_requested_two = py_trees.meta.success_is_running(py_trees.blackboard.CheckBlackboardVariable)(
        name="Scan?",
        variable_name='event_scan_button',
        expected_value=True
    )
    scanning = py_trees.composites.Parallel(name="Scanning", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
    scan_rotate = py_trees_ros.actions.ActionClient(
        name="Rotate",
        action_namespace="/rotate",
        action_spec=py_trees_msgs.RotateAction,
        action_goal=py_trees_msgs.RotateGoal(),
        override_feedback_message_on_running="rotating"
    )
    scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Blue", colour="blue")
    scan_celebrate = py_trees.composites.Parallel(name="Celebrate", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
    scan_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Green", colour="green")
    scan_pause = py_trees.timers.Timer("Pause", duration=3.0)
    idle = py_trees.behaviours.Running(name="Idle")

    # tree
    root.add_children([topics2bb, priorities])
    topics2bb.add_children([scan2bb, battery2bb])
    priorities.add_children([battery_check, scan, idle])
    battery_check.add_children([is_battery_ok, flash_led_strip])
    scan.add_children([is_scan_requested, scan_preempt, scan_celebrate])
    scan_preempt.add_children([is_scan_requested_two, scanning])
    scanning.add_children([scan_rotate, scan_flash_blue])
    scan_celebrate.add_children([scan_flash_green, scan_pause])

Guards

The entire scan branch is protected by a guard (note that the blackbox in the above diagram is exactly that, a black box representing the lower part of the tree). Once the scan event is received, this branch gets to work until it either finishes, or is pre-empted by the higher priority low battery branch.

A Kind of Preemption

The second part of the tree enables a kind of pre-emption on the scanning action. If a new request comes in, it will trigger the secondary scan event check, invalidating whatever scanning action was currently running. This will clear the led command and cancel the rotate action. On the next tick, the scan event check will fail (it was consumed on the last tick) and the scanning will restart.

Note

This is not true pre-emption since it cancels the rotate action and restarts it. It is however, exactly the pattern that is required in many instances. For true pre-emption you could bundle both scan check and rotation action in the same behaviour or dynamically insert action goals on the fly from the parent class.

Handling Failure

If the rotate action should fail, then the whole branch will also fail. Subsequently dropping the robot back to its idle state. A failure event could be generated by simply watching either the ‘Scanning’ parallel or the tip() of the tree and reacting to it’s state change.

Behaviours

Introducing the rotate action client behaviour!

py_trees_ros/tutorials/five.py#action_client_instantiation

    scan_rotate = py_trees_ros.actions.ActionClient(
        name="Rotate",
        action_namespace="/rotate",
        action_spec=py_trees_msgs.RotateAction,
        action_goal=py_trees_msgs.RotateGoal(),
        override_feedback_message_on_running="rotating"

py_trees_ros/actions.py#ActionClient

class ActionClient(py_trees.behaviour.Behaviour):
    """
    A generic action client interface. This simply sends a pre-configured
    goal to the action client.

    Cases where you might want to subclass and extend this behaviour:

    * Update the goal data in :meth:`~py_trees_ros.actions.ActionClient.initialise()`

      * e.g. use blackboard data to determine the new characteristics of your goal
    * Trigger true pre-emption by sending a new goal in :meth:`~py_trees_ros.actions.ActionClient.update()`

    Args:
        name (:obj:`str`): name of the behaviour
        action_spec (:obj:`any`): spec type for the action (e.g. move_base_msgs.msg.MoveBaseAction)
        action_goal (:obj:`any`): preconfigured action goal (e.g. move_base_msgs.msg.MoveBaseGoal())
        action_namespace (:obj:`str`): where you can find the action topics
        override_feedback_message_on_running (:obj:`str`): override the feedback message from the server

    Feedback messages are often accompanied with detailed messages that continuously change - since these
    are not significant and we don't want to log every change due to these messages, you can provide an override
    here that simply signifies the action is running.

    @todo: a more comprehensive way of filtering/parsing feedback messages to customise a running state so
    that it can identify and react to 'significant' events while running.
    """
    def __init__(self, name="Action Client", action_spec=None, action_goal=None, action_namespace="/action",
                 override_feedback_message_on_running="moving"):
        super(ActionClient, self).__init__(name)
        self.action_client = None
        self.sent_goal = False
        self.action_spec = action_spec
        self.action_goal = action_goal
        self.action_namespace = action_namespace
        self.override_feedback_message_on_running = override_feedback_message_on_running

    def setup(self, timeout):
        """
        Args:
            timeout (:obj:`float`): time to wait (0.0 is blocking forever)

        Returns:
            :obj:`bool`: whether it timed out trying to setup
        """
        self.logger.debug("%s.setup()" % self.__class__.__name__)
        self.action_client = actionlib.SimpleActionClient(
            self.action_namespace,
            self.action_spec
        )
        if not self.action_client.wait_for_server(rospy.Duration(timeout)):
            self.logger.error("{0}.setup() could not connect to the action server at '{1}'".format(self.__class__.__name__, self.action_namespace))
            self.action_client = None
            return False
        return True

    def initialise(self):
        """
        Reset the internal variables.
        """
        self.logger.debug("{0}.initialise()".format(self.__class__.__name__))
        self.sent_goal = False

    def update(self):
        """
        Check only to see whether the underlying action server has
        succeeded, is running, or has cancelled/aborted for some reason and
        map these to the usual behaviour return states.
        """
        self.logger.debug("{0}.update()".format(self.__class__.__name__))
        if not self.action_client:
            self.feedback_message = "no action client, did you call setup() on your tree?"
            return py_trees.Status.INVALID
        # pity there is no 'is_connected' api like there is for c++
        if not self.sent_goal:
            self.action_client.send_goal(self.action_goal)
            self.sent_goal = True
            self.feedback_message = "sent goal to the action server"
            return py_trees.Status.RUNNING
        self.feedback_message = self.action_client.get_goal_status_text()
        if self.action_client.get_state() in [actionlib_msgs.GoalStatus.ABORTED,
                                              actionlib_msgs.GoalStatus.PREEMPTED]:
            return py_trees.Status.FAILURE
        result = self.action_client.get_result()
        if result:
            return py_trees.Status.SUCCESS
        else:
            self.feedback_message = self.override_feedback_message_on_running
            return py_trees.Status.RUNNING

    def terminate(self, new_status):
        """
        If running and the current goal has not already succeeded, cancel it.

        Args:

The ActionClient is a generic template that can be used as a drop-in for very simply monitoring the aborted/cancelled/running/success state of an underlying controller with a pre-configured goal. See the api for details on when/how you might wish to extend this.

Running

$ roslaunch py_trees_ros tutorial_five.launch --screen

Playing with the Spaghetti

• Press the scan button to start a scan
• Press the scan button again while mid-scanning to pre-empt
• Set battery low in reconfigure whilst mid-scanning to priority switch

Tutorial 6 - Context Switching

About

This tutorial inserts a context switching behaviour to run in tandem with the scan rotation. It will reconfigure the rotation speed (to be slower) and enable a hypothetical safety sensor pipeline immediately prior to running the rotate action and subsequently reset the context to what it was before commencing upon termination (success/failure or interruption from above) of the scan.

Note

This context switch could have easily been bundled into the scan behaviour itself, however keeping it separate promotes visibility and allows the scan behaviour to remain eminently reusable. Context switches can tend to be quite specific to a use case (i.e. not a behaviour).

Tree

py_trees_ros/tutorials/six.py#create_root

##############################################################################


def create_root():
    # behaviours
    root = py_trees.composites.Parallel("Tutorial")
    topics2bb = py_trees.composites.Sequence("Topics2BB")
    scan2bb = py_trees_ros.subscribers.EventToBlackboard(
        name="Scan2BB",
        topic_name="/dashboard/scan",
        variable_name="event_scan_button"
    )
    battery2bb = py_trees_ros.battery.ToBlackboard(name="Battery2BB",
                                                   topic_name="/battery/state",
                                                   threshold=30.0
                                                   )
    priorities = py_trees.composites.Selector("Priorities")
    battery_check = py_trees.meta.success_is_failure(py_trees.composites.Selector)(name="Battery Emergency")
    is_battery_ok = py_trees.blackboard.CheckBlackboardVariable(
        name="Battery Ok?",
        variable_name='battery_low_warning',
        expected_value=False
    )
    flash_led_strip = py_trees_ros.tutorials.behaviours.FlashLedStrip(
        name="Flash Red",
        colour="red")

    scan = py_trees.composites.Sequence(name="Scan")
    is_scan_requested = py_trees.blackboard.CheckBlackboardVariable(
        name="Scan?",
        variable_name='event_scan_button',
        expected_value=True
    )
    scan_preempt = py_trees.composites.Selector(name="Preempt?")
    is_scan_requested_two = py_trees.meta.success_is_running(py_trees.blackboard.CheckBlackboardVariable)(
        name="Scan?",
        variable_name='event_scan_button',
        expected_value=True
    )
    scanning = py_trees.composites.Parallel(name="Scanning", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
    scan_context_switch = py_trees_ros.tutorials.behaviours.ScanContext("Context Switch")
    scan_rotate = py_trees_ros.actions.ActionClient(
        name="Rotate",
        action_namespace="/rotate",
        action_spec=py_trees_msgs.RotateAction,
        action_goal=py_trees_msgs.RotateGoal(),
        override_feedback_message_on_running="rotating"
    )
    scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Blue", colour="blue")
    scan_celebrate = py_trees.composites.Parallel(name="Celebrate", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
    scan_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Green", colour="green")
    scan_pause = py_trees.timers.Timer("Pause", duration=3.0)
    idle = py_trees.behaviours.Running(name="Idle")

    # tree
    root.add_children([topics2bb, priorities])
    topics2bb.add_children([scan2bb, battery2bb])
    priorities.add_children([battery_check, scan, idle])
    battery_check.add_children([is_battery_ok, flash_led_strip])
    scan.add_children([is_scan_requested, scan_preempt, scan_celebrate])

Context Switch

The context switch is embedded beneath a parallel. Context will be cached and reconfigured upon entry to the context and reset when the parallel finalises or is interrupted from above.

Behaviours

Introducing the scan context behaviour!

py_trees_ros/tutorials/behaviours.py#scan_context

class ScanContext(py_trees.behaviour.Behaviour):
    """
    This behavoiur simply shoots a command off to the LEDStrip to flash
    a certain colour and returns :attr:`~py_trees.common.Status.RUNNING`.
    Note that this behaviour will never return with
    :attr:`~py_trees.common.Status.SUCCESS` but will send a clearing
    command to the LEDStrip if it is cancelled or interrupted by a higher
    priority behaviour.

    Args:
        name (:obj:`str`): name of the behaviour
    """
    def __init__(self, name):
        super(ScanContext, self).__init__(name=name)
        self.initialised = False
        self._namespaces = ["safety_sensors",
                            "rotate",
                            ]
        self._dynamic_reconfigure_clients = {}
        for name in self._namespaces:
            self._dynamic_reconfigure_clients[name] = None
        self._dynamic_reconfigure_configurations = {}

    def setup(self, timeout):
        """
        Try and connect to the dynamic reconfigure server on the various namespaces.
        """
        self.logger.debug("%s.setup()" % self.__class__.__name__)
        for namespace in self._namespaces:
            if not self._dynamic_reconfigure_clients[namespace]:
                try:
                    self._dynamic_reconfigure_clients[namespace] = dynamic_reconfigure.client.Client(
                        name=namespace,
                        timeout=timeout
                    )
                except rospy.ROSException:
                    rospy.logwarn("ScanContext [%s" % self.name + "]: could not connect to dynamic reconfigure server [%s][%s secs]" % (namespace, timeout))
                    self.feedback_message = "could not connect to dynamic reconfigure server [%s][%s secs]" % (namespace, timeout)
                    return False
        return True

    def initialise(self):
        """
        Get various dyn reconf configurations and cache/set the new variables.
        """
        self.logger.debug("%s.initialise()" % self.__class__.__name__)

        for name, client in self._dynamic_reconfigure_clients.iteritems():
            self._dynamic_reconfigure_configurations[name] = client.get_configuration()
        try:
            self.safety_sensors_enable = self._dynamic_reconfigure_configurations["safety_sensors"]["enable"]
            self._dynamic_reconfigure_clients["safety_sensors"].update_configuration({"enable": True})
        except dynamic_reconfigure.DynamicReconfigureParameterException:
            self.feedback_message = "failed to configure the 'enable' parameter [safety_sensors]"
            self.initialised = False
        try:
            self.rotate_duration = self._dynamic_reconfigure_configurations["rotate"]["duration"]
            self._dynamic_reconfigure_clients["rotate"].update_configuration({"duration": 8.0})
        except dynamic_reconfigure.DynamicReconfigureParameterException:
            self.feedback_message = "failed to configure the 'duration' parameter [rotate]"
            self.initialised = False

        self.initialised = True
        self.feedback_message = "reconfigured the context for scanning"

    def update(self):
        self.logger.debug("%s.update()" % self.__class__.__name__)
        if not self.initialised:
            return py_trees.common.Status.FAILURE
        # used under a parallel, never returns success
        return py_trees.common.Status.RUNNING

    def terminate(self, new_status):
        """
        Regardless of whether it succeeed or failed or is getting set to invalid we have to be absolutely
        sure to reset the navi context.
        """
        self.logger.debug("%s.terminate(%s)" % (self.__class__.__name__, "%s->%s" % (self.status, new_status) if self.status != new_status else "%s" % new_status))
        if self.initialised:
            try:
                self._dynamic_reconfigure_clients["safety_sensors"].update_configuration({"enable": self.safety_sensors_enable})
            except dynamic_reconfigure.DynamicReconfigureParameterException:
                self.feedback_message = "failed to reset the 'enable' parameter [safety_sensors]"
            try:
                self._dynamic_reconfigure_clients["rotate"].update_configuration({"duration": self.rotate_duration})
            except dynamic_reconfigure.DynamicReconfigureParameterException:
                self.feedback_message = "failed to reset the 'duration' parameter [rotate]"
            self.initialised = False

As you can see, all the action is happening in the initialise() and terminate() methods. This class is intended for use underneath a parallel with the action(s) that are designed to run in this context. This guarantees that the context is reset no matter whether the action(s) succeed or fail, or the parallel itself is interrupted from above.

Running

$ roslaunch py_trees_ros tutorial_six.launch --screen

Playing with the Spaghetti

• Watch the the rotate and safety_sensor namespaces in rqt_reconfigure
• Press the scan button to start a scan
• The enable and duration variables should change while running
• The enable and duration variables should reset to initial values when finished

You can see in the diagrams below the relevant dynamic reconfigure variables switching as the context runs.

Tutorial 7 - Docking, Cancelling & Failing

About

Round out the scan job subtree with:

• docking/undocking
• moving out and back to home on either side of the scan
• cancel request handlers
• failure notifications (flashing red)

Nothing new here technically, this serves as a reasonable template from which to start your own job subtrees.

Tree

py_trees_ros/tutorials/seven.py#create_root

def create_root():
    # behaviours
    root = py_trees.composites.Parallel("Tutorial")
    topics2bb = py_trees.composites.Sequence("Topics2BB")
    scan2bb = py_trees_ros.subscribers.EventToBlackboard(
        name="Scan2BB",
        topic_name="/dashboard/scan",
        variable_name="event_scan_button"
    )
    cancel2bb = py_trees_ros.subscribers.EventToBlackboard(
        name="Cancel2BB",
        topic_name="/dashboard/cancel",
        variable_name="event_cancel_button"
    )
    battery2bb = py_trees_ros.battery.ToBlackboard(name="Battery2BB",
                                                   topic_name="/battery/state",
                                                   threshold=30.0
                                                   )
    priorities = py_trees.composites.Selector("Priorities")
    battery_check = py_trees.meta.success_is_failure(py_trees.composites.Selector)(name="Battery Emergency")
    is_battery_ok = py_trees.blackboard.CheckBlackboardVariable(
        name="Battery Ok?",
        variable_name='battery_low_warning',
        expected_value=False
    )
    flash_led_strip = py_trees_ros.tutorials.behaviours.FlashLedStrip(
        name="Flash Red",
        colour="red")
    scan = py_trees.composites.Sequence(name="Scan")
    is_scan_requested = py_trees.blackboard.CheckBlackboardVariable(
        name="Scan?",
        variable_name='event_scan_button',
        expected_value=True
    )
    scan_or_die = py_trees.composites.Selector(name="Scan or Die")
    die = py_trees_ros.tutorials.behaviours.FlashLedStrip(
        name="Uh Oh",
        colour="red")
    ere_we_go = py_trees.composites.Sequence(name="Ere we Go")
    undock = py_trees_ros.actions.ActionClient(
        name="UnDock",
        action_namespace="/dock",
        action_spec=py_trees_msgs.DockAction,
        action_goal=py_trees_msgs.DockGoal(False),
        override_feedback_message_on_running="undocking"
    )
    scan_or_be_cancelled = py_trees.composites.Selector("Scan or Be Cancelled")
    cancelling = py_trees.composites.Sequence("Cancelling?")
    is_cancel_requested = py_trees.blackboard.CheckBlackboardVariable(
        name="Cancel?",
        variable_name='event_cancel_button',
        expected_value=True
    )
    move_home_after_cancel = py_trees_ros.actions.ActionClient(
        name="Move Home",
        action_namespace="/move_base",
        action_spec=move_base_msgs.MoveBaseAction,
        action_goal=move_base_msgs.MoveBaseGoal()
    )
    move_out_and_scan = py_trees.composites.Sequence("Move Out and Scan")
    move_base = py_trees_ros.actions.ActionClient(
        name="Move Out",
        action_namespace="/move_base",
        action_spec=move_base_msgs.MoveBaseAction,
        action_goal=move_base_msgs.MoveBaseGoal()
    )
    scanning = py_trees.composites.Parallel(name="Scanning", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
    scan_context_switch = py_trees_ros.tutorials.behaviours.ScanContext("Context Switch")
    scan_rotate = py_trees_ros.actions.ActionClient(
        name="Rotate",
        action_namespace="/rotate",
        action_spec=py_trees_msgs.RotateAction,
        action_goal=py_trees_msgs.RotateGoal(),
        override_feedback_message_on_running="rotating"
    )
    scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Blue", colour="blue")
    move_home_after_scan = py_trees_ros.actions.ActionClient(
        name="Move Home",
        action_namespace="/move_base",
        action_spec=move_base_msgs.MoveBaseAction,
        action_goal=move_base_msgs.MoveBaseGoal()
    )
    celebrate = py_trees.composites.Parallel(name="Celebrate", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
    celebrate_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Green", colour="green")
    celebrate_pause = py_trees.timers.Timer("Pause", duration=3.0)
    dock = py_trees_ros.actions.ActionClient(
        name="Dock",
        action_namespace="/dock",
        action_spec=py_trees_msgs.DockAction,
        action_goal=py_trees_msgs.DockGoal(True),
        override_feedback_message_on_running="docking"
    )
    idle = py_trees.behaviours.Running(name="Idle")

    # tree
    root.add_children([topics2bb, priorities])
    topics2bb.add_children([scan2bb, cancel2bb, battery2bb])
    priorities.add_children([battery_check, scan, idle])
    battery_check.add_children([is_battery_ok, flash_led_strip])
    scan.add_children([is_scan_requested, scan_or_die])
    scan_or_die.add_children([ere_we_go, die])
    ere_we_go.add_children([undock, scan_or_be_cancelled, dock, celebrate])
    scan_or_be_cancelled.add_children([cancelling, move_out_and_scan])
    cancelling.add_children([is_cancel_requested, move_home_after_cancel])
    move_out_and_scan.add_children([move_base, scanning, move_home_after_scan])

Tree Status Reports

Of interest also here is the use of a post tick handler to do tree introspection and publish a status report. In this example the status report is very simple, it merely publishes whether it is “scanning”, “cancelling” or is “idle’.

py_trees_ros/tutorials/seven.py#reality_report

    return root


class SplinteredReality(object):

    def __init__(self):
        self.tree = py_trees_ros.trees.BehaviourTree(create_root())
        self.tree.add_post_tick_handler(self.publish_reality_report)
        self.report_publisher = rospy.Publisher("~report", std_msgs.String, queue_size=5)

    def setup(self):
        return self.tree.setup(timeout=15)

    def publish_reality_report(self, tree):
        if tree.tip().has_parent_with_name("Battery Emergency"):
            self.report_publisher.publish("battery")
        elif tree.tip().has_parent_with_name("Cancelling?"):
            self.report_publisher.publish("cancelling")
        elif tree.tip().has_parent_with_name("Scan"):
            self.report_publisher.publish("scanning")
        else:
            self.report_publisher.publish("idle")

    def tick_tock(self):
        self.tree.tick_tock(500)

Running

$ roslaunch py_trees_ros tutorial_seven.launch --screen

Playing with the Spaghetti

• Press the scan button to start a scan
• Pre-empting no longer works, press the cancel button to interrupt a scan

Tutorial 8 - Dynamic Job Handling

About

The previous tutorial enables execution of a specific job, ‘scanning’ upon request. However, as you start expanding the scope of your development, you will need to start handling use cases with increasing complexity.

1. Multiple jobs at different priorities, Job A > Job B > Job C
2. Multiple jobs but execution is exclusive, Job A, B, if Job A is running, reject requests for Job B
3. Same core tree, but different jobs on different robots, Job A, B on Robot A, Job C on Robot B
4. Alter the list of permitted jobs dynamically, Job A on Robot A, later Job A, B on Robot A

The first use case is not as common as you’d expect - more often than not a robot must follow a job through from start to finish - higher priority switching does not support this.

Here we demonstrate how to (naively) facilitate 2 and 3. A list of jobs destined to run will be loaded at launch time, but subtrees are not inserted until execution is requested, i.e. just in time. When an incoming request is received, the corresponding subtree will be inserted if anoother subtree is not currently running. Once a subtree runs through to completion, it will be pruned. Insertion and pruning happens in the window between ticks.

An technical requirement that makes this implementation practical is to decouple the dependencies on job providers so that your launch does not become burdened by explicity dependencies on any and all jobs. Applications should depend on the application launcher, not the other way around.

Tree

The core tree is identical to that used in Tutorial 7 - Docking, Cancelling & Failing, but with the job subtree removed.

Job Handler

The job subtree create method is moved out into a separate class. Potentially this could be a class in another module, another package (i.e. decopuled from where the core subtree is defined. The class includes additional machinery listening for triggers to initiate job subtree insertion (and subsequently, execution).

py_trees_ros/tutorials/jobs.py#scan

class Scan(object):
    """
    A job handler that instantiates a subtree for scanning to be executed by
    a behaviour tree.
    """

    def __init__(self):
        """
        Tune into a channel for incoming goal requests. This is a simple
        subscriber here but more typically would be a service or action interface.
        """
        self._subscriber = rospy.Subscriber("/dashboard/scan", std_msgs.Empty, self.incoming)
        self._goal = None
        self._lock = threading.Lock()

    @property
    def goal(self):
        """
        Getter for the variable indicating whether or not a goal has recently been received
        but not yet handled. It simply makes sure it is wrapped with the appropriate locking.
        """
        with self._lock:
            g = copy.copy(self._goal) or self._goal
        return g

    @goal.setter
    def goal(self, value):
        """
        Setter for the variable indicating whether or not a goal has recently been received
        but not yet handled. It simply makes sure it is wrapped with the appropriate locking.
        """
        with self._lock:
            self._goal = value

    def incoming(self, msg):
        """
        Incoming goal callback.

        Args:
            msg (:class:`~std_msgs.Empty`): incoming goal message
        """
        if self.goal:
            rospy.logerr("Scan: rejecting new goal, previous still in the pipeline")
        else:
            self.goal = msg

    def create_report_string(self, subtree_root):
        """
        Introspect the subtree root to determine an appropriate human readable status report string.

        Args:
            subtree_root (:class:`~py_trees.behaviour.Behaviour`): introspect the subtree

        Returns:
            :obj:`str`: human readable substring
        """
        if subtree_root.tip().has_parent_with_name("Cancelling?"):
            return "cancelling"
        else:
            return "scanning"

    @staticmethod
    def create_root(goal=std_msgs.Empty()):
        """
        Create the job subtree based on the incoming goal specification.

        Args:
            goal (:class:`~std_msgs.msg.Empty`): incoming goal specification

        Returns:
           :class:`~py_trees.behaviour.Behaviour`: subtree root
        """
        # beahviors
        root = py_trees.composites.Selector(name="Scan or Die")
        die = py_trees_ros.tutorials.behaviours.FlashLedStrip(
            name="Uh Oh",
            colour="red")
        ere_we_go = py_trees.composites.Sequence(name="Ere we Go")
        undock = py_trees_ros.actions.ActionClient(
            name="UnDock",
            action_namespace="/dock",
            action_spec=py_trees_msgs.DockAction,
            action_goal=py_trees_msgs.DockGoal(False),
            override_feedback_message_on_running="undocking"
        )
        scan_or_be_cancelled = py_trees.composites.Selector("Scan or Be Cancelled")
        cancelling = py_trees.composites.Sequence("Cancelling?")
        is_cancel_requested = py_trees.blackboard.CheckBlackboardVariable(
            name="Cancel?",
            variable_name='event_cancel_button',
            expected_value=True
        )
        move_home_after_cancel = py_trees_ros.actions.ActionClient(
            name="Move Home",
            action_namespace="/move_base",
            action_spec=move_base_msgs.MoveBaseAction,
            action_goal=move_base_msgs.MoveBaseGoal()
        )
        move_out_and_scan = py_trees.composites.Sequence("Move Out and Scan")
        move_base = py_trees_ros.actions.ActionClient(
            name="Move Out",
            action_namespace="/move_base",
            action_spec=move_base_msgs.MoveBaseAction,
            action_goal=move_base_msgs.MoveBaseGoal()
        )
        scanning = py_trees.composites.Parallel(name="Scanning", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        scan_context_switch = py_trees_ros.tutorials.behaviours.ScanContext("Context Switch")
        rospy.logwarn("Setting up rotating")
        scan_rotate = py_trees_ros.actions.ActionClient(
            name="Rotate",
            action_namespace="/rotate",
            action_spec=py_trees_msgs.RotateAction,
            action_goal=py_trees_msgs.RotateGoal(),
            override_feedback_message_on_running="rotating"
        )
        scan_flash_blue = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Blue", colour="blue")
        move_home_after_scan = py_trees_ros.actions.ActionClient(
            name="Move Home",
            action_namespace="/move_base",
            action_spec=move_base_msgs.MoveBaseAction,
            action_goal=move_base_msgs.MoveBaseGoal()
        )
        celebrate = py_trees.composites.Parallel(name="Celebrate", policy=py_trees.common.ParallelPolicy.SUCCESS_ON_ONE)
        celebrate_flash_green = py_trees_ros.tutorials.behaviours.FlashLedStrip(name="Flash Green", colour="green")
        celebrate_pause = py_trees.timers.Timer("Pause", duration=3.0)
        dock = py_trees_ros.actions.ActionClient(
            name="Dock",
            action_namespace="/dock",
            action_spec=py_trees_msgs.DockAction,

Job Loading

Job handlers are loaded via a string at runtime. This ensures decoupling of the the tree implementation from the job providers. The SplinteredReality class here is responsible for setting up and tick tocking the tree.

py_trees_ros/tutorials/eight.py#job_loading

class SplinteredReality(object):

    def __init__(self, jobs):
        """
        Initialise a core tree (minus a job) and preload job classes ready to
        be used in spinning up and running the job later when requested.

        Args:
            jobs ([:obj:`str`]): list of module names as strings (e.g. 'py_trees_ros.tutorials.jobs.Scan')
        """
        self.tree = py_trees_ros.trees.BehaviourTree(create_root())
        self.tree.add_pre_tick_handler(self.pre_tick_handler)
        self.tree.add_post_tick_handler(self.post_tick_handler)
        self.report_publisher = rospy.Publisher("~report", std_msgs.String, queue_size=5)
        self.jobs = []
        for job in jobs:
            module_name = '.'.join(job.split('.')[:-1])
            class_name = job.split('.')[-1]
            self.jobs.append(getattr(importlib.import_module(module_name), class_name)())
        self.current_job = None

Just in Time

Job subtrees are inserted and pruned via the tree pre and post tick handlers.

Note

This is done in the free window between ticks, but the checking/insertion/deletion logic could alternatively have been achieved from behaviours inside the tree.

py_trees_ros/tutorials/eight.py#just_in_time

    def pre_tick_handler(self, tree):
        """
        Check if a job is running. If not, spin up a new job subtree
        if a request has come in.

        Args:
            tree (:class:`~py_trees.trees.BehaviourTree`): tree to investigate/manipulate.
        """
        if not self.busy():
            for job in self.jobs:
                if job.goal:
                    job_root = job.create_root(job.goal)
                    if not job_root.setup(timeout=15):
                        rospy.logerr("{0}: failed to setup".format(job.name))
                        continue
                    tree.insert_subtree(job_root, self.priorities.id, 1)
                    rospy.loginfo("{0}: inserted job subtree".format(job_root.name))
                    job.goal = None
                    self.current_job = job

    def post_tick_handler(self, tree):
        """
        Check if a job is running and if it has finished. If so, prune the job subtree from the tree.
        Additionally, make a status report upon introspection of the tree.

        Args:
            tree (:class:`~py_trees.trees.BehaviourTree`): tree to investigate/manipulate.
        """
        # delete the job subtree if it is finished
        if self.busy():
            job = self.priorities.children[-2]
            if job.status == py_trees.common.Status.SUCCESS or job.status == py_trees.common.Status.FAILURE:
                rospy.loginfo("{0}: finished [{1}]".format(job.name, job.status))
                tree.prune_subtree(job.id)
                self.current_job = None
        # publish a status report
        if self.busy():
            job = self.priorities.children[-2]
            self.report_publisher.publish(self.current_job.create_report_string(job))
        elif tree.tip().has_parent_with_name("Battery Emergency"):
            self.report_publisher.publish("battery")
        else:
            self.report_publisher.publish("idle")

Running

$ roslaunch py_trees_ros tutorial_eight.launch --screen

Tutorial 9 - Bags

About

Tutorial nine is a repeat of Tutorial 8 - Dynamic Job Handling with some pointers illustrating how the bagging functionality of the BehaviourTree.

Running

$ roslaunch py_trees_ros tutorial_nine.launch --screen

Bagging starts as soon as a BehaviourTree is instantiated and will record something only when the tree changes, i.e.

• behaviours are inserted or pruned from the tree
• a behaviour changes state
• the feedback message in a behaviour changes

Messages recorded are exactly the same type as those used to feed the visual monitoring tool - rqt_py_trees. Trigger a scan and finally close down the launcher. The bags will be saved in:

${ROS_HOME}/behaviour_trees/<date>/behaviour_tree_<date>.bag

Playback

Restart the visual monitoring tool in playback mode on the latest saved bag:

# in a first shell
roscore
# in a second shell
rqt_py_trees --latest-bag

Move around the replay via the timeline widget at the bottom of the plugin window.

Other Ideas

If you are executing just in time as in Tutorial 8 - Dynamic Job Handling, then an interesting alternative is to use BehaviourTree to inspire your own behaviour tree manager that creates a bag that records for the duration of the job execution only. That is, start as the job subtree is inserted and close the bag as the job subtree is pruned. This provides small, modular bags for convenient storage and lookup.