- Userdata transfer from State to sub State Machine
If we would like to transfer data from state in the top State machine to the sub state machine, we can remap the outkey of state with the input key of sub state machine , which could also be used in any transformation from state to state machine,but also we could create a userdata in the top state machine and remap it from outkey of state when adding the state and remap it to input key of sub state machine when adding the sub state machine, which means that we create an userdata as a bridge between state and sub state machine.
If we want to use this input key of sub state machine, for example, passing the input key to an user data in sub state machine which will be used for other states in this sub state machine, then we need rewrite the class of this sub state machine to overriding the function of execute where we can set the user data according to the input key. - Concurrence
This state allows for simple split-join concurrency. The user adds a set ofstates which are all executed simultaneously. The concurrent split statecan only transition once all contained states are ready to transition.This container can be configured to return a given outcome as a function ofthe outcomes of the contained states. This is specified in the constructorof the class, or after construction with L{Concurrence.add_outcome_map}.
While a concurrence will not terminate until all if its children terminate,it is possible for it to preempt a subset of states- All child states terminate- At least one child state terminates- A user-defined callback signals termination
Given these causes of termination, the outcome can be determined in four ways:- A user-defined callback returns an outcome- A child-outcome map which requires ALL states to terminate is satisfied- A child-outcome map which requires ONE state to terminate is satisfied- No maps are satisfied, so the default outcome is returned
The specification of the outcome maps and the outcome callback aredescribed in the constructor documentation below. More than one policy canbe supplied, and each policy has the potential to not be satisfied. In thesituation in which multiple policies are provided, and a given policy isnot satisfied, the outcome choice precedence is as follows:- Outcome callback- First-triggered outcome map- last-triggered outcome map- Default outcome
In practive it is best to try to accomplish your task with just ONE outcomepolicy
def __init__ ( self ,outcomes ,default_outcome ,input_keys = [],output_keys = [],outcome_map = {},outcome_cb = None ,child_termination_cb = None):"""Constructor for smach Concurrent Split.
@type outcomes: list of strings@param outcomes: The potential outcomes of this state machine.
@type default_outcome: string@param default_outcome: The outcome of this state if no elements in theoutcome map are satisfied by the outcomes of the contained states.
@type outcome_map: list@param outcome_map: This is an outcome map for determining theoutcome of this container. Each outcome of the container is mappedto a dictionary mapping child labels onto outcomes. If none of thechild-outcome maps is satisfied, the concurrence will terminatewith thhe default outcome.For example, if the and_outcome_map is:{'succeeded' : {'FOO':'succeeded', 'BAR':'done'},'aborted' : {'FOO':'aborted'}}Then the concurrence will terimate with outcome 'succeeded' only ifBOTH states 'FOO' and 'BAR' have terminatedwith outcomes 'succeeded' and 'done', respectively. The outcome'aborted' will be returned by the concurrence if the state 'FOO'returns the outcome 'aborted'.
If the outcome of a state is not specified, it will be treated asirrelevant to the outcome of the concurrence
If the criteria for one outcome is the subset of another outcome,the container will choose the outcome which has more child outcomecriteria satisfied. If both container outcomes have the samenumber of satisfied criteria, the behavior is undefined.
If a more complex outcome policy is required, see the user canprovide an outcome callback. See outcome_cb, below.
@type child_termination_cb: callale@param child_termination_cb: This callback gives the user the abilityto force the concurrence to preempt running states given thetermination of some other set of states. This is useful when usinga concurrence as a monitor container.
This callback is called each time a child state terminates. It ispassed a single argument, a dictionary mapping child state labelsonto their outcomes. If a state has not yet terminated, it's dictvalue will be None.
This function can return three things:- False: continue blocking on the termination of all other states- True: Preempt all other states- list of state labels: Preempt only the specified states
I{If you just want the first termination to cause the other childrento terminate, the callback (lamda so: True) will always return True.}
@type outcome_cb: callable@param outcome_cb: If the outcome policy needs to be more complicatedthan just a conjunction of state outcomes, the user can supplya callback for specifying the outcome of the container.
This callback is called only once all child states have terminated,and it is passed the dictionary mapping state labels onto theirrespective outcomes.
If the callback returns a string, it will treated as the outcome ofthe container.
If the callback returns None, the concurrence will first check theoutcome_map, and if no outcome in the outcome_map is satisfied, itwill return the default outcome.
B{ NOTE : This callback should be a function ONLY of the outcomes ofthe child states. It should not access any other resources.} - Simple Action State (ROS)
class SimpleActionState ( State ):"""Simple action client state.Use this class to represent an actionlib as a state in a state machine."""
# Meta-states for this actionWAITING_FOR_SERVER = 0INACTIVE = 1ACTIVE = 2PREEMPTING = 3COMPLETED = 4
def __init__ ( self ,# Action infoaction_name ,action_spec ,# Default goalgoal = None ,goal_key = None ,goal_slots = [],goal_cb = None ,goal_cb_args = [],goal_cb_kwargs = {},# Result modesresult_key = None ,result_slots = [],result_cb = None ,result_cb_args = [],result_cb_kwargs = {},# Keysinput_keys = [],output_keys = [],outcomes = [],# Timeoutsexec_timeout = None ,preempt_timeout = rospy.Duration( 60.0 ),server_wait_timeout = rospy.Duration( 60.0 )):"""Constructor for SimpleActionState action client wrapper.@type action_name: string@param action_name: The name of the action as it will be broadcast over ros.
@type action_spec: actionlib action msg (.action file to generate automatically)
@param action_spec: The type of action to which this client will connect.
@type goal: actionlib goal msg@param goal: If the goal for this action does not need to be generated atruntime, it can be passed to this state on construction.
@type goal_key: string@param goal_key: Pull the goal message from a key in the userdata.This will be done before calling the goal_cb if goal_cb is defined.
@type goal_slots: list of string (as input_keys remapping to userdata)
@param goal_slots: Pull the goal fields (__slots__) from like-namedkeys in userdata. This will be done before calling the goal_cb ifgoal_cb is defined.
@type goal_cb: callable@param goal_cb: If the goal for this action needs to be generated atruntime, a callback can be stored which modifies the default goalobject. The callback is passed two parameters:- userdata- current stored goalThe callback returns a goal message.
@type result_key: string@param result_key: Put the result message into the userdata withthe given key. This will be done after calling the result_cbif result_cb is defined.
@type result_slots: list of strings (as output_keys directly remapping to userdata)
@param result_slots: Put the result message fields (__slots__)into the userdata with keys like the field names. This will be doneafter calling the result_cb if result_cb is defined.
@type result_cb: callable@param result_cb: If result information from this action needs to bestored or manipulated on reception of a result from this action, acallback can be stored which is passed this information. The callbackis passed three parameters:- userdata (L{UserData<smach.user_data.UserData>})- result status (C{actionlib.GoalStatus})- result (actionlib result msg)
@type exec_timeout: C{rospy.Duration}@param exec_timeout: This is the timeout used for sending a preempt messageto the delegate action. This is C{None} by default, which implies notimeout.
@type preempt_timeout: C{rospy.Duration}@param preempt_timeout: This is the timeout used for aborting after apreempt has been sent to the action and no result has been received. Thistimeout begins counting after a preempt message has been sent.
@type server_wait_timeout: C{rospy.Duration}@param server_wait_timeout: This is the timeout used for aborting whilewaiting for an action server to become active.""" - Sequence container
sq = Sequence( outcomes = ['succeeded','aborted','preempted'], connector_outcome = 'succeeded') with sq: Sequence.add('MOVE_ARM_GRAB_PRE', MoveVerticalGripperPoseActionState()) Sequence.add('MOVE_GRIPPER_OPEN', MoveGripperState(GRIPPER_MAX_WIDTH)) Sequence.add('MOVE_ARM_GRAB', MoveVerticalGripperPoseActionState()) Sequence.add('MOVE_GRIPPER_CLOSE', MoveGripperState(grab_width)) Sequence.add('MOVE_ARM_GRAB_POST', MoveVerticalGripperPoseActionState())
class Sequence ( smach . state_machine . StateMachine ):"""Sequence Container
This container inherits functionality from L{smach.StateMachine} and addssome auto-generated transitions that create a sequence of states from theorder in which said states are added to the container."""def __init__ ( self ,outcomes ,connector_outcome ,input_keys =[],output_keys =[]):"""Constructor.
@type outcomes: list of string@param outcomes: The potential outcomes of this container.
@type connector_outcome: string@param connector_outcome: The outcome used to connect states in thesequence."""smach.state_machine.StateMachine. __init__ ( self , outcomes, input_keys, output_keys)
self ._last_added_seq_label = Noneself ._connector_outcome = connector_outcome
### Construction Methods@ staticmethoddef add ( label , state , transitions = None , remapping = None ):"""Add a state to the sequence.Each state added will receive an additional transition from it to thestate which is added after it. The transition will follow the outcomespecified at construction of this container.@type label: string@param label: The label of the state being added.@param state: An instance of a class implementing the L{State} interface.@param transitions: A dictionary mapping state outcomes to other statelabels. If one of these transitions follows the connector outcomespecified in the constructor, the provided transition will overridethe automatically generated connector transition.""" - Iterator container
class Iterator ( smach . container . Container ):"""Sequence Container
This container inherits functionality from L{smach.StateMachine} and addssome auto-generated transitions that create a sequence of states from theorder in which said states are added to the container."""def __init__ ( self ,outcomes ,input_keys ,output_keys ,it = [],it_label = 'it_data' ,exhausted_outcome = 'exhausted' ):"""Constructor.
@type outcomes: list of string@param outcomes: The potential outcomes of this container.
@type it: iterable@param iteritems: Items to iterate over on each cycle
@type it_label: string@param iteritems_label: The label that the item in the currentiteration will be given when it is put into the container's localuserdata."""if exhausted_outcome not in outcomes:outcomes.append(exhausted_outcome)smach.container.Container. __init__ ( self , outcomes, input_keys, output_keys)
self ._items = itself ._items_label = it_label
self ._is_running = False
self ._state_label = ''self ._state = Noneself ._loop_outcomes = []self ._break_outcomes = []self ._final_outcome_map = {}self ._exhausted_outcome = exhausted_outcome
### Construction Methods@ staticmethoddef set_iteritems ( it , it_label = 'it_data' ):"""Set the list or generator for the iterator to iterate over.@type it: iterable@param iteritems: Items to iterate over on each cycle
@type it_label: string@param iteritems_label: The label that the item in the currentiteration will be given when it is put into the container's localuserdata.
@type exhausted_outcome: string@param exhausted_outcome: If the iterable is exhausted without a breakcondition this outcome is emitted by the container."""# Get currently opened containerself = Iterator._currently_opened_container()self ._items = itself ._items_label = it_label
@ staticmethoddef set_contained_state (label ,state ,loop_outcomes = [],break_outcomes = [],final_outcome_map = {}):"""Set the contained state@type label: string@param label: The label of the state being added.@type state: L{smach.State}@param state: An instance of a class implementing the L{smach.State} interface.
@param loop_outcomes: List of contained state outcomes that should causethe iterator to continue. If this is empty, all outcomes that are notin the break_outcomes list will cause the iterator to continue toiterate. NOTE : loop_outcomes will be overriden by break_outcomes if bothparameters are used.
@param break_outcomes: List of contained state outcomes that shouldcause the iterator to break. When the contained state emits an outcomein this list, the container will terminate and return either thatoutcome or the outcome it is mapped to in final_outcome_map. NOTE :loop_outcomes will be overriden by break_outcomes if bothparameters are used.
@param final_outcome_map: A map from contained state outcomes to containeroutcomes. On termination of the iterator (either from finishing or froma break) this map will be used to translate contained state outcomes tocontainer outcomes.Unspecified contained state outcomes will fall through ascontainer outcomes."""
- ActionServerWrapper (Wrapper a container with Actionlib)
# Construct state machine sm = StateMachine( outcomes=['did_something','did_something_else','aborted','preempted'], input_keys = ['my_awesome_goal'], output_keys = ['egad_its_a_result'])
# Construct action server wrapperasw = ActionServerWrapper( 'my_action_server_name', MyAction, sm,succeeded_outcomes =['did_something','did_something_else'],
aborted_outcomes = ['aborted'],
preempted_outcomes = ['preempted'], goal_key = 'my_awesome_goal', result_key = 'egad_its_a_result' )
SMACH provides the top-level container called ActionServerWrapper. This class advertises anactionlibaction server. Instead of being executed by a parent,its contained state goes active when the action server receives a goal. Accordingly, this container does not inherit from the smach.State base class and cannot be put inside of another container
class ActionServerWrapper ():"""SMACH container wrapper with actionlib ActionServer.
Use this class to associate an action server with a smachL{StateMachine<smach.state_machine.StateMachine>}. This allows invocationof the state machine over the actionlib API/protocol.
This class delegates to a provided SMACH container and associates it with anaction server. The user can specify lists of outcomes which correspond todifferent action result statuses (SUCCEEDED, ABORTED, PREEMPTED). Once thedelegate state machine leaves one of these outcomes, this wrapper class willcause the state machine to terminate, and cause the action server to returna result.
Note that this class does not inherit from L{smach.State<smach.State>} andcan 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 willpresent.
@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 bestuffed when the action server receives one.
@type feedback_key: string@param feedback_key: The userdata key into which the SMACH containercan put feedback information relevant to the action.
@type result_key: string@param result_key: The userdata key into which the SMACH containercan put result information from this action."""
-
ServiceState (ROS)
You could simply call any service from ageneric state, but SMACH has specific support to call services, saving you a lot of code! SMACH provides a state class that acts as a proxy to aROS service. The instantiation of the state takes a service name, service type, and some policy for generating a service request. The possible outcomes of the service state are 'succeeded', 'preempted' and 'aborted'.
![/!\](http://wiki.ros.org/moin_static197/rostheme/img/alert.png "/!\")
The service state is almost identical to the simple action state. Just replace 'goal' with 'request', and 'result' with 'response'. - MonitorState(ROS)
class MonitorState ( smach . State ):"""A state that will check a given ROS topic with a condition function."""def __init__ ( self , topic , msg_type , cond_cb , max_checks =- 1 , input_keys = [], output_keys =[]):smach.State. __init__ ( self , outcomes =[ 'valid' , 'invalid' , 'preempted' ], input_keys = input_keys, output_keys = output_keys)
self ._topic = topicself ._msg_type = msg_typeself ._cond_cb = cond_cbself ._max_checks = max_checksself ._n_checks = 0
self ._trigger_event = threading.Event()
def execute ( self , ud ):# If prempted before even getting a chance, give up.if self .preempt_requested():self .service_preempt()return 'preempted'self ._n_checks = 0self ._trigger_event.clear()self ._sub = rospy.Subscriber( self ._topic, self ._msg_type, self ._cb, callback_args =ud)
self ._trigger_event.wait()self ._sub.unregister()
if self .preempt_requested():self .service_preempt()return 'preempted'
if self ._max_checks > 0 and self ._n_checks >= self ._max_checks:return 'valid'
return 'invalid'
def _cb ( self , msg , ud ) :self ._n_checks += 1try :if ( self ._max_checks > 0 and self ._n_checks >= self ._max_checks) or not self ._cond_cb(ud, msg):self ._trigger_event.set()except Exception as e:rospy.logerr( "Error thrown while executing condition callback %s: %s" % ( str ( self ._cond_cb), e))self ._trigger_event.set()
def request_preempt ( self ):smach.State.request_preempt( self )self ._trigger_event.set()
Python-learning-SMACH-Notes
最新推荐文章于 2022-03-22 20:16:26 发布
1381
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
