SMACH Use Case

This use case will explore the usability and learning curve of SMACH. This use case starts with a simple use of just the SMACH API, and finishes with a concrete example that interfaces with other ROS systems: an executive that will coordinate two turtles in turtlesim.

This use case assumes a basic understanding of python, ROS, rospy, and actionlib.

Create The Package

First, create the pacakge with a number of dependencies:

$ roscreate-pkg smach_usecase rospy std_srvs smach turtlesim
$ roscd smach_usecase

Create a launchfile to launch a turtlesim environment. Call it "turtle_nodes.launch":

   1 <launch>
   2   <node pkg="turtlesim" name="turtlesim" type="turtlesim_node"/>
   3 </launch>

You should now be able to run the following to launch a turtlesim environment:

$ roslaunch smach_usecase turtle_nodes.launch

Create The Executive

Create the directory "scripts" in the "smach_usecase" package. In this directory, create a python script called "executive.py". This script will be the primary focus of the rest of the use case.

In this script, create the backbone of the SMACH executive. This involves, the necessary imports, the definition of a main() function, and creation of an empty SMACH state machine.

   1 #!/usr/bin/env python
   2 
   3 import roslib; roslib.load_manifest('smach_usecase')
   4 
   5 import rospy
   6 import smach
   7 
   8 def main():
   9     rospy.init_node('smach_usecase_executive')
  10 
  11     sm_root = smach.StateMachine(outcomes=[])
  12 
  13     with sm_root:
  14         pass
  15 
  16     outcome = sm_root.execute()
  17 
  18     rospy.spin()
  19 
  20 if __name__ == '__main__':
  21     main()

Add Some Service Call States

Add two states to the (currently empty) state machine "sm_root." The first state should call the ROS service provided by turtlesim to reset the simulation. The second state should spawn a new turtle at (0,0) called "turtle2".

These two tasks are both executed with service calls and can be implemented with SMACH smach.ServiceStates. The ServiceState represents the execution of a ROS service call. They provide the potential outcomes "succeeded", "aborted", and "preempted".

• The first service is called "reset" and its service type is std_srvs.Empty.

• The second service is called "spawn" and its service type is turtlesim.srv.Spawn. Its request type, turtlesim.srv.SpawnRequest, takes four arguments (x, y, theta, name)

Once you have added the two states, you should be able to launch your turtlesim launchfile, and then run the executive. When running the executive, you should see turtlesim get reset and then a new turtle should appear in the corner at (0,0).

Attach SMACH Viewer For Introspection

It's possible to view the structure and state of a SMACH tree while it's executing. This is done by attaching a smach.IntrospectionServer to the root of a given SMACH tree. Then, smach_viewer can be ran to introspect on the tree.

The smach_viewer tutorial shows how this is done. Once you have added the introspection server, you can run SMACH Viewer like so:

$ rosrun smach_viewer smach_viewer.py

If you named your states "RESET" and "SPAWN" in the previous section, you should see a structure like this:

In the above image, states are represented by ovals, and their outcomes are represented by directed edges. The outcomes of the container "sm_root" are shown in red. It is a good convention to capitalize state names, to write outcomes in lower case, and to use underscores where multiple words are present in a name.

Add Interface To TurtleSim Shape Action

Next you will add some more tasks sequence of states. These tasks will involve calling some more services, as well as a couple of actionlib actions. Before adding the code that calls the actions, we need to add the action servers to our launchfile.

Adding the following lines to "turtle_nodes.launch" will bring up a pair of action servers that can draw polygons with the two turtles:

   1   <node pkg="turtle_actionlib" name="turtle_shape1" type="shape_server"/>
   2   <node pkg="turtle_actionlib" name="turtle_shape2" type="shape_server">
   3     <remap from="/turtle1/pose" to="/turtle2/pose"/>
   4     <remap from="/turtle1/cmd_vel" to="/turtle2/cmd_vel"/>
   5   </node>

Before adding the action states, add two more service states. The first should move "turtle1" to the coordinates (5.0,1.0), and the second should move "turtle2" to the coordinates (9.0,5.0). These services are called "turtle1/teleport_absolute" and "turtle2/teleport_absolute", respectively. They use the service of type turtlesim.srv.TeleportAbsolute, and the request type has three arguments (x, y, theta).

Next add two states that will send goals to the action servers "turtle_shape1" and "turtle_shape2", the two action servers that we added to the launchfile above. This can be done trivially with smach.SimpleActionStates.

The first state should draw a big undecagon (11 sides) of radius 4.0, with turtle1, and the second state should draw a small hexagon of radius 0.5 with turtle2. The action type is turtle_actionlib.msg.ShapeAction and the goal type has two arguments (edges, radius).

Once you have added the four states described above, you should be able to run the script and see the two turtles each draw a polygon.

If you run SMACH Viewer, and used the naming convention shown, you should see the following graph while turtle1 is drawing the big polygon.

Make Shapes Drawn in Parallel

In this next step, you will put the two shape-drawing states into a concurrence. This will send the two action goals simultaneously, and wait for them both to terminate.

Fist construct a concurrence, a tutorial for which is can be found here. Add the concurrence to the state machine, and then move the two states that send shape goals from the root state machine into the concurrence.

When executing the code, you should now see both turtles move simultaneously like so:

Additionally, if you followed the same naming conventions, SMACH Viewer should now show a structure similar to the following:

Note that this shows that the small polygon has finished being drawn, but the big one is still active.

Make The Second Turtle Stop When the First Gets Too Close

In this next step, you will make turtle2 stop when turtle1 gets too close. This involves implementing a monitor pattern. The idea behind this pattern is to allow a state to preempt (or stop) a sibling when some condition is no longer satisfied.

The SMACH ROS library has a monitor state which provides the mechanism for associating a condition callback with a topic and message type. The callback is called each time the state receives a message on the specified topic. If the callback returns True, then it continues to block, but if it returns False, it will terminate with the outcome 'invalid'. More on the monitor state can be found on the MonitorState Tutorial page.

Such a state can be placed in a concurrence with another state. Then the only thing necessary to do to make it kill the sibling when the condition no longer holds, is to give the concurrence a "child termination callback". This callback (if specified) is called each time a child in a SMACH Concurrence terminates, and determines whether or not the other states should be sent preempt signals.

When using a concurrence and a monitor state to implement this pattern, the child termination callback can simply be an anonymous function that always returns True:

   1     ...
   2     child_termination_cb = lambda state_outcomes: True,
   3     ...

This way, the first state that terminates will cause the other states in the concurrence to be preempted. If you follow the same naming conventions, after making this modification to the SMACH tree in the executive, the SMACH Viewer should now show a structure similar to the following:

Make The Second Turtle Start Over When the First Leaves

The final modification to the behavior of this executive, is to add some simple recovery behavior to the small polygon drawing. In the previous step, once turtle2 stopped drawing, it never started again. In this step, you will need to add another monitor which holds while the turtles are close.

In order to create the outcome loop-back, you will also need to put the monitor concurrence along with this new monitor into a state machine. See the graph in SMACH Viewer below to get a better idea of how this will look.

Wrap SMACH Executive Into An Action

• Remove execute call
• Add wrapping code
• Create a python script that calls the action
• Run