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.

Developing Actionlib States

Motivation

FlexBE states are the high-level building blocks from which behaviors are constructed and are supposed to interface with the capabilities of your robot system. actionlib provides such an interface where robot capabilities can be provided by ROS nodes implementing an action server and states can then access them by acting as an action client. The interface is designed for long-term (longer than one update cycle) actions as it is non-blocking and optionally provides feedback while being executed. This makes it a perfect interface to be used along with FlexBE and it is recommended to base your states on action interfaces whenever they do something more complex than just subscribing to a topic.

Action Client

This tutorial is based on the ExampleActionState which is one of the example states automatically created when you run the create_repo script.

First, make sure you import the action client proxy and the required message types of the action interface:

   1 from flexbe_core.proxy import ProxyActionClient
   2 # example import of required action
   3 from chores.msg import DoDishesAction, DoDishesGoal

Declaration

It is recommended to create the action client in the constructor of your state as this will check the availability of the action server before starting the behavior and thus, reduce the risk for runtime failure.

In order to declare the required action client, add the following code to the constructor:

   1 self._topic = 'do_dishes'
   2 self._client = ProxyActionClient({self._topic: DoDishesAction})

Sending a Goal

Typically, a state sends its goal once when it becomes active in order to trigger a certain action. Thus, create and send the action goal in the on_enter callback of your state. You can access userdata input keys here as required.

   1 goal = DoDishesGoal()
   2 goal.dishwasher_id = userdata.dishwasher
   3 
   4 self._error = False
   5 try:
   6         self._client.send_goal(self._topic, goal)
   7 except Exception as e:
   8         Logger.logwarn('Failed to send the DoDishes command:\n%s' % str(e))
   9         self._error = True

For robustness, it is recommended to embed the action call in a try/catch block in case there are any problems during runtime. The variable self._error can be used in the execute function to return a failure outcome if any problems occured:

   1 # Check if the client failed to send the goal.
   2 if self._error:
   3         return 'command_error'

Checking for Result

Finally, in the execution loop, you can check if the action has already finished and evaluate its result. You can also store relevant parts of its result in the userdata.

   1 if self._client.has_result(self._topic):
   2         result = self._client.get_result(self._topic)
   3         dishes_cleaned = result.total_dishes_cleaned
   4 
   5         userdata.cleaned = dishes_cleaned
   6 
   7         if dishes_cleaned > self._dishes_to_do:
   8                 return 'cleaned_enough'
   9         else:
  10                 return 'cleaned_some'

You can also access the result status of the action call if there is no notation of success provided by the result message itself:

   1 if self._client.has_result(self._topic):
   2         status = self._client.get_state(self._topic)
   3         if status == GoalStatus.SUCCEEDED:
   4                 return 'success'
   5         elif status in [GoalStatus.PREEMPTED, GoalStatus.REJECTED, GoalStatus.RECALLED, GoalStatus.ABORTED]:
   6                 Logger.logwarn('Action failed: %s' % str(status))
   7                 return 'failed'

Note that you need to import the GoalStatus message provided by actionlib for this:

from actionlib_msgs.msg import GoalStatus