actionlib2.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('asmach_tutorials')
00004 import rospy
00005 import asmach as smach
00006 import smach_ros
00007 
00008 from smach_tutorials.msg import TestAction, TestGoal
00009 from actionlib import *
00010 from actionlib.msg import *
00011 
00012 
00013 # Create a trivial action server
00014 class TestServer:
00015     def __init__(self,name):
00016         self._sas = SimpleActionServer(name,
00017                 TestAction,
00018                 execute_cb=self.execute_cb)
00019 
00020     def execute_cb(self, msg):
00021         if msg.goal == 0:
00022             self._sas.set_succeeded()
00023         elif msg.goal == 1:
00024             self._sas.set_aborted()
00025         elif msg.goal == 2:
00026             self._sas.set_preempted()
00027 
00028 def main():
00029     rospy.init_node('smach_example_actionlib')
00030 
00031     # Start an action server
00032     server = TestServer('test_action')
00033 
00034     # Create a SMACH state machine
00035     sm0 = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
00036 
00037     # Open the container
00038     with sm0:
00039         # Add states to the container
00040 
00041         # Add a simple action sttate. This will use an emtpy, default goal
00042         # As seen in TestServer above, an empty goal will always return with
00043         # GoalStatus.SUCCEEDED, causing this simple action state to return
00044         # the outcome 'succeeded'
00045         smach.StateMachine.add('GOAL_DEFAULT',
00046                                smach_ros.SimpleActionState('test_action', TestAction),
00047                                {'succeeded':'GOAL_STATIC'})
00048 
00049         # Add another simple action state. This will give a goal
00050         # that should abort the action state when it is received, so we
00051         # map 'aborted' for this state onto 'succeeded' for the state machine.
00052         smach.StateMachine.add('GOAL_STATIC',
00053                                smach_ros.SimpleActionState('test_action', TestAction,
00054                                                        goal = TestGoal(goal=1)),
00055                                {'aborted':'GOAL_CB'})
00056 
00057         
00058         # Add another simple action state. This will give a goal
00059         # that should abort the action state when it is received, so we
00060         # map 'aborted' for this state onto 'succeeded' for the state machine.
00061         def goal_callback(userdata, default_goal):
00062             goal = TestGoal()
00063             goal.goal = 2
00064             return goal
00065 
00066         smach.StateMachine.add('GOAL_CB',
00067                                smach_ros.SimpleActionState('test_action', TestAction,
00068                                                        goal_cb = goal_callback),
00069                                {'aborted':'succeeded'})
00070 
00071         # For more examples on how to set goals and process results, see 
00072         # executive_smach/smach_ros/tests/smach_actionlib.py
00073 
00074     # Execute SMACH plan
00075     outcome = sm0.execute()
00076 
00077     rospy.signal_shutdown('All done.')
00078 
00079 
00080 if __name__ == '__main__':
00081     main()


asmach_tutorials
Author(s): Jonathan Bohren
autogenerated on Wed Sep 16 2015 04:38:38