class ActionServerWrapper(): """SMACH container wrapper with actionlib ActionServer. Use this class to associate an action server with a smach L{StateMachine<smach.state_machine.StateMachine>}. This allows invocation of the state machine over the actionlib API/protocol. This class delegates to a provided SMACH container and associates it with an action server. The user can specify lists of outcomes which correspond to different action result statuses (SUCCEEDED, ABORTED, PREEMPTED). Once the delegate state machine leaves one of these outcomes, this wrapper class will cause the state machine to terminate, and cause the action server to return a result. Note that this class does not inherit from L{smach.State<smach.State>} and can only be used as a top-level container. """ def __init__(self, server_name, action_spec, wrapped_container, succeeded_outcomes = [], aborted_outcomes = [], preempted_outcomes = [], goal_key = 'action_goal', feedback_key = 'action_feedback', result_key = 'action_result', goal_slots_map = {}, feedback_slots_map = {}, result_slots_map = {}, expand_goal_slots = False, pack_result_slots = False ): """Constructor. @type server_name: string @param server_name: The name of the action server that this container will present. @type action_spec: actionlib action msg @param action_spec: The type of action this server will present @type wrapped_container: L{StateMachine} @param wrapped_container: The state machine to manipulate @type succeeded_outcomes: array of strings @param succeeded_outcomes: Array of terminal state labels which, when left, should cause the action server to return SUCCEEDED as a result status. @type aborted_outcomes: array of strings @param aborted_outcomes: Array of terminal state labels which, when left, should cause the action server to return ABORTED as a result status. @type preempted_outcomes: array of strings @param preempted_outcomes: Array of terminal state labels which, when left, should cause the action server to return PREEMPTED as a result status. @type goal_key: string @param goal_key: The userdata key into which the action goal should be stuffed when the action server receives one. @type feedback_key: string @param feedback_key: The userdata key into which the SMACH container can put feedback information relevant to the action. @type result_key: string @param result_key: The userdata key into which the SMACH container can put result information from this action. """ # Store state machine self.wrapped_container = wrapped_container """State machine that this wrapper talks to.""" # Register state machine callbacks self.wrapped_container.register_transition_cb(self.transition_cb) self.wrapped_container.register_termination_cb(self.termination_cb) # Grab reference to state machine user data (the user data in the children # states scope) self.userdata = smach.UserData() # Store special userdata keys self._goal_key = goal_key self._feedback_key = feedback_key self._result_key = result_key self._goal_slots_map = goal_slots_map self._feedback_slots_map = feedback_slots_map self._result_slots_map = result_slots_map self._expand_goal_slots = expand_goal_slots self._pack_result_slots = pack_result_slots # Store goal, result, and feedback types self.userdata[self._goal_key] = copy.copy(action_spec().action_goal.goal) self.userdata[self._result_key] = copy.copy(action_spec().action_result.result) self.userdata[self._feedback_key] = copy.copy(action_spec().action_feedback.feedback) # Action info self._server_name = server_name self._action_spec = action_spec # Construct action server (don't start it until later) self._action_server = SimpleActionServer( self._server_name, self._action_spec, execute_cb=self.execute_cb, auto_start=False) # Store and check the terminal outcomes self._succeeded_outcomes = set(succeeded_outcomes) self._aborted_outcomes = set(aborted_outcomes) self._preempted_outcomes = set(preempted_outcomes) # Make sure the sets are disjoint card_of_unions = len(self._succeeded_outcomes | self._aborted_outcomes | self._preempted_outcomes) sum_of_cards = (len(self._succeeded_outcomes) + len(self._aborted_outcomes) + len(self._preempted_outcomes)) if card_of_unions != sum_of_cards: rospy.logerr("Succeeded, aborted, and preempted outcome lists were not mutually disjoint... expect undefined behavior.") def run_server(self): """Run the state machine as an action server. Note that this method does not block. """ # Register action server callbacks #self._action_server.register_goal_callback(self.goal_cb) self._action_server.register_preempt_callback(self.preempt_cb) # Stat server (because we disabled auto-start to register the callbacks) self._action_server.start() rospy.loginfo("Started SMACH action server wrapper, adversiting as '%s'" % self._server_name) ### State machine callbacks def transition_cb(self, userdata, active_states): """Transition callback passed to state machine. This method is called each time the state machine transitions. """ rospy.logdebug("Publishing action feedback.") # Publish action feedback self.publish_feedback(userdata) def termination_cb(self, userdata, terminal_states, container_outcome): """Termination callback passed to state machine. This callback receives the final state and state machine outcome as specified by the state-outcome map given to the delegate container on construction (see L{ActionServerWrapper.__init__}). Remember that in this context, the SMACH container is just a single state object, which has an outcome like any other state; it is this outcome on which we switch here. This method will determine from the state machine outcome which result should be returned to the action client for this goal. """ rospy.logdebug("Wrapped state machine has terminated with final state: "+str(terminal_states)+" and container outcome: "+str(container_outcome)) def publish_feedback(self, userdata): """Publish the feedback message in the userdata db. Note that this feedback is independent of smach. """ if self._feedback_key in userdata: # This was spewing errors after the fix to ticket #5033 was submitted # in the case when _feedback_key is not set # For now, the fix is just checking if it exists, and not publishing otherwise # The spewage used to not happen because we were looking in self.userdata # and the constructor of this class sets the feedback key there to an empty struct # TODO figure out what the hell is going on here. self._action_server.publish_feedback(userdata[self._feedback_key]) ### Action server callbacks def execute_cb(self, goal): """Action server goal callback This method is called when the action server associated with this state machine receives a goal. This puts the goal into the userdata, which is the userdata of the contained state. """ # If the state machine is running, we should preempt it before executing it # it again. rospy.logdebug("Starting wrapped SMACH container") # Accept goal #goal = self._action_server.accept_new_goal() # Expand the goal into the root userdata for this server if self._expand_goal_slots: for slot in goal.__slots__: self.userdata[slot] = getattr(goal, slot) # Store the goal in the container local userdate self.userdata[self._goal_key] = goal # Store mapped goal slots in local userdata for from_key,to_key in ((k,self._goal_slots_map[k]) for k in self._goal_slots_map): self.userdata[to_key] = getattr(goal,from_key) # Run the state machine (this blocks) try: container_outcome = self.wrapped_container.execute( smach.Remapper( self.userdata, self.wrapped_container.get_registered_input_keys(), self.wrapped_container.get_registered_output_keys(), {})) except smach.InvalidUserCodeError as ex: rospy.logerr("Exception thrown while executing wrapped container.") self._action_server.set_aborted() return except: rospy.logerr("Exception thrown:while executing wrapped container: " + traceback.format_exc()) self._action_server.set_aborted() return # Grab the (potentially) populated result from the userdata result = self.userdata[self._result_key] # Store mapped slots in result for from_key, to_key in ((k,self._result_slots_map[k]) for k in self._result_slots_map): setattr(result, from_key, self.userdata[to_key]) # If any of the result members have been returned to the parent ud # scope, overwrite the ones from the full structure if self._pack_result_slots: for slot in result.__slots__: if slot in self.userdata: setattr(result, slot, self.userdata[slot]) # Set terminal state based on state machine state outcome if container_outcome in self._succeeded_outcomes: rospy.loginfo('SUCCEEDED') self._action_server.set_succeeded(result) elif container_outcome in self._preempted_outcomes: rospy.loginfo('PREEMPTED') self._action_server.set_preempted(result) else: #if container_outcome in self._aborted_outcomes: rospy.loginfo('ABORTED') self._action_server.set_aborted(result) def preempt_cb(self): """Action server preempt callback. This method is called when the action client preempts an active goal. In this case, the StateMachine needs to propagate said preemption to the currently active delegate action (the current state). """ rospy.loginfo("Preempt on state machine requested!") self.wrapped_container.request_preempt()
class MotionControllerNode: """ This is a port of the AMR C++ MotionControllerNode """ CONTROLLER_TYPE_DIFF = 'diff' CONTROLLER_TYPE_OMNI = 'omni' CONTROLLER_TYPE_UNSPECIFIED = 'unsp' def __init__(self): rospy.init_node(NODE) """ Parameters """ max_linear_velocity = rospy.get_param('max_linear_velocity', 0.3) max_linear_acceleration = rospy.get_param('max_linear_acceleration', 0.05) linear_tolerance = rospy.get_param('linear_tolerance', 0.02) max_angular_velocity = rospy.get_param('max_angular_velocity', 0.2) max_angular_acceleration = rospy.get_param('max_angular_acceleration', 0.03) angular_tolerance = rospy.get_param('angular_tolerance', 0.02) abort_if_obstacle_detected = rospy.get_param('abort_if_obstacle_detected', True) self._controller_frequency = rospy.get_param('controller_frequency', 10.0) controller_type = rospy.get_param('~controller', self.CONTROLLER_TYPE_UNSPECIFIED) if controller_type == self.CONTROLLER_TYPE_DIFF: #Create diff controller self._velocity_controller = DiffVelocityController(max_linear_velocity, linear_tolerance, max_angular_velocity, angular_tolerance) elif controller_type == self.CONTROLLER_TYPE_OMNI: #Create omni controller """ ========================= YOUR CODE HERE ========================= Instructions: create an instance of OmniVelocityController. Hint: you may copy-paste from the DiffVelocityController case and adjust the arguments in the call to the constructor to conform to what you have implemented in that class. """ self._velocity_controller = OmniVelocityController(max_linear_velocity, max_linear_acceleration, linear_tolerance, max_angular_velocity, max_angular_acceleration, angular_tolerance, self._controller_frequency) elif controller_type == self.CONTROLLER_TYPE_UNSPECIFIED: rospy.logerr('Controller type not specified. ' 'Check the [controller] launch parameter') exit() else: #Unknown controller rospy.logerr('Requested controller type "{0}" unknown. ' 'Check the [controller] launch parameter'.format(controller_type)) exit() """ Publishers """ self._velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) self._current_goal_publisher = rospy.Publisher(NODE+'/current_goal', PoseStamped, latch=True, queue_size=0) self._action_goal_publisher = rospy.Publisher(NODE+'/move_to/goal', MoveToActionGoal, queue_size=1) """ Subscribers """ self._simple_goal_subscriber = rospy.Subscriber(NODE+'/move_to_simple/goal', PoseStamped, self._simple_goal_callback, queue_size=1) if abort_if_obstacle_detected: self._obstacles_subscriber = rospy.Subscriber('obstacles', Obstacle, self._obstacles_callback, queue_size=100) """ Action server """ self._move_to_server = SimpleActionServer(NODE+'/move_to', MoveToAction, self._move_to_callback, auto_start=False) self._move_to_server.start() self._tf= tf.TransformListener() rospy.loginfo('Started [motion_controller] node.') def _move_to_callback(self, move_to_goal): """ Triggered with a request to move_to action server """ rospy.loginfo('Received [move_to] action command.') if not self._set_new_goal(move_to_goal): return else: rate = rospy.Rate(self._controller_frequency) while not rospy.is_shutdown(): if not self._move_to_server.is_active(): # Exit if the goal was aborted return #are there any preempted requests? if self._move_to_server.is_preempt_requested(): rospy.loginfo('found a preempted request, ') if ( self._move_to_server.is_new_goal_available() and self._set_new_goal(self._move_to_server.accept_new_goal()) ): # New goal already set pass else: # No new goals, preempt explicitly and exit the callback self._publish_zero_velocity() self._move_to_server.set_preempted() return if not self._move_towards_goal(): # Finish execution if the goal was reached self._move_to_server.set_succeeded(MoveToResult(), 'Goal reached.') self._publish_zero_velocity() return rate.sleep() self._move_to_server.set_aborted(MoveToResult(), 'Aborted. The node has been killed.') def _simple_goal_callback(self, target_pose): """ Wrapper for simple goal action. Forwards as a request to the move_to action server. Has to be tested! """ rospy.loginfo('Received target pose through the "simple goal" topic.' 'Wrapping it in the action message and forwarding to the server.') rospy.logwarn('Simple goal control is yet to be tested!') action_goal = MoveToActionGoal() action_goal.header.stamp = rospy.Time.now() action_goal.goal.target_pose = target_pose self._action_goal_publisher.publish(action_goal) def _obstacles_callback(self, obstacle_msg): rospy.logwarn('An obstacle was detected. Will stop the robot and cancel the current action.') if self._move_to_server.is_active(): self._move_to_server.set_aborted(MoveToResult(), 'Obstacle encountered, aborting...') self._publish_zero_velocity() def _move_towards_goal(self): try: time = self._tf.getLatestCommonTime("odom", "base_footprint") position, quaternion = self._tf.lookupTransform("odom", "base_footprint", time) except Exception as ex: rospy.logwarn('Transform lookup failed (\odom -> \base_footprint). ' 'Reason: {0}.'.format(ex.message)) return True current_pose = Pose2D() current_pose.x, current_pose.y = position[0], position[1] current_pose.theta = tf.transformations.euler_from_quaternion(quaternion)[2] velocity = self._velocity_controller.compute_velocity(current_pose) if self._velocity_controller.is_target_reached(): rospy.loginfo('The goal was reached') return False else: self._publish_velocity(velocity) return True def _set_new_goal(self, new_goal): """ Set new target pose as given in the goal message. Checks if the orientation provided in the target pose is valid. Publishes the goal pose for the visualization purposes. Returns true if the goal was accepted. """ if not self._is_quaternion_valid(new_goal.target_pose.pose.orientation): rospy.logwarn('Aborted. Target pose has invalid quaternion.') self._move_to_server.set_aborted(MoveToResult(),'Aborted. Target pose has invalid quaternion.') return False else: x = new_goal.target_pose.pose.position.x y = new_goal.target_pose.pose.position.y yaw = tf.transformations.euler_from_quaternion([new_goal.target_pose.pose.orientation.x, new_goal.target_pose.pose.orientation.y, new_goal.target_pose.pose.orientation.z, new_goal.target_pose.pose.orientation.w])[2] pose = Pose2D(x, y, yaw) self._velocity_controller.set_target_pose(pose) self._current_goal_publisher.publish(new_goal.target_pose) rospy.loginfo('New target pose: {0}'.format(pose)) return True def _publish_zero_velocity(self): self._publish_velocity(Velocity()) def _publish_velocity(self, vel): self._velocity_publisher.publish(vel.get_twist()) def _is_quaternion_valid(self, q): if any([math.isinf(a) or math.isnan(a) for a in [q.x, q.y, q.z, q.w]]): rospy.logwarn('Quaternion has NaN\'s or infinities.') return False # TODO: check quaternion length and rotation test return True pass
class MotionControllerNode: """ This is a port of the AMR C++ MotionControllerNode """ CONTROLLER_TYPE_DIFF = 'diff' CONTROLLER_TYPE_OMNI = 'omni' CONTROLLER_TYPE_UNSPECIFIED = 'unsp' def __init__(self): rospy.init_node(NODE) """ Parameters """ max_linear_velocity = rospy.get_param('max_linear_velocity', 0.3) max_linear_acceleration = rospy.get_param('max_linear_acceleration', 0.05) linear_tolerance = rospy.get_param('linear_tolerance', 0.02) max_angular_velocity = rospy.get_param('max_angular_velocity', 0.2) max_angular_acceleration = rospy.get_param('max_angular_acceleration', 0.03) angular_tolerance = rospy.get_param('angular_tolerance', 0.02) abort_if_obstacle_detected = rospy.get_param( 'abort_if_obstacle_detected', True) self._controller_frequency = rospy.get_param('controller_frequency', 10.0) controller_type = rospy.get_param('~controller', self.CONTROLLER_TYPE_UNSPECIFIED) if controller_type == self.CONTROLLER_TYPE_DIFF: #Create diff controller self._velocity_controller = DiffVelocityController( max_linear_velocity, linear_tolerance, max_angular_velocity, angular_tolerance) elif controller_type == self.CONTROLLER_TYPE_OMNI: self._velocity_controller = OmniVelocityController( max_linear_velocity, linear_tolerance, max_angular_velocity, angular_tolerance) """ ========================= YOUR CODE HERE ========================= Instructions: create an instance of OmniVelocityController. Hint: you may copy-paste from the DiffVelocityController case and adjust the arguments in the call to the constructor to conform to what you have implemented in that class. """ elif controller_type == self.CONTROLLER_TYPE_UNSPECIFIED: rospy.logerr('Controller type not specified. ' 'Check the [controller] launch parameter') exit() else: #Unknown controller rospy.logerr('Requested controller type "{0}" unknown. ' 'Check the [controller] launch parameter'.format( controller_type)) exit() """ Publishers """ self._velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10) self._current_goal_publisher = rospy.Publisher(NODE + '/current_goal', PoseStamped, latch=True, queue_size=0) self._action_goal_publisher = rospy.Publisher(NODE + '/move_to/goal', MoveToActionGoal, queue_size=1) """ Subscribers """ self._simple_goal_subscriber = rospy.Subscriber( NODE + '/move_to_simple/goal', PoseStamped, self._simple_goal_callback, queue_size=1) if abort_if_obstacle_detected: self._obstacles_subscriber = rospy.Subscriber( 'obstacles', Obstacle, self._obstacles_callback, queue_size=100) """ Action server """ self._move_to_server = SimpleActionServer(NODE + '/move_to', MoveToAction, self._move_to_callback, auto_start=False) self._move_to_server.start() self._tf = tf.TransformListener() rospy.loginfo('Started [motion_controller] node.') def _move_to_callback(self, move_to_goal): """ Triggered with a request to move_to action server """ rospy.loginfo('Received [move_to] action command.') if not self._set_new_goal(move_to_goal): return else: rate = rospy.Rate(self._controller_frequency) while not rospy.is_shutdown(): if not self._move_to_server.is_active(): # Exit if the goal was aborted return if self._move_to_server.is_preempt_requested(): # Process pending preemption requests rospy.loginfo('Action preemption requested.') if (self._move_to_server.is_new_goal_available() and self._set_new_goal( self._move_to_server.accept_new_goal())): # New goal already set pass else: # No new goals, preempt explicitly and exit the callback self._publish_zero_velocity() self._move_to_server.set_preempted() return if not self._move_towards_goal(): # Finish execution if the goal was reached self._move_to_server.set_succeeded(MoveToResult(), 'Goal reached.') self._publish_zero_velocity() return rate.sleep() self._move_to_server.set_aborted( MoveToResult(), 'Aborted. The node has been killed.') def _simple_goal_callback(self, target_pose): """ Wrapper for simple goal action. Forwards as a request to the move_to action server. Has to be tested! """ rospy.loginfo( 'Received target pose through the "simple goal" topic.' 'Wrapping it in the action message and forwarding to the server.') rospy.logwarn('Simple goal control is yet to be tested!') action_goal = MoveToActionGoal() action_goal.header.stamp = rospy.Time.now() action_goal.goal.target_pose = target_pose self._action_goal_publisher.publish(action_goal) def _obstacles_callback(self, obstacle_msg): rospy.logwarn( 'An obstacle was detected. Will stop the robot and cancel the current action.' ) if self._move_to_server.is_active(): self._move_to_server.set_aborted( MoveToResult(), 'Obstacle encountered, aborting...') self._publish_zero_velocity() def _move_towards_goal(self): try: time = self._tf.getLatestCommonTime("odom", "base_footprint") position, quaternion = self._tf.lookupTransform( "odom", "base_footprint", time) except Exception as ex: rospy.logwarn( 'Transform lookup failed (\odom -> \base_footprint). ' 'Reason: {0}.'.format(ex.message)) return True current_pose = Pose2D() current_pose.x, current_pose.y = position[0], position[1] current_pose.theta = tf.transformations.euler_from_quaternion( quaternion)[2] velocity = self._velocity_controller.compute_velocity(current_pose) if self._velocity_controller.is_target_reached(): rospy.loginfo('The goal was reached') return False else: self._publish_velocity(velocity) return True def _set_new_goal(self, new_goal): """ Set new target pose as given in the goal message. Checks if the orientation provided in the target pose is valid. Publishes the goal pose for the visualization purposes. Returns true if the goal was accepted. """ if not self._is_quaternion_valid( new_goal.target_pose.pose.orientation): rospy.logwarn('Aborted. Target pose has invalid quaternion.') self._move_to_server.set_aborted( MoveToResult(), 'Aborted. Target pose has invalid quaternion.') return False else: x = new_goal.target_pose.pose.position.x y = new_goal.target_pose.pose.position.y yaw = tf.transformations.euler_from_quaternion([ new_goal.target_pose.pose.orientation.x, new_goal.target_pose.pose.orientation.y, new_goal.target_pose.pose.orientation.z, new_goal.target_pose.pose.orientation.w ])[2] pose = Pose2D(x, y, yaw) self._velocity_controller.set_target_pose(pose) self._current_goal_publisher.publish(new_goal.target_pose) rospy.loginfo('New target pose: {0}'.format(pose)) return True def _publish_zero_velocity(self): self._publish_velocity(Velocity()) def _publish_velocity(self, vel): self._velocity_publisher.publish(vel.get_twist()) def _is_quaternion_valid(self, q): if any([math.isinf(a) or math.isnan(a) for a in [q.x, q.y, q.z, q.w]]): rospy.logwarn('Quaternion has NaN\'s or infinities.') return False # TODO: check quaternion length and rotation test return True pass
class ActionServerWrapper(): """SMACH container wrapper with actionlib ActionServer. Use this class to associate an action server with a smach L{StateMachine<smach.state_machine.StateMachine>}. This allows invocation of the state machine over the actionlib API/protocol. This class delegates to a provided SMACH container and associates it with an action server. The user can specify lists of outcomes which correspond to different action result statuses (SUCCEEDED, ABORTED, PREEMPTED). Once the delegate state machine leaves one of these outcomes, this wrapper class will cause the state machine to terminate, and cause the action server to return a result. Note that this class does not inherit from L{smach.State<smach.State>} and can only be used as a top-level container. """ def __init__(self, server_name, action_spec, wrapped_container, succeeded_outcomes=[], aborted_outcomes=[], preempted_outcomes=[], goal_key='action_goal', feedback_key='action_feedback', result_key='action_result', goal_slots_map={}, feedback_slots_map={}, result_slots_map={}, expand_goal_slots=False, pack_result_slots=False): """Constructor. @type server_name: string @param server_name: The name of the action server that this container will present. @type action_spec: actionlib action msg @param action_spec: The type of action this server will present @type wrapped_container: L{StateMachine} @param wrapped_container: The state machine to manipulate @type succeeded_outcomes: array of strings @param succeeded_outcomes: Array of terminal state labels which, when left, should cause the action server to return SUCCEEDED as a result status. @type aborted_outcomes: array of strings @param aborted_outcomes: Array of terminal state labels which, when left, should cause the action server to return ABORTED as a result status. @type preempted_outcomes: array of strings @param preempted_outcomes: Array of terminal state labels which, when left, should cause the action server to return PREEMPTED as a result status. @type goal_key: string @param goal_key: The userdata key into which the action goal should be stuffed when the action server receives one. @type feedback_key: string @param feedback_key: The userdata key into which the SMACH container can put feedback information relevant to the action. @type result_key: string @param result_key: The userdata key into which the SMACH container can put result information from this action. """ # Store state machine self.wrapped_container = wrapped_container """State machine that this wrapper talks to.""" # Register state machine callbacks self.wrapped_container.register_transition_cb(self.transition_cb) self.wrapped_container.register_termination_cb(self.termination_cb) # Grab reference to state machine user data (the user data in the children # states scope) self.userdata = smach.UserData() # Store special userdata keys self._goal_key = goal_key self._feedback_key = feedback_key self._result_key = result_key self._goal_slots_map = goal_slots_map self._feedback_slots_map = feedback_slots_map self._result_slots_map = result_slots_map self._expand_goal_slots = expand_goal_slots self._pack_result_slots = pack_result_slots # Store goal, result, and feedback types self.userdata[self._goal_key] = copy.copy( action_spec().action_goal.goal) self.userdata[self._result_key] = copy.copy( action_spec().action_result.result) self.userdata[self._feedback_key] = copy.copy( action_spec().action_feedback.feedback) # Action info self._server_name = server_name self._action_spec = action_spec # Construct action server (don't start it until later) self._action_server = SimpleActionServer(self._server_name, self._action_spec, execute_cb=self.execute_cb, auto_start=False) # Store and check the terminal outcomes self._succeeded_outcomes = set(succeeded_outcomes) self._aborted_outcomes = set(aborted_outcomes) self._preempted_outcomes = set(preempted_outcomes) # Make sure the sets are disjoint card_of_unions = len(self._succeeded_outcomes | self._aborted_outcomes | self._preempted_outcomes) sum_of_cards = (len(self._succeeded_outcomes) + len(self._aborted_outcomes) + len(self._preempted_outcomes)) if card_of_unions != sum_of_cards: rospy.logerr( "Succeeded, aborted, and preempted outcome lists were not mutually disjoint... expect undefined behavior." ) def run_server(self): """Run the state machine as an action server. Note that this method does not block. """ # Register action server callbacks #self._action_server.register_goal_callback(self.goal_cb) self._action_server.register_preempt_callback(self.preempt_cb) # Stat server (because we disabled auto-start to register the callbacks) self._action_server.start() rospy.loginfo( "Started SMACH action server wrapper, adversiting as '%s'" % self._server_name) ### State machine callbacks def transition_cb(self, userdata, active_states): """Transition callback passed to state machine. This method is called each time the state machine transitions. """ rospy.logdebug("Publishing action feedback.") # Publish action feedback self.publish_feedback(userdata) def termination_cb(self, userdata, terminal_states, container_outcome): """Termination callback passed to state machine. This callback receives the final state and state machine outcome as specified by the state-outcome map given to the delegate container on construction (see L{ActionServerWrapper.__init__}). Remember that in this context, the SMACH container is just a single state object, which has an outcome like any other state; it is this outcome on which we switch here. This method will determine from the state machine outcome which result should be returned to the action client for this goal. """ rospy.logdebug( "Wrapped state machine has terminated with final state: " + str(terminal_states) + " and container outcome: " + str(container_outcome)) def publish_feedback(self, userdata): """Publish the feedback message in the userdata db. Note that this feedback is independent of smach. """ if self._feedback_key in userdata: # This was spewing errors after the fix to ticket #5033 was submitted # in the case when _feedback_key is not set # For now, the fix is just checking if it exists, and not publishing otherwise # The spewage used to not happen because we were looking in self.userdata # and the constructor of this class sets the feedback key there to an empty struct # TODO figure out what the hell is going on here. self._action_server.publish_feedback(userdata[self._feedback_key]) ### Action server callbacks def execute_cb(self, goal): """Action server goal callback This method is called when the action server associated with this state machine receives a goal. This puts the goal into the userdata, which is the userdata of the contained state. """ # If the state machine is running, we should preempt it before executing it # it again. rospy.logdebug("Starting wrapped SMACH container") # Accept goal #goal = self._action_server.accept_new_goal() # Expand the goal into the root userdata for this server if self._expand_goal_slots: for slot in goal.__slots__: self.userdata[slot] = getattr(goal, slot) # Store the goal in the container local userdate self.userdata[self._goal_key] = goal # Store mapped goal slots in local userdata for from_key, to_key in ((k, self._goal_slots_map[k]) for k in self._goal_slots_map): self.userdata[to_key] = getattr(goal, from_key) # Run the state machine (this blocks) try: container_outcome = self.wrapped_container.execute( smach.Remapper( self.userdata, self.wrapped_container.get_registered_input_keys(), self.wrapped_container.get_registered_output_keys(), {})) except smach.InvalidUserCodeError as ex: rospy.logerr("Exception thrown while executing wrapped container.") self._action_server.set_aborted() return except: rospy.logerr( "Exception thrown:while executing wrapped container: " + traceback.format_exc()) self._action_server.set_aborted() return # Grab the (potentially) populated result from the userdata result = self.userdata[self._result_key] # Store mapped slots in result for from_key, to_key in ((k, self._result_slots_map) for k in self._result_slots_map): setattr(result, from_key, self.userdata[to_key]) # If any of the result members have been returned to the parent ud # scope, overwrite the ones from the full structure if self._pack_result_slots: for slot in result.__slots__: if slot in self.userdata: setattr(result, slot, self.userdata[slot]) # Set terminal state based on state machine state outcome if container_outcome in self._succeeded_outcomes: rospy.loginfo('SUCCEEDED') self._action_server.set_succeeded(result) elif container_outcome in self._preempted_outcomes: rospy.loginfo('PREEMPTED') self._action_server.set_preempted(result) else: #if container_outcome in self._aborted_outcomes: rospy.loginfo('ABORTED') self._action_server.set_aborted(result) def preempt_cb(self): """Action server preempt callback. This method is called when the action client preempts an active goal. In this case, the StateMachine needs to propagate said preemption to the currently active delegate action (the current state). """ rospy.loginfo("Preempt on state machine requested!") self.wrapped_container.request_preempt()