smach_ros.action_server

16 """SMACH container wrapper with actionlib ActionServer. 17 18 Use this class to associate an action server with a smach 19 L{StateMachine<smach.state_machine.StateMachine>}. This allows invocation 20 of the state machine over the actionlib API/protocol. 21 22 This class delegates to a provided SMACH container and associates it with an 23 action server. The user can specify lists of outcomes which correspond to 24 different action result statuses (SUCCEEDED, ABORTED, PREEMPTED). Once the 25 delegate state machine leaves one of these outcomes, this wrapper class will 26 cause the state machine to terminate, and cause the action server to return 27 a result. 28 29 Note that this class does not inherit from L{smach.State<smach.State>} and 30 can only be used as a top-level container. 31 """ 32

33 - def __init__(self, 34 server_name, action_spec, 35 wrapped_container, 36 succeeded_outcomes = [], 37 aborted_outcomes = [], 38 preempted_outcomes = [], 39 goal_key = 'action_goal', 40 feedback_key = 'action_feedback', 41 result_key = 'action_result', 42 goal_slots_map = {}, 43 feedback_slots_map = {}, 44 result_slots_map = {}, 45 expand_goal_slots = False, 46 pack_result_slots = False 47 ):

48 """Constructor. 49 50 @type server_name: string 51 @param server_name: The name of the action server that this container will 52 present. 53 54 @type action_spec: actionlib action msg 55 @param action_spec: The type of action this server will present 56 57 @type wrapped_container: L{StateMachine} 58 @param wrapped_container: The state machine to manipulate 59 60 @type succeeded_outcomes: array of strings 61 @param succeeded_outcomes: Array of terminal state labels which, when left, 62 should cause the action server to return SUCCEEDED as a result status. 63 64 @type aborted_outcomes: array of strings 65 @param aborted_outcomes: Array of terminal state labels which, when left, 66 should cause the action server to return ABORTED as a result status. 67 68 @type preempted_outcomes: array of strings 69 @param preempted_outcomes: Array of terminal state labels which, when left, 70 should cause the action server to return PREEMPTED as a result status. 71 72 @type goal_key: string 73 @param goal_key: The userdata key into which the action goal should be 74 stuffed when the action server receives one. 75 76 @type feedback_key: string 77 @param feedback_key: The userdata key into which the SMACH container 78 can put feedback information relevant to the action. 79 80 @type result_key: string 81 @param result_key: The userdata key into which the SMACH container 82 can put result information from this action. 83 """ 84 85 # Store state machine 86 self.wrapped_container = wrapped_container 87 """State machine that this wrapper talks to.""" 88 89 # Register state machine callbacks 90 self.wrapped_container.register_transition_cb(self.transition_cb) 91 self.wrapped_container.register_termination_cb(self.termination_cb) 92 93 # Grab reference to state machine user data (the user data in the children 94 # states scope) 95 self.userdata = smach.UserData() 96 97 # Store special userdata keys 98 self._goal_key = goal_key 99 self._feedback_key = feedback_key 100 self._result_key = result_key 101 102 self._goal_slots_map = goal_slots_map 103 self._feedback_slots_map = feedback_slots_map 104 self._result_slots_map = result_slots_map 105 106 self._expand_goal_slots = expand_goal_slots 107 self._pack_result_slots = pack_result_slots 108 109 # Store goal, result, and feedback types 110 self.userdata[self._goal_key] = copy.copy(action_spec().action_goal.goal) 111 self.userdata[self._result_key] = copy.copy(action_spec().action_result.result) 112 self.userdata[self._feedback_key] = copy.copy(action_spec().action_feedback.feedback) 113 114 # Action info 115 self._server_name = server_name 116 self._action_spec = action_spec 117 118 # Construct action server (don't start it until later) 119 self._action_server = SimpleActionServer( 120 self._server_name, 121 self._action_spec, 122 execute_cb = self.execute_cb, 123 auto_start = False) 124 125 # Store and check the terminal outcomes 126 self._succeeded_outcomes = set(succeeded_outcomes) 127 self._aborted_outcomes = set(aborted_outcomes) 128 self._preempted_outcomes = set(preempted_outcomes) 129 130 # Make sure the sets are disjoint 131 card_of_unions = len(self._succeeded_outcomes | self._aborted_outcomes | self._preempted_outcomes) 132 sum_of_cards = (len(self._succeeded_outcomes) + len(self._aborted_outcomes) + len(self._preempted_outcomes)) 133 if card_of_unions != sum_of_cards: 134 rospy.logerr("Succeeded, aborted, and preempted outcome lists were not mutually disjoint... expect undefined behavior.")

135

136 - def run_server(self):

137 """Run the state machine as an action server. 138 Note that this method does not block. 139 """ 140 141 # Register action server callbacks 142 #self._action_server.register_goal_callback(self.goal_cb) 143 self._action_server.register_preempt_callback(self.preempt_cb) 144 145 # Stat server (because we disabled auto-start to register the callbacks) 146 self._action_server.start() 147 148 rospy.loginfo("Started SMACH action server wrapper, adversiting as '%s'" % self._server_name)

149 150 ### State machine callbacks

151 - def transition_cb(self, userdata, active_states):

152 """Transition callback passed to state machine. 153 This method is called each time the state machine transitions. 154 """ 155 rospy.logdebug("Publishing action feedback.") 156 # Publish action feedback 157 self.publish_feedback(userdata)

158

159 - def termination_cb(self, userdata, terminal_states, container_outcome):

160 """Termination callback passed to state machine. 161 This callback receives the final state and state machine outcome as 162 specified by the state-outcome map given to the delegate container 163 on construction (see L{ActionServerWrapper.__init__}). 164 165 Remember that in this context, the SMACH container is just a single state 166 object, which has an outcome like any other state; it is this outcome on 167 which we switch here. This method will determine from the state machine 168 outcome which result should be returned to the action client for this goal. 169 """ 170 rospy.logdebug("Wrapped state machine has terminated with final state: "+str(terminal_states)+" and container outcome: "+str(container_outcome))

171

172 - def publish_feedback(self, userdata):

173 """Publish the feedback message in the userdata db. 174 Note that this feedback is independent of smach. 175 """ 176 self._action_server.publish_feedback(userdata[self._feedback_key])

177 178 ### Action server callbacks

179 - def execute_cb(self, goal):

180 """Action server goal callback 181 This method is called when the action server associated with this state 182 machine receives a goal. This puts the goal into the userdata, 183 which is the userdata of the contained state. 184 """ 185 186 # If the state machine is running, we should preempt it before executing it 187 # it again. 188 rospy.logdebug("Starting wrapped SMACH container") 189 190 # Accept goal 191 #goal = self._action_server.accept_new_goal() 192 193 # Expand the goal into the root userdata for this server 194 if self._expand_goal_slots: 195 for slot in goal.__slots__: 196 self.userdata[slot] = getattr(goal, slot) 197 198 # Store the goal in the container local userdate 199 self.userdata[self._goal_key] = goal 200 201 # Store mapped goal slots in local userdata 202 for from_key,to_key in self._goal_slots_map.iteritems(): 203 self.userdata[to_key] = getattr(goal,from_key) 204 205 # Run the state machine (this blocks) 206 try: 207 container_outcome = self.wrapped_container.execute( 208 smach.Remapper( 209 self.userdata, 210 self.wrapped_container.get_registered_input_keys(), 211 self.wrapped_container.get_registered_output_keys(), 212 {})) 213 214 except smach.InvalidUserCodeError as ex: 215 rospy.logerr("Exception thrown while executing wrapped container.") 216 self._action_server.set_aborted() 217 return 218 except: 219 rospy.logerr("Exception thrown:while executing wrapped container: " + traceback.format_exc()) 220 self._action_server.set_aborted() 221 return 222 223 # Grab the (potentially) populated result from the userdata 224 result = self.userdata[self._result_key] 225 226 # Store mapped slots in result 227 for from_key,to_key in self._result_slots_map.iteritems(): 228 setattr(result,from_key,self.userdata[to_key]) 229 230 # If any of the result members have been returned to the parent ud 231 # scope, overwrite the ones from the full structure 232 if self._pack_result_slots: 233 for slot in result.__slots__: 234 if slot in self.userdata: 235 setattr(result,slot,self.userdata[slot]) 236 237 # Set terminal state based on state machine state outcome 238 if container_outcome in self._succeeded_outcomes: 239 rospy.loginfo('SUCCEEDED') 240 self._action_server.set_succeeded(result) 241 elif container_outcome in self._preempted_outcomes: 242 rospy.loginfo('PREEMPTED') 243 self._action_server.set_preempted(result) 244 else: #if container_outcome in self._aborted_outcomes: 245 rospy.loginfo('ABORTED') 246 self._action_server.set_aborted(result)

247 248

249 - def preempt_cb(self):

250 """Action server preempt callback. 251 This method is called when the action client preempts an active goal. 252 253 In this case, the StateMachine needs to propagate said preemption to 254 the currently active delegate action (the current state). 255 """ 256 rospy.loginfo("Preempt on state machine requested!") 257 self.wrapped_container.request_preempt()

Source Code for Module smach_ros.action_server_wrapper