Move smach_tutorials (from https://github.com/eacousineau/executive_smach_tutorials.git). Catkinized.

smach_tutorials/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(smach_tutorials)
+
+find_package(catkin REQUIRED COMPONENTS actionlib_msgs message_generation)
+
+add_action_files(
+  DIRECTORY action
+  FILES Test.action)
+generate_messages(DEPENDENCIES actionlib_msgs)
+
+catkin_package(CATKIN_DEPENDS actionlib_msgs message_runtime)
+
+install(
+  DIRECTORY examples scripts
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS
+  FILES_MATCHING PATTERN "*.py"
+)

smach_tutorials/action/Test.action

@@ -0,0 +1,3 @@
+float64 goal
+---
+---

smach_tutorials/examples/actionlib2_test.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python
+
+import rospy
+import smach
+import smach_ros
+
+from smach_tutorials.msg import TestAction, TestGoal
+from actionlib import *
+from actionlib_msgs.msg import *
+
+
+# Create a trivial action server
+class TestServer:
+    def __init__(self,name):
+        self._sas = SimpleActionServer(name,
+                TestAction,
+                execute_cb=self.execute_cb)
+
+    def execute_cb(self, msg):
+        if msg.goal == 0:
+            self._sas.set_succeeded()
+        elif msg.goal == 1:
+            self._sas.set_aborted()
+        elif msg.goal == 2:
+            self._sas.set_preempted()
+
+def main():
+    rospy.init_node('smach_example_actionlib')
+
+    # Start an action server
+    server = TestServer('test_action')
+
+    # Create a SMACH state machine
+    sm0 = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
+
+    # Open the container
+    with sm0:
+        # Add states to the container
+
+        # Add a simple action state. This will use an empty, default goal
+        # As seen in TestServer above, an empty goal will always return with
+        # GoalStatus.SUCCEEDED, causing this simple action state to return
+        # the outcome 'succeeded'
+        smach.StateMachine.add('GOAL_DEFAULT',
+                               smach_ros.SimpleActionState('test_action', TestAction),
+                               {'succeeded':'GOAL_STATIC'})
+
+        # Add another simple action state. This will give a goal
+        # that should abort the action state when it is received, so we
+        # map 'aborted' for this state onto 'succeeded' for the state machine.
+        smach.StateMachine.add('GOAL_STATIC',
+                               smach_ros.SimpleActionState('test_action', TestAction,
+                                                       goal = TestGoal(goal=1)),
+                               {'aborted':'GOAL_CB'})
+
+
+        # Add another simple action state. This will give a goal
+        # that should abort the action state when it is received, so we
+        # map 'aborted' for this state onto 'succeeded' for the state machine.
+        def goal_callback(userdata, default_goal):
+            goal = TestGoal()
+            goal.goal = 2
+            return goal
+
+        smach.StateMachine.add('GOAL_CB',
+                               smach_ros.SimpleActionState('test_action', TestAction,
+                                                       goal_cb = goal_callback),
+                               {'aborted':'succeeded'})
+
+        # For more examples on how to set goals and process results, see 
+        # executive_smach/smach_ros/tests/smach_actionlib.py
+
+    # Execute SMACH plan
+    outcome = sm0.execute()
+
+    rospy.signal_shutdown('All done.')
+
+
+if __name__ == '__main__':
+    main()

smach_tutorials/examples/actionlib_test.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python
+"""
+Description:
+    Spawn an actionlib action server, then create a state machine that sends
+    some goals to the action server that will automatically succeed or abort.
+    We expect the first goal to succeed, and the second goal to abort, so
+    when the second goal aborts, we map that onto success of the state
+    machine.
+
+Usage:
+    $> ./actionlib.py
+
+Output:
+    [INFO] : State machine starting in initial state 'GOAL_DEFAULT' with userdata: 
+        []
+    [WARN] : Still waiting for action server 'test_action' to start... is it running?
+    [INFO] : State machine transitioning 'GOAL_DEFAULT':'succeeded'-->'GOAL_STATIC'
+    [INFO] : State machine terminating 'GOAL_STATIC':'aborted':'succeeded'
+"""
+
+import rospy
+import smach
+import smach_ros
+
+from smach_tutorials.msg import TestAction, TestGoal
+from actionlib import *
+from actionlib_msgs.msg import *
+
+# Create a trivial action server
+class TestServer:
+    def __init__(self,name):
+        self._sas = SimpleActionServer(name,
+                TestAction,
+                execute_cb=self.execute_cb)
+
+    def execute_cb(self, msg):
+        if msg.goal == 0:
+            self._sas.set_succeeded()
+        elif msg.goal == 1:
+            self._sas.set_aborted()
+        elif msg.goal == 2:
+            self._sas.set_preempted()
+
+def main():
+    rospy.init_node('smach_example_actionlib')
+
+    # Start an action server
+    server = TestServer('test_action')
+
+    # Create a SMACH state machine
+    sm0 = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
+
+    # Open the container
+    with sm0:
+        # Add states to the container
+
+        # Add a simple action sttate. This will use an emtpy, default goal
+        # As seen in TestServer above, an empty goal will always return with
+        # GoalStatus.SUCCEEDED, causing this simple action state to return
+        # the outcome 'succeeded'
+        smach.StateMachine.add('GOAL_DEFAULT',
+                smach_ros.SimpleActionState('test_action', TestAction),
+                {'succeeded':'GOAL_STATIC'})
+
+        # Add another simple action state. This will give a goal
+        # that should abort the action state when it is received, so we
+        # map 'aborted' for this state onto 'succeeded' for the state machine.
+        smach.StateMachine.add('GOAL_STATIC',
+                smach_ros.SimpleActionState('test_action', TestAction,
+                    goal = TestGoal(goal=1)),
+                {'aborted':'succeeded'})
+
+        # For more examples on how to set goals and process results, see 
+        # executive_python/smach/tests/smach_actionlib.py
+
+    # Execute SMACH plan
+    outcome = sm0.execute()
+
+    rospy.signal_shutdown('All done.')
+
+if __name__ == '__main__':
+    main()

smach_tutorials/examples/concurrence.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+"""
+Description:
+    This creates a concurrence with two states, one state, 'SET', waits three
+    seconds before setting something in userdata, while another state 'GET'
+    blocks while checking for thie userdata key, and only returns once it
+    has been set.
+
+Usage:
+    $> ./concurrence.py
+
+Output:
+    [INFO] 1279226335.169182: Concurrence starting with userdata: 
+            []
+    [INFO] : >>> Waiting for data...
+    [INFO] : >>> Waiting for data...
+    [INFO] : >>> Waiting for data...
+    [INFO] : >>> Waiting for data...
+    [INFO] : >>> Waiting for data...
+    [INFO] : >>> Waiting for data...
+    [INFO] : >>> Set data: hello
+    [INFO] : >>> GOT DATA! x = hello
+    [INFO] : Concurrent Outcomes: {'SET': 'set_it', 'GET': 'got_it'}
+"""
+
+import rospy
+import smach
+import smach_ros
+
+# Define a state to set some user data
+class Setter(smach.State):
+    def __init__(self, val):
+        smach.State.__init__(self, outcomes = ['set_it'], output_keys = ['x'])
+        self._val = val
+    def execute(self, ud):
+        # Delay a bit to make this clear
+        rospy.sleep(3.0)
+        # Set the data
+        ud.x = self._val
+        rospy.loginfo('>>> Set data: %s' % str(self._val))
+        return 'set_it'
+
+# Define a state to get some user data
+class Getter(smach.State):
+    def __init__(self):
+        smach.State.__init__(self, outcomes = ['got_it'], input_keys = ['x'])
+    def execute(self, ud):
+        # Wait for data to appear
+        while 'x' not in ud:
+            rospy.loginfo('>>> Waiting for data...')
+            rospy.sleep(0.5)
+        rospy.loginfo('>>> GOT DATA! x = '+str(ud.x))
+        return 'got_it'
+
+def main():
+    rospy.init_node('smach_example_concurrence')
+
+    # Create a SMACH state machine
+    cc0 = smach.Concurrence(
+            outcomes=['succeeded', 'aborted'],
+            default_outcome='aborted',
+            outcome_map = {'succeeded':{'SET':'set_it','GET':'got_it'}})
+
+    # Open the container
+    with cc0:
+        # Add states to the container
+        smach.Concurrence.add('SET', Setter(val='hello'))
+        smach.Concurrence.add('GET', Getter())
+
+    # Execute SMACH plan
+    outcome = cc0.execute()
+
+if __name__ == '__main__':
+    main()

smach_tutorials/examples/concurrence2.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+
+import rospy
+import smach
+import smach_ros
+
+# define state Foo
+class Foo(smach.State):
+    def __init__(self):
+        smach.State.__init__(self, outcomes=['outcome1','outcome2'])
+        self.counter = 0
+
+    def execute(self, userdata):
+        rospy.loginfo('Executing state FOO')
+        if self.counter < 3:
+            self.counter += 1
+            return 'outcome1'
+        else:
+            return 'outcome2'
+
+
+# define state Bar
+class Bar(smach.State):
+    def __init__(self):
+        smach.State.__init__(self, outcomes=['outcome1'])
+
+    def execute(self, userdata):
+        rospy.loginfo('Executing state BAR')
+        return 'outcome1'
+
+
+
+# define state Bas
+class Bas(smach.State):
+    def __init__(self):
+        smach.State.__init__(self, outcomes=['outcome3'])
+
+    def execute(self, userdata):
+        rospy.loginfo('Executing state BAS')
+        return 'outcome3'
+
+
+
+
+def main():
+    rospy.init_node('smach_example_state_machine')
+
+    # Create the top level SMACH state machine
+    sm_top = smach.StateMachine(outcomes=['outcome6'])
+
+    # Open the container
+    with sm_top:
+
+        smach.StateMachine.add('BAS', Bas(),
+                               transitions={'outcome3':'CON'})
+
+        # Create the sub SMACH state machine
+        sm_con = smach.Concurrence(outcomes=['outcome4','outcome5'],
+                                   default_outcome='outcome4',
+                                   outcome_map={'outcome5':
+                                       { 'FOO':'outcome2',
+                                         'BAR':'outcome1'}})
+
+        # Open the container
+        with sm_con:
+            # Add states to the container
+            smach.Concurrence.add('FOO', Foo())
+            smach.Concurrence.add('BAR', Bar())
+
+        smach.StateMachine.add('CON', sm_con,
+                               transitions={'outcome4':'CON',
+                                            'outcome5':'outcome6'})
+
+    # Execute SMACH plan
+    outcome = sm_top.execute()
+
+
+if __name__ == '__main__':
+    main()