def main(): #Create a rospy node for the state machine rospy.init_node('IRA_statemachine') #Creating a state machine for banking scenario sm_bank = StateMachine(outcomes=['Stop']) #Create Smach containers for each state with sm_bank: StateMachine.add('Face Recognition', frn.Face_recognition(), transitions={'Detected':'Speech_Gesture','Not_Detected':'Face Recognition'},remapping={'Face_recognition_out':'speech_in'}) sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Speech1':'Completed','Gesture1':'Completed'}}) with sm_con: smach.Concurrence.add('Speech1',ss1.speech()) smach.Concurrence.add('Gesture1',g1.gesture()) smach.StateMachine.add('Speech_Gesture', sm_con, transitions={'Waiting':'Stop', 'Done':'Face Recognition'}) StateMachine.add('Hotword_detection', hwdc.Hotword_detection(), transitions={'Detected':'Speech_Recognition','Not_Detected':'Face Recognition'}) StateMachine.add('Speech_Recognition', src.Speech_recognition(), transitions={'Completed':'Speech_database','Not_Completed':'Face Recognition'},remapping={'speech_recognition_out':'Database_in'}) StateMachine.add('Speech_database', sdc.Speech_database(), transitions={'Completed':'CON','Not_Completed':'Face Recognition'},remapping={'speech_database_out':'Text_in'}) sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Text_speech':'Completed'}},input_keys=['Text_in']) with sm_con: smach.Concurrence.add('Text_speech',ts.Text_speech()) #smach.Concurrence.add('ui1',ui1.Text_reply()) StateMachine.add('CON', sm_con, transitions={'Waiting':'Stop','Done':'Hotword_detection'}) #Code for initiating the smach_viewer Monitor = smach_ros.IntrospectionServer('IRA', sm_bank, '/IRA_FLOW') Monitor.start() #Execute the state machine outcome = sm_bank.execute() #Stop the smach_viewer if state machine stops Monitor.stop
def pass_first(): pass_ball_first = smach.StateMachine(outcomes=['successed', 'failed']) rospy.logwarn("pass_ball_first STARTING!!!") with pass_ball_first: start = smach.Concurrence(outcomes=['successed','failed'], default_outcome='successed', outcome_map={'successed':{'SHOVEL_DOWN':'successed', 'MOVE_POINT_TO_SHOOT':'successed'}}) with start: smach.Concurrence.add('SHOVEL_DOWN',Shovel_Down()) smach.Concurrence.add('MOVE_POINT_TO_SHOOT',Move_Point_To_Shoot()) smach.StateMachine.add('Start',start,transitions={'successed':'SHOOT1', 'failed':'failed' }) smach.StateMachine.add('SHOOT1',Shoot(), transitions={'successed':'Shovel_Control_Up1', 'failed':'failed'}) smach.StateMachine.add('Shovel_Control_Up1',Shovel_UP(), transitions={'successed':'ADJUST1', 'failed':'failed'}) smach.StateMachine.add('ADJUST1',Move_To_Find_Ball(), transitions={'successed':'FindBall', 'failed':'failed'}) smach.StateMachine.add('FindBall', Search_Ball(), transitions={'successed': 'ADJUST2', 'failed': 'failed'}) smach.StateMachine.add('ADJUST2',Shoot_Adjust(), transitions={'successed':'SHOOT2', 'failed':'failed'}) smach.StateMachine.add('SHOOT2',Shoot(), transitions={'successed':'Shovel_Control_Up2', 'failed':'failed'}) smach.StateMachine.add('Shovel_Control_Up2',Shovel_UP(), transitions={'successed':'My Life For Auir', 'failed':'failed'}) Auir = smach.Concurrence(outcomes=['successed','failed'], default_outcome='failed', outcome_map={'successed':{'RETURN':'successed', 'Return_Adjust':'successed'}}) with Auir: smach.Concurrence.add('RETURN',Return()) smach.Concurrence.add('Return_Adjust',Return_Adjust()) smach.StateMachine.add('My Life For Auir',Auir,transitions={'successed':'successed', 'failed':'failed'}) pass_ball_first.execute() rospy.logerr('Pass_Ball_First has finished!!!!')
def main(): rospy.init_node('arm_test_sm') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['succeeded', 'failed']) # Open the container with sm: # Add states to the container # smach.StateMachine.add('INIT', FakeState(), # transitions={'succeeded':'STOW_ARM', # 'aborted':'failed', # 'preempted':'failed'}) # smach.StateMachine.add('STOW_ARM', ArmStowState(), # transitions={'succeeded':'BEFORE_DETECT', # 'aborted':'failed', # 'preempted':'failed'}) smach.StateMachine.add('BEFORE_DETECT', FakeState(), transitions={ 'succeeded': 'WAIT_DETECT', 'aborted': 'failed', 'preempted': 'failed' }) wait_detect_concurrence = smach.Concurrence( outcomes=['succeeded', 'failed'], default_outcome='failed', output_keys=['detection_msg'], child_termination_cb=child_term_cb, outcome_cb=out_cb) with wait_detect_concurrence: smach.Concurrence.add('WAIT_FOR_DETECTION', create_detect_state()) #smach.Concurrence.add('WAIT_DETECT_FAKE', FakeState()) smach.StateMachine.add('WAIT_DETECT', wait_detect_concurrence, transitions={ 'succeeded': 'DETECTION_PICKUP', 'failed': 'failed' }) smach.StateMachine.add('DETECTION_PICKUP', DetectionPickupState( rospy.Duration.from_sec(40.0)), transitions={ 'succeeded': 'succeeded', 'aborted': 'failed', 'preempted': 'failed' }) sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm.execute() #rospy.spin() sis.stop()
def main(): rospy.init_node('smach_example_state_machine') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Open the container with sm: # Add states to the container smach.StateMachine.add('FOO', Foo(), transitions={ 'succeeded': 'SUB', 'aborted': 'aborted', 'preempted': 'preempted' }) sm_con = smach.Concurrence( outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='aborted', child_termination_cb=child_term_cb, outcome_cb=out_cb) with sm_con: smach.Concurrence.add('BAR', Bar()) smach.Concurrence.add('BAS', Bas()) smach.StateMachine.add('SUB', sm_con) # Create and start the introspection server sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm.execute() rospy.spin() sis.stop()
def execute(self, ud): rospy.loginfo("Starting next task....") cm_movingToPoint = smach.Concurrence( outcomes=['marker_detected', 'marker_undetected', 'time_out'], default_outcome='marker_undetected', outcome_map={ 'marker_detected': { 'MARKER_DETECT': 'succeeded' }, 'time_out': { 'CONTROLLER': 'succeeded' } }) moveGoal = advancedControllerGoal() moveGoal.x = ud.x2 moveGoal.y = ud.y2 moveGoal.depth = ud.depth markGoal = markerGoal() markGoal.order = header.DETECT_MARKER with cm_movingToPoint: smach.Concurrence.add( 'CONTROLLER', SimpleActionState("advancedController", advancedControllerAction, goal=moveGoal)) smach.Concurrence.add( 'MARKER_DETECT', SimpleActionState("marker_detect", goal=markGoal)) return 'done'
def main(): rospy.init_node('s_m', anonymous=True) base = smach.Concurrence(outcomes=[ 'looking1', 'looking2', 'looking3', 'looking4', 'moving_yes', 'moving_no', 'base_reset' ], default_outcome='base_reset', child_termination_cb=child_term_cb, outcome_cb=out_cb) with base: smach.Concurrence.add('WAITING', Waiting()) smach.Concurrence.add( 'SENSOR1', smach_ros.MonitorState("chat", String, monitor_cb1)) smach.Concurrence.add( 'SENSOR2', smach_ros.MonitorState("chat", String, monitor_cb2)) smach.Concurrence.add( 'SENSOR3', smach_ros.MonitorState("chat", String, monitor_cb3)) smach.Concurrence.add( 'SENSOR4', smach_ros.MonitorState("chat", String, monitor_cb4)) smach.Concurrence.add( 'SENSOR_YES', smach_ros.MonitorState("chat", String, monitor_cb_yes)) smach.Concurrence.add( 'SENSOR_NO', smach_ros.MonitorState("chat", String, monitor_cb_no)) # Create a SMACH state machine sm = smach.StateMachine(outcomes=['done']) # Open the container with sm: smach.StateMachine.add('SETUP', Setup(), transitions={'setup_done': 'BASE'}) smach.StateMachine.add('BASE', base, transitions={ 'looking1': 'GOAL_1', 'looking2': 'GOAL_2', 'looking3': 'GOAL_3', 'looking4': 'GOAL_4', 'moving_yes': 'GOAL_YES', 'moving_no': 'GOAL_NO', 'base_reset': 'BASE' }) smach.StateMachine.add('GOAL_1', gaze1(), {'succeeded': 'BASE'}) smach.StateMachine.add('GOAL_2', gaze2(), {'succeeded': 'BASE'}) smach.StateMachine.add('GOAL_3', gaze3(), {'succeeded': 'BASE'}) smach.StateMachine.add('GOAL_4', gaze4(), {'succeeded': 'BASE'}) smach.StateMachine.add('GOAL_YES', yes(), {'succeeded': 'BASE'}) smach.StateMachine.add('GOAL_NO', no(), {'succeeded': 'BASE'}) sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm.execute()
def __allocMoveConcurrency__(self, slave_namespaces): map_abort = dict() for name in slave_namespaces: map_abort[name + "/Move"] = 'aborted' map_abort[name + "/Move"] = 'preempted' map_succed = dict() for name in slave_namespaces: map_succed[name + "/Move"] = 'succeeded' sm_con = smach.Concurrence(outcomes=['success', 'error_occured'], input_keys=['slaves'], output_keys=['last_name'], default_outcome='error_occured', outcome_map={ 'success': map_succed, 'error_occured': map_abort }) with sm_con: for name in slave_namespaces: smach.Concurrence.add( name + "/Move", smach_ros.SimpleActionState(name + '/move_base', MoveBaseAction, goal_cb=partial( self.__calcGoal__, name), input_keys=['slaves'], output_keys=['last_name'])) return sm_con
def __init__(self, in_ttl=20.0): smach.StateMachine.__init__( self, outcomes=['succeeded', 'time_out'], input_keys=['object_to_grasp', 'time_out_grasp'], output_keys=['standard_error']) with self: self.userdata.time_grasp = 0.0 self.userdata.time_first = True sm_conc = smach.Concurrence(outcomes=['succeeded', 'time_out'], default_outcome='succeeded', input_keys=[ 'object_to_grasp', 'time_grasp', 'time_first', 'time_out_grasp' ], child_termination_cb=child_term_cb, outcome_cb=out_cb) with sm_conc: #Needs to be replaced by the actual Find_and_grasp_object SM sm_conc.add( 'Find_and_grasp_object', #Find_and_grab_object(), DummyStateMachine()) sm_conc.add('Time_State', Time_State(ttl=in_ttl)) smach.StateMachine.add('GRASP_CONCURRENCE', sm_conc, transitions={ 'succeeded': 'succeeded', 'time_out': 'time_out' })
def make_return_path(shared): search_sm = smach.Concurrence( ['succeeded', 'failed', 'preempted'], default_outcome='failed', outcome_map={'succeeded': { 'WAIT': 'succeeded' }}, child_termination_cb=lambda so: True) with search_sm: smach.Concurrence.add( 'PATTERN', common_states.WaypointSeriesState(shared, [ lambda cur: cur.left(2), lambda cur: cur.forward(2), lambda cur: cur.right(4), lambda cur: cur.backward(2) ], speed=.3)) smach.Concurrence.add( 'WAIT', legacy_vision_states.WaitForObjectsState(shared, 'find2_down_camera', 'pipe')) sm = smach.StateMachine(['succeeded', 'failed', 'preempted']) with sm: smach.StateMachine.add('RETURN_WAYPOINT', common_states.ReturnToWaypointState( shared, 'last_path'), transitions={'succeeded': 'SEARCH'}) smach.StateMachine.add('SEARCH', search_sm) return sm
def GetMoveRobotSateMachine(): sm_move = smach.Concurrence( outcomes=['succeeded', 'aborted'], default_outcome='aborted', outcome_map={ 'succeeded': { 'MOVE_BASE': 'succeeded', 'MOVE_PTU': 'succeeded' } }, input_keys=['goal_robot_pose', 'goal_ptu_position']) with sm_move: smach.Concurrence.add( 'MOVE_PTU', MovePTU(), remapping={'goal_ptu_position': 'goal_ptu_position'}) if rospy.get_param("/scene_exploration_sm/UseFakeMoveBase") is True: smach.Concurrence.add( 'MOVE_BASE', FakeMoveBase(), remapping={'goal_robot_pose': 'goal_robot_pose'}) else: smach.Concurrence.add( 'MOVE_BASE', MoveBase(), remapping={'goal_robot_pose': 'goal_robot_pose'}) return sm_move
def main(): rospy.init_node('smach_example_state_machine') sm_top = smach.StateMachine(outcomes=['outcome6']) with sm_top: smach.StateMachine.add('BAS', Bas(), transitions={'outcome3': 'CON'}) sm_con = smach.Concurrence( outcomes=['outcome4', 'outcome5'], default_outcome='outcome4', outcome_map={'outcome5': { 'BAR': 'outcome1', 'FOO': 'outcome2' }}) with sm_con: smach.Concurrence.add('FOO', Foo()) smach.Concurrence.add('BAR', Bar()) smach.StateMachine.add('CON', sm_con, transitions={ 'outcome4': 'CON', 'outcome5': 'outcome6' }) outcome = sm_top.execute()
def wrap_state_force_detection(state_name, state, force_thresh, outcomes, input_keys=[]): collision_outcomes = ['collision'] total_outcomes = outcomes + collision_outcomes def child_term_cb(outcome_map): # stop execution under all circumstances return True def out_cb(outcome_map): if outcome_map['FORCE_MONITOR_' + state_name] == 'collision': return 'collision' return outcome_map[state_name] sm_coll_detect_state = smach.Concurrence( outcomes=total_outcomes, input_keys=input_keys, default_outcome='aborted', child_termination_cb=child_term_cb, outcome_cb=out_cb) with sm_coll_detect_state: smach.Concurrence.add('FORCE_MONITOR_' + state_name, ForceCollisionMonitor(force_thresh)) smach.Concurrence.add(state_name, state) return sm_coll_detect_state
def main(): def subscriber_cb(data_raw, smdata): rospy.loginfo('SUBSCRIBE VECTOR3 %s' % data_raw) smdata.x = data_raw.x smdata.y = data_raw.y smdata.z = data_raw.z # smdata.vector3 = data_raw rospy.init_node('smach_example_state_machine') sm_top = smach.StateMachine(outcomes=['out1']) with sm_top: sm = smach.Concurrence(outcomes=['succeed', 'failed'], # input_keys=['x'], # output_keys=['x', 'y', 'z'], default_outcome='failed', outcome_map={'succeed': {'LISTENER1': 'right', 'LISTENER2': 'right'}}) with sm: # Add states to the container smach.Concurrence.add('LISTENER1', Listen1(),) # remapping={'x': 'x'}) smach.Concurrence.add('LISTENER2', Listen2(),) # remapping={'x': 'x'}) smach.StateMachine.add('SM', sm, transitions={'succeed': 'RESULT', 'failed': 'SM'}) smach.StateMachine.add('RESULT', Result(), transitions={'result': 'SM'}) # sis = smach_ros.IntrospectionServer('introspection_server', sm_mother, 'INTROSPECTION SERVER') # sis.start() # Execute SMACH plan outcome = sm_top.execute() rospy.spin()
def get_nav_approach(self): def child_term_cb(outcome_map): return True def out_cb(outcome_map): if outcome_map['DETECT_FORWARD_DISTANCE'] == 'reached': return 'succeeded' return 'shutdown' sm_nav_approach_state = smach.Concurrence( outcomes=['succeeded', 'shutdown'], default_outcome='shutdown', child_termination_cb=child_term_cb, outcome_cb=out_cb, output_keys=['nav_dist']) with sm_nav_approach_state: approach_goal = ApproachGoal() approach_goal.forward_vel = 0.05 approach_goal.forward_mult = 0.50 smach.Concurrence.add( 'MOVE_FORWARD', SimpleActionState('/approach_table/move_forward_act', ApproachAction, goal=approach_goal)) smach.Concurrence.add( 'DETECT_FORWARD_DISTANCE', DetectForwardDistance(self.get_transform, self.nav_approach_dist)) return sm_nav_approach_state
def main(): sm = smach.StateMachine(outcomes=['final_outcome_a', 'final_outcome_b', 'final_outcome_c']) with sm: sm_con = smach.Concurrence(outcomes=['con_outcome_1', 'con_outcome_2', 'con_default_outcome'], default_outcome='con_default_outcome', outcome_map={'con_outcome_1': { 'FOO_0': 'outcome_b', 'FOO_1': 'outcome_b'}, 'con_outcome_2': { 'FOO_0': 'outcome_a', 'FOO_1': 'outcome_a'}}) with sm_con: smach.Concurrence.add('FOO_0', Foo('FOO_0', 'outcome_a')) smach.Concurrence.add('FOO_1', Foo('FOO_1', 'outcome_a')) smach.StateMachine.add('CON', sm_con, transitions={'con_default_outcome':'CON', 'con_outcome_1':'final_outcome_b', 'con_outcome_2':'FOO_2'}) smach.StateMachine.add('FOO_2', Foo('FOO_2', 'outcome_a'), transitions={'outcome_a':'final_outcome_a', 'outcome_b':'final_outcome_c'}) outcome = sm.execute()
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()
def build(self): # Build the autonomous state as concurrence between wiimote and measuring behaviour to allow user preemption autonomous = smach.Concurrence(outcomes=['exitAutomode'], default_outcome='exitAutomode', outcome_map={ 'exitAutomode': { 'HMI': 'preempted', 'MEASURE': 'preempted' } }, child_termination_cb=onPreempt) with autonomous: smach.Concurrence.add('HMI', wii_states.interfaceState(self.hmi)) smach.Concurrence.add('MEASURE', measure_point.build()) # Build the top level mission control from the remote control state and the autonomous state mission_control = smach.StateMachine(outcomes=['preempted']) with mission_control: smach.StateMachine.add('REMOTE_CONTROL', wii_states.remoteControlState(self.hmi), transitions={ 'enterAutomode': 'AUTO_MODE', 'preempted': 'preempted' }) smach.StateMachine.add( 'AUTO_MODE', autonomous, transitions={'exitAutomode': 'REMOTE_CONTROL'}) return mission_control
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 # Once the outcome meet the outcome_map, sm_con publish 'outcome5' and leave the con 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'}) # Create and start the introspection server sis = smach_ros.IntrospectionServer('server_name', sm_top, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm_top.execute() sis.stop()
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()
def main(): #初始化节点 rospy.init_node('smach_example_state_machine_con') # Create a SMACH state machine # 使用StateMachine创建一个状态机 # 最外层状态机 最终输出的结果 'outcome6' sm_top = smach.StateMachine(outcomes=['outcome6']) # 打开容器 with sm_top: # 添加一个状态 smach.StateMachine.add( 'BAS', Bas(), #状态 transitions={'outcome3': 'CON_SM'}) #跳转到 CON_SM子状态机 # 添加一个 子 状态机 sm_con # 创建 可并行的子状态机 同步状态机 sm_con = smach.Concurrence( outcomes=['outcome4', 'outcome5'], #子状态机两个输出 default_outcome='outcome4', #默认输出'outcome4' 循环执行该状态机 # 设置了状态机同步运行的状态跳转 outcome_map={ 'outcome5': # 另一个'outcome5' { 'FOO': 'outcome2', # 当'FOO'输出'outcome2'且'BAR'输出'outcome1' 'BAR': 'outcome1' } }) # 时 才输出 'outcome5' 跳转到 最外层状态机 with sm_con: #使用同步状态机 向子状态机 容器内 添加状态 smach.Concurrence.add('FOO', Foo()) smach.Concurrence.add('BAR', Bar()) # 添加一个并行的子状态机 smach.StateMachine.add( 'CON_SM', sm_con, transitions={ 'outcome4': 'CON_SM', # 子状态机的一个输出 默认转到自己 循环 'outcome5': 'outcome6' }) # Create and start the introspection server # 可视化状态机服务器 可是使用 rosrun smach_viewer smach_viewer.py 查看 # 第一个参数是服务器的名字,可以根据需要自由给定; # 第二个参数是所要监测的状态机; # 第三个参数代表状态机的层级,因为SMACH状态机支持嵌套,状态内部还可以有自己的状态机。 sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm_top, '/SM_ROOT') sis.start() # Execute SMACH plan # 执行状态机 outcome = sm_top.execute() # Wait for ctrl-c to stop the application # 等待状态机结束 rospy.spin() sis.stop() #停止 可视化状态机服务器
def createConcurrence(self,fg_state,outcome_cb=None,child_termination_cb=lambda x: True): # Create the sub SMACH state machine if not outcome_cb: outcome_cb = self.concurrent_outcome_cb(self,fg_state) return smach.Concurrence(outcomes=['TASK_COMPLETED','TASK_INITIALISATION_FAILED','TASK_INTERRUPTED', 'TASK_FAILED','TASK_TIMEOUT','MISSION_COMPLETED'], default_outcome='TASK_FAILED', outcome_cb = outcome_cb, child_termination_cb=child_termination_cb)
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('INIT', smach_ros.MonitorState("/sm_reset", Empty, monitor_cb), transitions={ 'invalid': 'CON', 'valid': 'INIT', 'preempted': 'INIT' }) # 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', smach_ros.MonitorState( "/sm_reset2", Empty, monitor_cb), transitions={ 'invalid': 'outcome2', 'valid': 'FOO', 'preempted': 'FOO' }) smach.Concurrence.add('BAR', Bar()) smach.StateMachine.add('CON', sm_con, transitions={ 'outcome4': 'CON', 'outcome5': 'outcome6' }) # Create and start the introspection server sis = smach_ros.IntrospectionServer('my_smach_introspection_server', sm_top, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm_top.execute() # Wait for ctrl-c to stop the application rospy.spin() sis.stop()
def __init__(self): #rospy.init_node('state_machine') # Create a SMACH state machine self.sm_top = smach.StateMachine(outcomes=['succeeded', 'error']) # get test config package_name = rospy.get_param("/atf/package_name") print("package_name:", package_name) test_generation_config_file = rospy.get_param("/atf/test_generation_config_file", "atf/test_generation_config.yaml") print("test_generation_config_file:", test_generation_config_file) test_name = rospy.get_param("/atf/test_name") print("test_name:", test_name) atf_configuration_parser = ATFConfigurationParser(package_name, test_generation_config_file) tests = atf_configuration_parser.get_tests() for test in tests: #print "test.name:", test.name if test_name == test.name: break #print "current test:", test.name outcome_map_succeeded = {} outcome_map_error = {} for testblock in list(test.testblockset_config.keys()): outcome_map_succeeded[testblock] = 'succeeded' outcome_map_error[testblock] = 'error' outcome_map = {'succeeded':outcome_map_succeeded, 'error':outcome_map_error} recorder_handle = ATFRecorder(test) # Open the container with self.sm_top: # Create the sub SMACH state machine sm_con = smach.Concurrence(outcomes=['succeeded','error'], default_outcome='error', outcome_map=outcome_map) sm_con.userdata = self.sm_top.userdata # Open the container with sm_con: # Add states to the container for testblock in list(test.testblockset_config.keys()): #print "adding testblock:", testblock smach.Concurrence.add(testblock, SmAtfTestblock(testblock, recorder_handle)) # TODO preempt all other concurrent States as soon as one state returns 'error' smach.StateMachine.add('CON', sm_con, transitions={'succeeded':'succeeded', 'error':'error'}) self.sis = smach_ros.IntrospectionServer('/state_machine/machine', self.sm_top, 'SM_ATF') self.sis.start()
def main(): rospy.init_node('smach_example_state_machine') # Create the top level SMACH state machine sm_top = smach.StateMachine(outcomes=['done', 'exit']) # Open the container with sm_top: goal1 = MoveBaseGoal() goal1.target_pose.header.frame_id = "dtw_robot1/map" goal1.target_pose.pose.position.x = 0.5 goal1.target_pose.pose.orientation.w = 1.0 smach.StateMachine.add('MOVE1', smach_ros.SimpleActionState( '/dtw_robot1/move_base', MoveBaseAction, goal=goal1), transitions={ 'succeeded': 'TASK2', 'preempted': 'done', 'aborted': 'done' }) task2_concurrence = smach.Concurrence( outcomes=['done', 'exit'], default_outcome='done', child_termination_cb=child_term_cb, outcome_cb=out_cb) with task2_concurrence: goal2 = MoveBaseGoal() goal2.target_pose.header.frame_id = "dtw_robot1/map" goal2.target_pose.pose.position.x = -0.5 goal2.target_pose.pose.orientation.w = 1.0 smach.Concurrence.add( 'MOVE2', smach_ros.SimpleActionState('/dtw_robot1/move_base', MoveBaseAction, goal=goal2)) smach.Concurrence.add( 'MONITOR2', smach_ros.MonitorState("/sm_stop", Empty, monitor_cb)) smach.StateMachine.add('TASK2', task2_concurrence, transitions={ 'done': 'done', 'exit': 'exit' }) # Execute SMACH plan sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT') sis.start() outcome = sm_top.execute() rospy.spin() sis.stop()
def __init__(self): smach.StateMachine.__init__( self, outcomes=['succeeded', 'preempted', 'aborted'], output_keys=['standard_error']) with self: self.userdata.tts_wait_before_speaking = 0 self.userdata.tts_text = None self.userdata.tts_lang = None self.userdata.standar_error = "ok" self.userdata.word_to_listen = None smach.StateMachine.add('INIT_VAR', init_var(), transitions={ 'succeeded': 'CONCURRENCE', 'aborted': 'aborted', 'preempted': 'preempted' }) sm = smach.Concurrence( outcomes=['DOOR_OPEN', 'OPERATOR', 'aborted'], default_outcome='DOOR_OPEN', input_keys=['word_to_listen'], child_termination_cb=child_term_cb, outcome_cb=out_cb) with sm: # it will finisheed with succeeded if it check a door sm.add('CHECK_DOOR', look_for_elevator_door()) # here i have to listen if they say me to get out of the lift sm.add('LISTEN_OPERATOR_FOR_EXIT', ListenWordSM_Concurrent("go out")) smach.StateMachine.add('CONCURRENCE', sm, transitions={ 'OPERATOR': 'SAY_OUT', 'DOOR_OPEN': 'SAY_OUT', 'aborted': 'CONCURRENCE' }) # it says i'm going out smach.StateMachine.add('SAY_OUT', text_to_say(SAY_OUT_FRASE), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' })
def main(): rospy.init_node('st_mach') rospy.loginfo('Inited node st_mach') global pub pub = rospy.Publisher('/drone_state', String, queue_size=10) rospy.Subscriber('/mavros/battery', BatteryState, battery_update) rospy.Subscriber('/detected_objects', PoseArray, HumFound) objects_history = [] global battery battery = None # creating the concurrence state machine flying_concurrence = smach.Concurrence( outcomes=['patrol_done', 'low_bat', 'hovering'], default_outcome='patrol_done', child_termination_cb=child_term_cb, outcome_cb=out_cb) with flying_concurrence: smach.Concurrence.add( 'START_HOVERING', smach_ros.MonitorState("/sm_hover", Empty, monitor_cb)) smach.Concurrence.add( 'RETURN_TO_BASE', smach_ros.MonitorState("/sm_charge", Empty, monitor_cb)) smach.Concurrence.add('PATROL_MODE', Flying()) smach.Concurrence.add('BATTERY_MONITOR', Bat_Monitor()) smach.Concurrence.add('OBJECT_FOUND', Camera_Search()) sm = smach.StateMachine(outcomes=['charging']) with sm: smach.StateMachine.add('BASE_CHARGING', Base(), transitions={'setup_done': 'FLYING'}) smach.StateMachine.add('FLYING', Flying(), transitions={ 'patrol_done': 'RETURN_TO_BASE', 'low_bat': 'RETURN_TO_BASE', 'hovering': 'HOVER' }) smach.StateMachine.add('RETURN_TO_BASE', Landing(), transitions={'landing_succeded': 'charging'}) smach.StateMachine.add('HOVER', Hover(), transitions={'hovering_timeout': 'FLYING'}) sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT') sis.start() sm.execute() rospy.spin() sis.stop()
def main(): rospy.init_node('psr_state_machine', anonymous=True) # Create a SMACH state machine 'sm' sm = smach.StateMachine(outcomes=['Exit']) # Local variables for sm #sm.userdata.sm_psr_data = [1] # Open the container sm with sm: # Add states to the container 'sm' smach.StateMachine.add('IDLE', Idle(), transitions={'Go':'INITIAL', 'ShutDown':'Exit'}) smach.StateMachine.add('INITIAL', Initial(), transitions={'Go':'CON', 'Cancel':'IDLE'}) # Create a sub SMACH state machine 'sm_con' sm_con = smach.Concurrence(outcomes=['Completed','Aborted'], default_outcome='Aborted', outcome_map={'Completed': { 'PSR_CLIENT':'Completed', 'DATA_COLLECT':'Completed'}}) #sm_con.userdata.sm_con_psr_data = [2] # Add states to the container 'sm_con' with sm_con: smach.Concurrence.add('PSR_CLIENT', PSR_client()) smach.Concurrence.add('DATA_COLLECT', Data_collect()) smach.StateMachine.add('CON', sm_con, transitions={'Completed':'SAVE', 'Aborted':'SAVE'}) # Add states to the container 'sm' smach.StateMachine.add('SAVE', Save_Data(), transitions={'Done':'IDLE'}) # Create and start the introspection server sis = smach_ros.IntrospectionServer('server_psr', sm, '/SM_ROOT') sis.start() # Execute SMACH plan outcome = sm.execute() # Wait for ctrl-c to stop the application rospy.spin() sis.stop()
def create_sm(): sm = smach.StateMachine(outcomes=['DONE']) sm.userdata = create_userdata() with sm: smach.StateMachine.add('LAUNCH', launch_sm.create(), transitions={ 'succeeded': 'HEALTHY', 'preempted': 'SHUTDOWN', 'failed': 'SHUTDOWN' }) safety_concurrence = smach.Concurrence( outcomes=['succeeded', 'failed', 'preempted'], default_outcome='failed', outcome_map={ 'failed': { 'SAFETY': 'invalid', 'STATE_MACHINE': 'preempted' }, 'preempted': { 'SAFETY': 'preempted', 'STATE_MACHINE': 'preempted' }, 'succeeded': { 'SAFETY': 'valid', 'STATE_MACHINE': 'succeeded' } }, child_termination_cb=lambda _: True, input_keys=['sounds'], output_keys=['sounds']) with safety_concurrence: smach.Concurrence.add('SAFETY', SafetyState()) smach.Concurrence.add('STATE_MACHINE', HealthyStateMachine()) smach.StateMachine.add('HEALTHY', safety_concurrence, transitions={ 'succeeded': 'SHUTDOWN', 'failed': 'ERROR', 'preempted': 'SHUTDOWN' }) smach.StateMachine.add('ERROR', ErrorState(), transitions={'succeeded': 'SHUTDOWN'}) smach.StateMachine.add('SHUTDOWN', ShutdownState(), transitions={'succeeded': 'DONE'}) return sm
def __init__(self, type_movement="yes", tts_text=''): smach.StateMachine.__init__( self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['tts_text', 'type_movment'], output_keys=['standard_error']) self.type_movement = type_movement self.tts_text = tts_text with self: self.userdata.tts_text = None self.userdata.type_movment = None self.userdata.tts_lang = None self.userdata.tts_wait_before_speaking = 0 self.userdata.standard_error = 'OK' self.userdata.manip_time_to_play = 30 self.userdata.skip_planning = False smach.StateMachine.add('INIT_VAR', init_var(self.type_movement, self.tts_text), transitions={ 'succeeded': 'PUT_MOVMENT', 'aborted': 'aborted', 'preempted': 'preempted' }) sm = smach.Concurrence( outcomes=['succeeded', 'preempted', 'aborted'], default_outcome='succeeded', input_keys=[ 'tts_text', 'manip_motion_to_play', 'manip_time_to_play', 'tts_wait_before_speaking', 'tts_lang', 'skip_planning' ]) self.userdata.tts_wait_before_speaking = 0 self.userdata.tts_text = None self.userdata.tts_lang = None with sm: sm.add('SAY', text_to_say()) sm.add('MOVE', play_motion_sm(skip_planning=False)) smach.StateMachine.add('PUT_MOVMENT', sm, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' })
def SynchroConcurrence(): Synchro_cc = smach.Concurrence( outcomes=["succeeded", "aborted"], default_outcome='aborted', outcome_map={"succeeded": { 'Ping': "ping", 'Pong': "pong" }}) with Synchro_cc: Synchro_cc.add('Ping', Ping()) Synchro_cc.add('Pong', Pong()) return Synchro_cc