def main(): smach_states = rospy.get_param("smach_states") # The autonomy_sm handles the full autonomy sequence for the competition run autonomy_sm = StateMachine(outcomes=['succeeded','aborted','preempted']) with autonomy_sm: # TODO: create localization state. might just be a MonitorState # Sequentially add all the states from the config file to the state machine, # where state i transitions to state i+1 names = [] for i in range(len(smach_states)): state = smach_states[i] name, action_state = create_action_state(state) # If the state name is already in the state machine, add the new # one with increasing numbers if name in names: name = name+"2" while name in names: name = name[:-1]+str(int(name[-1])+1) # increase num names.append(name) StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded']) # StateMachine.add() # Create the concurrence contained for the fully autonomy sequence. This # runs the state machine for the competition run. It also concurrently runs # a state with a timer counting down from 10 minutes and a state that listens # to the /click/start_button topic. If either of these are triggered, it will # end autonomy and place us into the teleop state. # TODO: add 10 minute competition timer state autonomy_concurrence = Concurrence(outcomes=['enter_teleop', 'stay', 'aborted'], default_outcome='enter_teleop', child_termination_cb=autonomy_child_term_cb, outcome_cb=autonomy_out_cb) with autonomy_concurrence: # state that runs full autonomy state machine Concurrence.add('AUTONOMY', autonomy_sm) # state that listens for toggle message Concurrence.add('TOGGLE_LISTEN', MonitorState('/click/start_button', Empty, monitor_cb)) # Top level state machine, containing the autonomy and teleop machines. top_sm = StateMachine(outcomes=['DONE']) with top_sm: StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb), transitions={'invalid':'AUTONOMY_MODE', 'valid':'TELEOP_MODE', 'preempted':'AUTONOMY_MODE'}) StateMachine.add('AUTONOMY_MODE', autonomy_concurrence, transitions={'enter_teleop':'TELEOP_MODE', 'stay':'AUTONOMY_MODE', 'aborted':'DONE'}) #StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb), # transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'}) sis = IntrospectionServer('smach_introspection_server', top_sm, '/COMPETITION_SMACH') sis.start() top_sm.execute() rospy.spin() sis.stop()
def __init__(self): self.find_people_ = MonitorState('follow', xm_FollowPerson, self.people_cb, max_checks =5, output_keys=['pos_xm']) self.tf_listener = tf.TransformListener()
def test_userdata(self): """Test serial manipulation of userdata.""" pinger_thread = threading.Thread(target=pinger) pinger_thread.start() init_ud = UserData() init_ud.b = 'A' sm = StateMachine(['valid', 'invalid', 'preempted']) sm.set_initial_state(['MON'], userdata=init_ud) assert 'b' in sm.userdata assert sm.userdata.b == 'A' with sm: StateMachine.add( 'MON', MonitorState('/ping', Empty, cond_cb, input_keys=['b'], output_keys=['a'])) outcome = sm.execute() assert outcome == 'invalid' assert 'b' in sm.userdata assert sm.userdata.b == 'A' assert 'a' in sm.userdata assert sm.userdata.a == 'A'
def __init__(self): self.tracker = MonitorState('/people_tracked', PersonArray, self.people_cb, max_checks=5, input_keys=['people_id'], output_keys=['pos_xm', 'people_id']) self.tf_listener = tf.TransformListener()
def __init__(self): rospy.sleep(5.0) self.people_direct = MonitorState('/hark_source', HarkSource, self.hark_cb, max_checks=30, output_keys=['turn_degree'])
def __init__(self): rospy.init_node('final_project_kl', anonymous=False) rospy.on_shutdown(self.shutdown) self.keyPointManager = KeyPointManager() # Create the nav_patrol state machine using a Concurrence container self.nav_patrol = Concurrence( outcomes=['succeeded', 'key_points', 'stop'], default_outcome='succeeded', outcome_map={'key_points': { 'MONITOR_AR': 'invalid' }}, child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine with self.nav_patrol: Concurrence.add('SM_NAV', Patrol().getSM()) Concurrence.add( 'MONITOR_AR', MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb)) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_top.userdata.sm_ar_tag = None with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'key_points': 'PATROL_KEYPOINTS', 'stop': 'STOP' }) StateMachine.add('PATROL_KEYPOINTS', PatrolThroughKeyPoints( self.keyPointManager).getSM(), transitions={'succeeded': 'STOP'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): rospy.init_node('xm_arm_ar_tags') rospy.on_shutdown(self.shutdown) # design the main part of the test snippet # # self.arm_concurrence = StateMachine(outcomes =['succeeded','aborted','preempted']) # # TODO:add the speech-recognize function with the pocketsphinx package # this will download from the source in the kinetic # http://blog.csdn.net/x_r_su/article/details/53022746?locationNum=1&fps=1 # with self.arm_concurrence: self.sm_ArTags = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_ArTags: #TODO:here we need to make the topic only return when the ar_tags is detected # and we should do that:the libfreenectgrabber to publish the PointClouds data real-time # but make the Octomap only generate for a certain time self.sm_ArTags.userdata.object_position = PointStamped() StateMachine.add('GETPOISITION', MonitorState('ar_pose_marker', AlvarMarkers, self.position_cb, max_checks=1, output_keys=['obj_pos']), transitions={ 'valid': 'PICK', 'invalid': 'aborted', 'preempted': 'aborted' }, remapping={'obj_pos': 'object_position'}) StateMachine.add('PICK', ArmGo(), transitions={ 'succeeded': 'WAIT', 'aborted': 'GETPOISITION', 'preempted': 'aborted' }, remapping={'obj_pos': 'object_position'}) self.sm_ArTags.userdata.rec = 2.0 StateMachine.add('WAIT', Wait(), remapping={'rec': 'rec'}, transitions={'succeeded': 'GETPOISITION'}) outcome = self.sm_ArTags.execute()
def __init__(self): rospy.init_node('monitor_fake_battery', anonymous=False) rospy.on_shutdown(self.shutdown) # Set the low battery threshold (between 0 and 100) self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50) # Initialize the state machine sm_battery_monitor = StateMachine(outcomes=[]) with sm_battery_monitor: # Add a MonitorState to subscribe to the battery level topic StateMachine.add( 'MONITOR_BATTERY', MonitorState('battery_level', Float32, self.battery_cb), transitions={ 'invalid': 'RECHARGE_BATTERY', 'valid': 'MONITOR_BATTERY', 'preempted': 'MONITOR_BATTERY' }, ) # Add a ServiceState to simulate a recharge using the set_battery_level service StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, request=100), transitions={ 'succeeded': 'MONITOR_BATTERY', 'aborted': 'MONITOR_BATTERY', 'preempted': 'MONITOR_BATTERY' }) # Create and start the SMACH introspection server intro_server = IntrospectionServer('monitor_battery', sm_battery_monitor, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = sm_battery_monitor.execute() intro_server.stop()
def __init__(self): Concurrence.__init__( self, outcomes=['exit', 'restart', 'aborted', 'preempted'], default_outcome='restart', output_keys=['control_infos'], child_termination_cb=self.child_termination_cb, outcome_cb=self.outcome_cb) self.userdata.control_infos = {} with self: Concurrence.add( 'CHANGE_CONTROL_MONITOR', MonitorState("/change_control", String, self.change_control_cb, input_keys=['control_infos'], output_keys=['control_infos'])) Concurrence.add('TIMER', Timer()) Concurrence.add('CONTROL', Control())
def __init__(self): rospy.init_node('xm_arm_ar') rospy.logwarn('Come on! Let us go now!') self.arm_stack_service = rospy.Service('arm_stack',xm_PickOrPlace,self.callback) self.sm_ArTags =StateMachine(outcomes =['succeeded','aborted','error']) with self.sm_ArTags: self.sm_ArTags.userdata.arm_ps = PointStamped() StateMachine.add('GETPOISITION', MonitorState('ar_pose_marker',AlvarMarkers,self.position_cb,max_checks=1,output_keys =['obj_pos']), transitions={'valid':'PICK','invalid':'aborted','preempted':'aborted'}, remapping={'obj_pos':'arm_ps'}) self.sm_ArTags.userdata.arm_mode_1 =1 StateMachine.add('PICK', ArmGo(), transitions={'succeeded':'PLACE','aborted':'GETPOISITION','error':'error'}, remapping={'arm_ps':'arm_ps','arm_mode':'arm_mode_1'}) self.sm_ArTags.userdata.arm_mode_2 = 2 StateMachine.add('PLACE',ArmGo(), transitions={"succeeded":'succeeded','aborted':'aborted','error':'error'}, remapping={'arm_ps':'arm_ps','arm_mode':'arm_mode_2'}) outcome = self.sm_ArTags.execute()
def __init__(self): rospy.init_node('smach_home_status', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) ##################################### # JO IS AWAKE ##################################### # State machine for Jo-awake-go-sleep self.sm_jo_awake_sleep = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_awake_sleep: StateMachine.add('LOOK_WAKE', MonitorState("/JO/sleep", Empty, self.empty_cb), transitions={ 'valid': 'GOING_SLEEP', 'preempted': 'preempted', 'invalid': 'GOING_SLEEP' }) StateMachine.add('GOING_SLEEP', JoGoingSleep(), transitions={'succeeded': 'LOOK_IN_BED'}) StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={ 'valid': 'IN_BED', 'preempted': 'preempted', 'invalid': 'IN_BED' }) StateMachine.add('IN_BED', JoInBed(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-awake-bothgo-sleep self.sm_jo_awake_bothsleep = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_awake_bothsleep: StateMachine.add('LOOK_WAKE', MonitorState("/BOTH/sleep", Empty, self.empty_cb), transitions={ 'valid': 'GOING_SLEEP', 'preempted': 'preempted', 'invalid': 'GOING_SLEEP' }) StateMachine.add('GOING_SLEEP', BothGoingSleep(), transitions={'succeeded': 'LOOK_IN_BED'}) StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={ 'valid': 'IN_BED', 'preempted': 'preempted', 'invalid': 'IN_BED' }) StateMachine.add('IN_BED', BothInBed(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-awake-go-out self.sm_jo_awake_out = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_awake_out: StateMachine.add('LOOK_OUT', MonitorState("/JO/go_out", Empty, self.empty_cb), transitions={ 'valid': 'PAUSE', 'preempted': 'preempted', 'invalid': 'PAUSE' }) StateMachine.add('PAUSE', Pause(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-awake self.sm_jo_awake = Concurrence( outcomes=['succeeded', 'stop', 'go_sleep', 'go_out'], default_outcome='succeeded', child_termination_cb=self.jo_awake_child_termination_cb, outcome_cb=self.jo_awake_outcome_cb) with self.sm_jo_awake: Concurrence.add('SM_GO_TO_SLEEP', self.sm_jo_awake_sleep) Concurrence.add('SM_BOTH_GO_TO_SLEEP', self.sm_jo_awake_bothsleep) Concurrence.add('SM_GO_OUT', self.sm_jo_awake_out) ##################################### # JO IS SLEEPING ##################################### # State machine for Jo-sleep-waking self.sm_jo_sleep_waking = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_sleep_waking: StateMachine.add('WAIT_WAKE_UP', MonitorState("/JO/wake_up", Empty, self.empty_cb), transitions={ 'valid': 'WAKING_UP', 'preempted': 'preempted', 'invalid': 'WAKING_UP' }) StateMachine.add('WAKING_UP', JoWakingUp(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-sleep-bothwaking self.sm_jo_sleep_bothwaking = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_sleep_bothwaking: StateMachine.add('WAIT_WAKE_UP', MonitorState("/BOTH/wake_up", Empty, self.empty_cb), transitions={ 'valid': 'WAKING_UP', 'preempted': 'preempted', 'invalid': 'WAKING_UP' }) StateMachine.add('WAKING_UP', BothWakingUp(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-awake self.sm_jo_sleep = Concurrence( outcomes=['succeeded', 'aborted', 'preempted', 'wake_up'], default_outcome='succeeded', child_termination_cb=self.jo_sleep_child_termination_cb, outcome_cb=self.jo_sleep_outcome_cb) with self.sm_jo_sleep: Concurrence.add('SM_WAKE_UP', self.sm_jo_sleep_waking) Concurrence.add('SM_BOTH_WAKE_UP', self.sm_jo_sleep_bothwaking) ##################################### # JO IS OUT TODO ##################################### # State machine for Jo-out-back self.sm_jo_out_back = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_out_back: StateMachine.add('WAIT_BACK_HOME', MonitorState("/JO/back_home", Empty, self.empty_cb), transitions={ 'valid': 'WAIT_MYO', 'preempted': 'preempted', 'invalid': 'WAIT_MYO' }) StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={ 'valid': 'COMING_BACK', 'preempted': 'preempted', 'invalid': 'COMING_BACK' }) StateMachine.add('COMING_BACK', JoBackHome(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-out-bothback self.sm_jo_out_bothback = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo_out_bothback: StateMachine.add('WAIT_BACK_HOME', MonitorState("/BOTH/back_home", Empty, self.empty_cb), transitions={ 'valid': 'WAIT_MYO', 'preempted': 'preempted', 'invalid': 'WAIT_MYO' }) StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={ 'valid': 'COMING_BACK', 'preempted': 'preempted', 'invalid': 'COMING_BACK' }) StateMachine.add('COMING_BACK', BothBackHome(), transitions={'succeeded': 'succeeded'}) # State machine for Jo-out self.sm_jo_out = Concurrence( outcomes=['succeeded', 'aborted', 'preempted', 'back_home'], default_outcome='succeeded', child_termination_cb=self.jo_out_child_termination_cb, outcome_cb=self.jo_out_outcome_cb) with self.sm_jo_out: Concurrence.add('SM_BACK_HOME', self.sm_jo_out_back) Concurrence.add('SM_BOTH_BACK_HOME', self.sm_jo_out_bothback) ##################################### # TOP LVL JO SM ##################################### # State machine for JO self.sm_jo = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_jo: StateMachine.add('AWAKE', self.sm_jo_awake, transitions={ 'succeeded': 'succeeded', 'stop': 'aborted', 'go_sleep': 'SLEEP', 'go_out': 'OUT' }) StateMachine.add('SLEEP', self.sm_jo_sleep, transitions={ 'succeeded': 'succeeded', 'wake_up': 'AWAKE' }) StateMachine.add('OUT', self.sm_jo_out, transitions={ 'succeeded': 'succeeded', 'back_home': 'AWAKE' }) ##################################### # TOP LVL CAROLE SM TODO ##################################### # State machine for CAROLE self.sm_carole = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_carole: StateMachine.add('WAIT3', MonitorState("/TEST/wait3", Empty, self.empty_cb), transitions={ 'valid': 'PAUSE', 'preempted': 'preempted', 'invalid': 'PAUSE' }) StateMachine.add('PAUSE', Pause(), transitions={ 'succeeded': 'WAIT3', 'aborted': 'aborted' }) ##################################### # TOP LVL EAT SM TODO ##################################### # State machine for EAT self.sm_eat = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_eat: StateMachine.add('WAIT2', MonitorState("/TEST/wait2", Empty, self.empty_cb), transitions={ 'valid': 'PAUSE', 'preempted': 'preempted', 'invalid': 'PAUSE' }) StateMachine.add('PAUSE', Pause(), transitions={ 'succeeded': 'WAIT2', 'aborted': 'aborted' }) ##################################### # TOP LVL SHOWER SM ##################################### # State machine for SHOWER self.sm_shower = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_shower: StateMachine.add('WAIT_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb), transitions={ 'valid': 'PREPARING_SHOWER', 'preempted': 'preempted', 'invalid': 'PREPARING_SHOWER' }) StateMachine.add('PREPARING_SHOWER', PreparingShower(), transitions={ 'succeeded': 'GO_SHOWER', 'aborted': 'WAIT1' }) StateMachine.add('GO_SHOWER', GoShower(), transitions={ 'succeeded': 'STOP_SHOWER', 'aborted': 'aborted' }) StateMachine.add('STOP_SHOWER', StopShower(), transitions={ 'succeeded': 'WAIT1', 'aborted': 'aborted' }) ##################################### # TOP LVL SM ##################################### # Create the top level state machine self.sm_top = Concurrence( outcomes=['succeeded', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: Concurrence.add('SM_JO', self.sm_jo) Concurrence.add('SM_CAROLE', self.sm_carole) Concurrence.add('SM_EAT', self.sm_eat) Concurrence.add('SM_SHOWER', self.sm_shower) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): rospy.init_node('patrol_task_concurrence', anonymous=False) # 设置关闭机器人函数(stop the robot) rospy.on_shutdown(self.shutdown) # 初始化voice_id self.voice_id_data = -1 # 初始化一些参数和变量 setup_task_environment(self) # 跟踪到达目标位置的成功率 self.n_succeeded, self.n_aborted, self.n_preempted = 0, 0, 0 # 保存上一个或者当前的导航目标点的变量 self.last_nav_state = None # 指示是否正在充电的标志 self.recharging = False # 保存导航目标点的列表 nav_states = {} # 把waypoints变成状态机的状态 for waypoint in self.room_locations.iterkeys(): nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.room_locations[waypoint] move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(180.0), server_wait_timeout=rospy.Duration(10.0)) # nav_states.append(move_base_state) nav_states[waypoint] = move_base_state # 为扩展底座(docking station)创建一个MoveBaseAction state nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(180.0), server_wait_timeout=rospy.Duration(10.0)) # 为语音指令子任务创建一个状态机 sm_voice_command = StateMachine(outcomes=[ 'livingroom_id', 'bedroom_id', 'kitchen_id', 'balcony_id', 'None_id' ]) # 然后添加子任务 with sm_voice_command: StateMachine.add('RECOGNIZE_VOICE_COMMAND', Voice_Command(self.voice_id_data, 5), transitions={ 'livingroom_id': '', 'bedroom_id': '', 'kitchen_id': '', 'balcony_id': '', 'None_id': '' }) # 为等待任务创建一个状态机 sm_waite_command = StateMachine(outcomes=['waite', 'preempted']) # 然后添加子任务 with sm_waite_command: StateMachine.add('WAITE_VOICE_COMMAND', None_Command('waite_voice_command', 5), transitions={ 'waite': '', 'preempted': '' }) # 为living_room子任务创建一个状态机 sm_living_room = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_living_room: StateMachine.add('CLEAN_LIVING_ROOM', LivingRoom('living_room', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 为bedroom子任务创建一个状态机 sm_bedroom = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_bedroom: StateMachine.add('CLEAN_BEDROOM', Bedroom('bedroom', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 为kitchen子任务创建一个状态机 sm_kitchen = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_kitchen: StateMachine.add('CLEAN_KITCHEN', Kitchen('kitchen', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 为balcony子任务创建一个状态机 sm_balcony = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_balcony: StateMachine.add('CLEAN_BALCONY', Balcony('balcony', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 初始化导航的状态机 self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 使用transitions将导航的状态添加到状态机 with self.sm_nav: StateMachine.add('VOICE_COMMAND', sm_voice_command, transitions={ 'livingroom_id': 'CLEAN_LIVING_ROOM', 'bedroom_id': 'CLEAN_BEDROOM', 'kitchen_id': 'CLEAN_KITCHEN', 'balcony_id': 'CLEAN_BALCONY', 'None_id': 'WAITE_VOICE_COMMAND' }) ''' Add the living room subtask(s) ''' StateMachine.add('CLEAN_LIVING_ROOM', nav_states['living_room'], transitions={ 'succeeded': 'CLEAN_LIVING_ROOM_TASKS', 'aborted': '', 'preempted': '' }) # 当任务完成时, 继续进行kitchen任务 StateMachine.add('CLEAN_LIVING_ROOM_TASKS', sm_living_room, transitions={ 'succeeded': 'VOICE_COMMAND', 'aborted': '', 'preempted': '' }) ''' Add the kitchen subtask(s) ''' StateMachine.add('CLEAN_BEDROOM', nav_states['bedroom'], transitions={ 'succeeded': 'CLEAN_BEDROOM_TASKS', 'aborted': '', 'preempted': '' }) # 当任务完成时, 继续进行bathroom任务 StateMachine.add('CLEAN_BEDROOM_TASKS', sm_bedroom, transitions={ 'succeeded': 'VOICE_COMMAND', 'aborted': '', 'preempted': '' }) ''' Add the bathroom subtask(s) ''' StateMachine.add('CLEAN_KITCHEN', nav_states['kitchen'], transitions={ 'succeeded': 'CLEAN_KITCHEN_TASKS', 'aborted': '', 'preempted': '' }) # 当任务完成时, 继续进行hallway任务 StateMachine.add('CLEAN_KITCHEN_TASKS', sm_kitchen, transitions={ 'succeeded': 'VOICE_COMMAND', 'aborted': '', 'preempted': '' }) ''' Add the balcony subtask(s) ''' StateMachine.add('CLEAN_BALCONY', nav_states['balcony'], transitions={ 'succeeded': 'CLEAN_BALCONY_TASKS', 'aborted': '', 'preempted': '' }) # 当任务完成时, stop StateMachine.add('CLEAN_BALCONY_TASKS', sm_balcony, transitions={ 'succeeded': 'VOICE_COMMAND', 'aborted': '', 'preempted': '' }) ''' Add the balcony subtask(s) ''' # 当没有识别到语音指令时,等待 StateMachine.add('WAITE_VOICE_COMMAND', sm_waite_command, transitions={ 'waite': 'VOICE_COMMAND', 'preempted': '' }) # 在sm_nav状态机中注册一个回调函数以启动状态转换(state transitions) self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # 初始化充电的状态机 self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) # 使用并发容器(Concurrence container)创建nav_patrol状态机 self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # 将sm_nav machine和battery MonitorState添加到nav_patrol状态机里面 with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add( 'MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) # 创建顶层状态机 self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted', 'waite']) # 将nav_patrol,sm_recharge和Stop添加到sm_top状态机 with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # 创建并开始SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # 运行状态机 sm_outcome = self.sm_top.execute()
def __init__(self): self.door_detect_ = MonitorState('DoorState', Bool, self.door_cb, max_checks=1)
def __init__(self): rospy.on_shutdown(self.ShutdownCallback) # Subscribe to message to cancel missions self.__cancel_sub = rospy.Subscriber('/missions/mission_cancel', Empty, self.CancelCallback) # Create an instance of the missions helper class self.__missions_helper = MissionsHelper() # ------------------------- Top level state machine ------------------------- # Create top level state machine self.__sm = StateMachine(outcomes=['preempted','aborted'], output_keys=['mission_data']) with self.__sm: # Add a state which monitors for a mission to run StateMachine.add('WAITING', MonitorState('/missions/mission_request', String, self.MissionRequestCB, output_keys = ['mission']), transitions={'valid':'WAITING', 'invalid':'PREPARE', 'preempted':'preempted'}) # Add state to prepare the mission StateMachine.add('PREPARE', Prepare(self.__missions_helper), transitions={'mission1':'MISSION1','mission2':'MISSION2','mission4':'MISSION4', 'done_task':'WAITING','head_default':'DEFAULT_HEAD_POSITION', 'move_head':'MOVE_HEAD'}) # Add the reporting state StateMachine.add('REPORT', Report(), transitions={'success':'DEFAULT_HEAD_POSITION'}) # Set up action goal for deafult head position default_position_pan, default_position_tilt = self.__missions_helper.CameraDefaultPos() head_goal = point_headGoal() head_goal.absolute = True head_goal.pan = default_position_pan head_goal.tilt = default_position_tilt # Add the default camera position state. Which moves the head to the default position StateMachine.add('DEFAULT_HEAD_POSITION', SimpleActionState('head_control_node', point_headAction, result_cb = self.__missions_helper.CameraAtDefaultPos, goal = head_goal), transitions={'succeeded':'WAITING','preempted':'WAITING','aborted':'aborted'}) # Add the move head state StateMachine.add('MOVE_HEAD', SimpleActionState('head_control_node', point_headAction, goal_slots=['absolute', 'pan', 'tilt']), transitions={'succeeded':'WAITING', 'preempted':'WAITING', 'aborted':'aborted'}, remapping={'absolute':'user_data_absolute', 'pan':'user_data_pan', 'tilt':'user_data_tilt'}) # ------------------------- Sub State machine for mission 1 --------------------- # Create a sub state machine for mission 1 - take a message to self.__sm_mission1 = missions_lib.Mission1StateMachine(self.__missions_helper) # Now add the sub state machine for mission 1 to the top level one StateMachine.add('MISSION1', self.__sm_mission1, transitions={'complete':'REPORT','preempted':'REPORT','aborted':'REPORT'}) # ------------------------- Sub State machine for mission 2 --------------------- # Create a sub state machine for mission 2 - face detection and greeting self.__sm_mission2 = missions_lib.Mission2StateMachine(self.__missions_helper) # Now add the sub state machine for mission 2 to the top level one StateMachine.add('MISSION2', self.__sm_mission2, transitions={'complete':'REPORT','preempted':'REPORT','aborted':'aborted'}) # ------------------------- Sub State machine for mission 4 --------------------- # Create a sub state machine for mission 4 - Go Home self.__sm_mission4 = missions_lib.Mission4StateMachine(self.__missions_helper) # Now add the sub state machine for mission 4 to the top level one StateMachine.add('MISSION4', self.__sm_mission4, transitions={'complete':'REPORT','preempted':'REPORT','aborted':'REPORT'}) # ------------------------------------------------------------------------------- # Create and start the introspective server so that we can use smach_viewer sis = IntrospectionServer('server_name', self.__sm, '/SM_ROOT') sis.start() self.__sm.execute() # Wait for ctrl-c to stop application rospy.spin() sis.stop()
def __init__(self): rospy.init_node('patrol_smach_concurrence', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # A variable to hold the last/current navigation goal self.last_nav_state = None # A flag to indicate whether or not we are rechargin self.recharging = False # A list to hold then navigation goa nav_states = list() # Turn the waypoints into SMACH states for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) # Create a MoveBaseAction state for the docking station nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(10.0)) # Initialize the navigation state machine self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add the nav states to the state machine with the appropriate transitions with self.sm_nav: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'}) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'}) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':''}) # Register a callback function to fire on state transitions within the sm_nav state machine self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # Initialize the recharge state machine self.sm_recharge = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded':'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded':''}) # Create the nav_patrol state machine using a Concurrence container self.nav_patrol = Concurrence(outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'recharge':'RECHARGE', 'stop':'STOP'}) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): rospy.init_node('patrol_smach_concurrence', anonymous=False) # 设置关闭机器人函数(stop the robot) rospy.on_shutdown(self.shutdown) # 初始化一些参数和变量 setup_task_environment(self) # 跟踪到达目标位置的成功率 self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # 保存上一个或者当前的导航目标点的变量 self.last_nav_state = None # 指示是否正在充电的标志 self.recharging = False # 保存导航目标点的列表 nav_states = list() # 把waypoints变成状态机的状态 for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) # 为扩展底座(docking station)创建一个MoveBaseAction state nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(10.0)) # 初始化导航的状态机 self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 使用transitions将导航的状态添加到状态机 with self.sm_nav: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={ 'succeeded': 'NAV_STATE_1', 'aborted': 'NAV_STATE_1' }) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={ 'succeeded': 'NAV_STATE_2', 'aborted': 'NAV_STATE_2' }) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={ 'succeeded': 'NAV_STATE_3', 'aborted': 'NAV_STATE_3' }) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={ 'succeeded': 'NAV_STATE_4', 'aborted': 'NAV_STATE_4' }) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={ 'succeeded': '', 'aborted': '' }) # 在sm_nav状态机中注册一个回调函数以启动状态转换(state transitions) self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # 初始化充电的状态机 self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) # 使用并发容器(Concurrence container)创建nav_patrol状态机 self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # 将sm_nav machine和battery MonitorState添加到nav_patrol状态机里面 with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add( 'MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) # 创建顶层状态机 self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 将nav_patrol,sm_recharge和Stop添加到sm_top状态机 with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # 创建并开始SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # 运行状态机 sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): rospy.init_node('operating_mode', anonymous=False) global mb mb = MongoBridge() # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create the top level SMACH state machine self.sm_top = StateMachine(outcomes=['stop']) # Open the container with self.sm_top: # ===== SETUP State ===== StateMachine.add('SETUP', Setup(), transitions={ 'succeeded': 'TOP_CONTROL', 'preempted': 'stop', 'aborted': 'stop' }) StateMachine.add('RECHARGE', Recharge(), transitions={ 'succeeded': 'SETUP', 'preempted': 'stop', 'aborted': 'stop' }) # ===== TOP_CONTROL State ===== self.sm_battery_concurrence = Concurrence( outcomes=[ 'restart', 'recharge', 'preempted', 'aborted', 'stop' ], default_outcome='recharge', child_termination_cb=self.child_termination_cb, outcome_cb=self.outcome_cb) self.sm_battery_concurrence.userdata.mode_change_infos = { 'mode': None, 'change_mode': False } # Open the container with self.sm_battery_concurrence: # Add states to the container Concurrence.add( 'BATTERY_MONITOR', MonitorState("/battery_level", Float32, self.battery_monitor_cb)) Concurrence.add( 'CHANGE_MODE_MONITOR', MonitorState("/change_mode", String, self.change_mode_monitor_cb, input_keys=['mode_change_infos'], output_keys=['mode_change_infos'])) self.operating_mode = StateMachine( outcomes=['aborted', 'preempted', 'standby'], input_keys=['mode_change_infos']) with self.operating_mode: StateMachine.add( 'MODE_MANAGEMENT', OperatingMode(), transitions={'succeeded': 'MODE_MANAGEMENT'}) Concurrence.add('OPERATING_MODE', self.operating_mode) StateMachine.add('TOP_CONTROL', self.sm_battery_concurrence, transitions={ 'restart': 'TOP_CONTROL', 'recharge': 'RECHARGE', 'aborted': 'stop', 'preempted': 'stop' }) # Create and start the SMACH introspection server intro_server = IntrospectionServer('mode', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def main(): # The autonomy_sm handles the full autonomy sequence for the competition run autonomy_sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with autonomy_sm: # Add states for setup (these run once each) for i in range(len(setup_sequence)): name, action_type = get_name_and_type(setup_sequence[i]) func_to_run = None if action_type == 'localize': func_to_run = localization_cb elif action_type == 'move': # action instead of function func_to_run = 'action' else: raise NotImplementedError('Only localize is supported rn') # if this is the last setup state if i == len(loop_sequence) - 1: # tie the last setup state to the first loop state first_loop_name = get_name_and_type(loop_sequence[0]) if func_to_run == 'action': action_state = create_action_state(name, action_type) StateMachine.add( name, action_state, transitions={'succeeded': first_loop_name}) else: StateMachine.add(name, CBState(func_to_run), {'succeeded': first_loop_name}) else: # add_auto adds this to the state machine and automatically # ties the next and previous states together if func_to_run == 'action': action_state = create_action_state(name, action_type) StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded']) else: StateMachine.add_auto(name, CBState(func_to_run), connector_outcomes=['succeeded']) names = [] # store names to check and handle dupes for i in range(len(loop_sequence)): name, action_type = get_name_and_type(loop_sequence[i]) action_state = create_action_state(name, action_type) # "move" states all have the same name and smach requires unique # state names, so check and add a number to the name if needed if name in names: name = name + "2" while name in names: name = name[:-1] + str( int(name[-1]) + 1) # increase num by 1 names.append(name) if i == len(loop_sequence) - 1: # tie the last state to the first one, so they just keep looping StateMachine.add(name, action_state, transitions={'succeeded': names[0]}) else: # add_auto adds this to the state machine and automatically # ties the next and previous states together StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded']) # Create the concurrence container for the fully autonomy sequence. This # runs the state machine for the competition run. It also concurrently runs # a state that listens to the /click_start_button topic. If this is # triggered place us into the teleop state. autonomy_concurrence = Concurrence( outcomes=['enter_teleop', 'stay', 'aborted'], default_outcome='enter_teleop', child_termination_cb=autonomy_child_term_cb, outcome_cb=autonomy_out_cb) with autonomy_concurrence: # state that runs full autonomy state machine Concurrence.add('AUTONOMY', autonomy_sm) # state that listens for toggle message Concurrence.add( 'TOGGLE_LISTEN', MonitorState('/click_start_button', Empty, start_btn_cb)) teleop_concurrence = Concurrence( outcomes=['enter_autonomy', 'stay', 'done'], default_outcome='enter_autonomy', child_termination_cb=teleop_child_term_cb, outcome_cb=teleop_out_cb) with teleop_concurrence: Concurrence.add( 'TOGGLE_LISTEN', MonitorState('/click_start_button', Empty, start_btn_cb)) Concurrence.add( 'EXIT_LISTEN', MonitorState('/click_select_button', Empty, select_btn_cb)) # Top level state machine, containing the autonomy and teleop machines. top_sm = StateMachine(outcomes=['DONE']) with top_sm: StateMachine.add('TELEOP_MODE', teleop_concurrence, transitions={ 'enter_autonomy': 'AUTONOMY_MODE', 'stay': 'TELEOP_MODE', 'done': 'DONE' }) StateMachine.add('AUTONOMY_MODE', autonomy_concurrence, transitions={ 'enter_teleop': 'TELEOP_MODE', 'stay': 'AUTONOMY_MODE', 'aborted': 'DONE' }) #StateMachine.add('TELEOP_MODE', MonitorState('/click_start_button', Empty, monitor_cb), # transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'}) sis = IntrospectionServer('smach_introspection_server', top_sm, '/COMPETITION_SMACH') sis.start() top_sm.execute() rospy.spin() sis.stop()
def main(): rospy.init_node('smach_usecase_step_06') # Construct static goals polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0) polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5) # Create a SMACH state machine sm0 = StateMachine(outcomes=['succeeded','aborted','preempted']) # Open the container with sm0: # Reset turtlesim StateMachine.add('RESET', ServiceState('reset', std_srvs.srv.Empty), {'succeeded':'SPAWN'}) # Create a second turtle StateMachine.add('SPAWN', ServiceState('spawn', turtlesim.srv.Spawn, request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')), {'succeeded':'TELEPORT1'}) # Teleport turtle 1 StateMachine.add('TELEPORT1', ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute, request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)), {'succeeded':'TELEPORT2'}) # Teleport turtle 2 StateMachine.add('TELEPORT2', ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute, request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)), {'succeeded':'DRAW_SHAPES'}) # Draw some polygons shapes_cc = Concurrence( outcomes=['succeeded','aborted','preempted'], default_outcome='aborted', outcome_map = {'succeeded':{'BIG':'succeeded','SMALL':'succeeded'}}) StateMachine.add('DRAW_SHAPES',shapes_cc) with shapes_cc: # Draw a large polygon with the first turtle Concurrence.add('BIG', SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction, goal = polygon_big)) # Draw a small polygon with the second turtle draw_monitor_cc = Concurrence( ['succeeded','aborted','preempted'], 'aborted', child_termination_cb = lambda so: True, outcome_map = { 'succeeded':{'DRAW':'succeeded'}, 'preempted':{'DRAW':'preempted','MONITOR':'preempted'}, 'aborted':{'MONITOR':'invalid'}}) Concurrence.add('SMALL',draw_monitor_cc) with draw_monitor_cc: Concurrence.add('DRAW', SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction, goal = polygon_small)) def turtle_far_away(ud, msg): """Returns True while turtle pose in msg is at least 1 unit away from (9,5)""" if sqrt(pow(msg.x-9.0,2) + pow(msg.y-5.0,2)) > 2.0: return True return False Concurrence.add('MONITOR', MonitorState('/turtle1/pose',turtlesim.msg.Pose, cond_cb = turtle_far_away)) # Attach a SMACH introspection server sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE') sis.start() # Set preempt handler smach_ros.set_preempt_handler(sm0) # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target = sm0.execute) smach_thread.start() # Signal handler rospy.spin()
def __init__(self): rospy.init_node('HOME_automation_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi / 2, pi, 3 * pi / 2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append( Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append( Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append( Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # State machine for light entry self.sm_light_entry = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_light_entry.userdata.day_mode = 1 with self.sm_light_entry: StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb), transitions={ 'valid': 'LOOK_ENTRY', 'invalid': 'LIGHT_UP' }) StateMachine.add('LIGHT_UP', LightEntry(), transitions={'succeeded': 'succeeded'}) # State machine for dark entry self.sm_dark_entry = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_dark_entry.userdata.day_mode = 1 with self.sm_dark_entry: StateMachine.add('LOOK_ENTRY_OFF', MonitorState("/HOME/entry_noOne", Empty, self.empty_cb), transitions={ 'valid': 'LOOK_ENTRY_OFF', 'invalid': 'LIGHT_DOWN' }) StateMachine.add('LIGHT_DOWN', DarkEntry(), transitions={'succeeded': 'succeeded'}) # State machine for day mode self.sm_day_mode = Concurrence( outcomes=[ 'succeeded', 'aborted', 'preempted', 'go_shower', 'go_sleep', 'go_eat', 'go_out' ], default_outcome='succeeded', child_termination_cb=self.daymode_child_termination_cb, outcome_cb=self.daymode_outcome_cb) self.sm_day_mode.userdata.day_mode = 1 with self.sm_day_mode: Concurrence.add( 'LOOK_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb)) Concurrence.add( 'LOOK_LEAVING', MonitorState("/HOME/leaving_home", Empty, self.empty_cb)) Concurrence.add( 'LOOK_SLEEP', MonitorState("/HOME/go_sleep", Empty, self.empty_cb)) Concurrence.add('LOOK_EAT', MonitorState("/HOME/go_eat", Empty, self.empty_cb)) Concurrence.add('LOOK_ENTRY', self.sm_light_entry) Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry) # State machine for leaving home self.sm_leaving_home = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_leaving_home: StateMachine.add('LEAV', Pause(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) # State machine for going to sleep self.sm_going_sleep = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_going_sleep: StateMachine.add('GOING_SLEEP', GoingSleep(), transitions={'succeeded': 'succeeded'}) # State machine for day mode self.sm_wait_bed = Concurrence( outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='succeeded', child_termination_cb=self.useless_child_termination_cb, outcome_cb=self.useless_outcome_cb) with self.sm_wait_bed: Concurrence.add( 'LOOK_INBED', MonitorState("/METAWATCH/button2", Empty, self.empty_cb)) Concurrence.add('TIMEOUT', TimeoutToBed()) # State machine for in bed self.sm_in_bed = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_in_bed: StateMachine.add('IN_BED', InBed(), transitions={'succeeded': 'succeeded'}) # State machine for light entry self.sm_lightn_entry = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_lightn_entry.userdata.day_mode = 0 with self.sm_lightn_entry: StateMachine.add('LOOK_ENTRY', MonitorState("/HOME/entry_move", Empty, self.empty_cb), transitions={ 'valid': 'LOOK_ENTRY', 'invalid': 'LIGHT_UP' }) StateMachine.add('LIGHT_UP', LightEntry(), transitions={'succeeded': 'succeeded'}) # State machine for night mode self.sm_night_mode = Concurrence( outcomes=['succeeded', 'aborted', 'preempted', 'wake_up'], default_outcome='succeeded', child_termination_cb=self.nightmode_child_termination_cb, outcome_cb=self.nightmode_outcome_cb) self.sm_night_mode.userdata.day_mode = 0 with self.sm_night_mode: Concurrence.add( 'LOOK_WAKE', MonitorState("/HOME/wake_up", Empty, self.empty_cb)) Concurrence.add('LOOK_ENTRY', self.sm_lightn_entry) Concurrence.add('LOOK_ENTRY_OFF', self.sm_dark_entry) # State machine for night mode #self.sm_night_mode = StateMachine(outcomes=['succeeded','aborted','preempted']) #self.sm_night_mode.userdata.day_mode = 0; #with self.sm_night_mode: # StateMachine.add('NIGHT_MOD', Pause(), # transitions={'succeeded':'succeeded', # 'aborted':'aborted'}) # State machine for waking up self.sm_waking_up = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_waking_up.userdata.day_mode = 0 with self.sm_waking_up: StateMachine.add('WAKING_UP', WakingUp(), transitions={'succeeded': 'succeeded'}) # State machine for waking up self.sm_going_eat = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_going_eat.userdata.day_mode = 1 with self.sm_going_eat: StateMachine.add('EATTTTTTTT', Pause(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) # State machine for home self.sm_home = StateMachine( outcomes=['succeeded', 'aborted', 'preempted', 'going_out']) with self.sm_home: StateMachine.add('DAY_MODE', self.sm_day_mode, transitions={ 'succeeded': 'DAY_MODE', 'go_shower': 'PREPARING_SHOWER', 'go_sleep': 'GOING_SLEEP', 'go_eat': 'GOING_EAT', 'go_out': 'LEAVING_HOME', 'aborted': 'aborted' }) StateMachine.add('PREPARING_SHOWER', PreparingShower(), transitions={ 'succeeded': 'GO_SHOWER', 'aborted': 'aborted' }) StateMachine.add('GO_SHOWER', GoShower(), transitions={ 'succeeded': 'STOP_SHOWER', 'aborted': 'aborted' }) StateMachine.add('STOP_SHOWER', StopShower(), transitions={ 'succeeded': 'DAY_MODE', 'aborted': 'aborted' }) StateMachine.add('LEAVING_HOME', self.sm_leaving_home, transitions={ 'succeeded': 'going_out', 'aborted': 'aborted' }) StateMachine.add('GOING_SLEEP', self.sm_going_sleep, transitions={ 'succeeded': 'WAIT_TO_BED', 'aborted': 'aborted' }) StateMachine.add('WAIT_TO_BED', self.sm_wait_bed, transitions={ 'succeeded': 'IN_BED', 'preempted': 'IN_BED', 'aborted': 'aborted' }) StateMachine.add('IN_BED', self.sm_in_bed, transitions={ 'succeeded': 'NIGHT_MODE', 'aborted': 'aborted' }) StateMachine.add('NIGHT_MODE', self.sm_night_mode, transitions={ 'succeeded': 'NIGHT_MODE', 'wake_up': 'WAKING_UP', 'aborted': 'aborted' }) StateMachine.add('WAKING_UP', self.sm_waking_up, transitions={ 'succeeded': 'DAY_MODE', 'aborted': 'aborted' }) StateMachine.add('GOING_EAT', self.sm_going_eat, transitions={ 'succeeded': 'DAY_MODE', 'aborted': 'aborted' }) # State machine for waking up self.sm_guarding = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_guarding: StateMachine.add('GUARD', Pause(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) # State machine with concurrence self.sm_incoming_home = Concurrence( outcomes=['succeeded', 'aborted'], default_outcome='succeeded', child_termination_cb=self.incoming_child_termination_cb, outcome_cb=self.incoming_outcome_cb) # Add the sm_actions machine and a battery MonitorState to the nav_patrol machine with self.sm_incoming_home: Concurrence.add( 'LOOK_CONNECTION', MonitorState("/METAWATCH/connected", Empty, self.empty_cb)) Concurrence.add( 'LOOK_ENTERING', MonitorState("/HOME/entry_door_open", Empty, self.empty_cb)) # State machine for away self.sm_away = StateMachine( outcomes=['succeeded', 'aborted', 'preempted', 'entering_home']) with self.sm_away: StateMachine.add('GUARDING_MODE', self.sm_guarding, transitions={ 'succeeded': 'INCOMING_HOME', 'aborted': 'aborted' }) StateMachine.add('INCOMING_HOME', self.sm_incoming_home, transitions={ 'succeeded': 'entering_home', 'aborted': 'aborted' }) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('AT_HOME', self.sm_home, transitions={ 'succeeded': 'AT_HOME', 'going_out': 'AWAY' }) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('AWAY', self.sm_away, transitions={ 'succeeded': 'AWAY', 'entering_home': 'AT_HOME' }) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def main(): rospy.init_node('tinker_mission_manipulation') trans = tf.TransformListener() rospy.loginfo("Waiting for tf ...") rospy.sleep(3) assert (len(trans.getFrameStrings()) > 0) state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with state: def kinect_callback(userdata, result): userdata.objects = [] objects = [] sum_x = 0 for obj in result.objects.objects: position = obj.pose.pose.pose.position if position.y > 0.5 or position.y < -0.5: continue _levels = [0.10, 0.44, 0.78, 1.12, 1.46, 1.80] for l in _levels: if fabs(l - position.z) < 0.05: position.z = l + 0.05 if position.z > 1.1: position.z += 0.05 obj.header.stamp = rospy.Time(0) kinect_point = PointStamped(header=obj.header, point=position) odom_point = trans.transformPoint('odom', kinect_point) sum_x += odom_point.point.x rospy.loginfo( colored('[Kinect Object(odom)] from:(%f %f %f)', 'yellow'), odom_point.point.x, odom_point.point.y, odom_point.point.z) objects.append(odom_point) avg_x = sum_x / len(objects) for from_point in objects: to_point = copy.deepcopy(from_point) to_point.point.x = avg_x to_point.point.z = find_div(from_point.point.z) userdata.objects.append({'from': from_point, 'to': to_point}) rospy.loginfo( colored('[Kinect Object(odom)] to:(%f %f %f)', 'yellow'), to_point.point.x, to_point.point.y, to_point.point.z) rospy.loginfo(colored('Total Object: %d', 'green'), len(objects)) return 'succeeded' StateMachine.add('Arm_Mode_Kinect', ArmModeState(ArmModeState.Arm_Mode_Kinect), transitions={'succeeded': 'Start_Button'}) StateMachine.add('Start_Button', MonitorStartButtonState(), transitions={ 'valid': 'Start_Button', 'invalid': 'S_Kinect_Recognition' }) StateMachine.add('S_Kinect_Recognition', ServiceState(service_name='/kinect_find_objects', service_spec=FindObjects, input_keys=['objects'], output_keys=['objects'], response_cb=kinect_callback), transitions={'succeeded': 'Generate_Report'}) StateMachine.add('Generate_Report', GenerateReportState(image='result.png', text='object_names.txt'), transitions={'succeeded': 'IT_Objects_Iterator'}) objects_iterator = Iterator( outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['objects'], output_keys=[], it=lambda: state.userdata.objects, it_label='target', exhausted_outcome='succeeded') with objects_iterator: fetch_object_sequence = Sequence( outcomes=['succeeded', 'aborted', 'continue', 'preempted'], input_keys=['target'], connector_outcome='succeeded') with fetch_object_sequence: Sequence.add('Speak', SpeakState('New Object recognized')) Sequence.add('Gripper_Photo', GripperState(GripperState.GRIPPER_OPEN)) Sequence.add('Move_For_Photo', MoveArmState(Point(-0.7, 0, 0), target_key='from'), transitions={'aborted': 'continue'}) concurrence = Concurrence( outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=['target']) with concurrence: Concurrence.add( 'Move_Fetch', MoveArmState(Point(0.06, 0, 0), target_key='from')) Concurrence.add( 'Gripper_Laser_sensor', MonitorState('/gripper_laser_sensor', Bool, cond_cb=lambda x, y: False)) Sequence.add('Move_Fetch_Concurrence', concurrence) Sequence.add('Gripper_Fetch', GripperState(GripperState.GRIPPER_CLOSE)) Sequence.add( 'Move_Fetch_Back', MoveArmState(Point(-0.7, 0, 0), target_key='from')) Sequence.add('Move_Down', MoveArmState(Point(-0.6, 0, 0), target_key='to')) Sequence.add('Move_Put', MoveArmState(Point(0, 0, 0), target_key='to')) Sequence.add('Gripper_Put', GripperState(GripperState.GRIPPER_OPEN)) Sequence.add('Move_Put_Back', MoveArmState(Point(-0.6, 0, 0), target_key='to'), transitions={'succeeded': 'continue'}) Iterator.set_contained_state('Seq_Fetch_Object', fetch_object_sequence, loop_outcomes=['continue']) # end of objects_iterator StateMachine.add('IT_Objects_Iterator', objects_iterator, transitions={ 'succeeded': 'A_Move_Reset', 'aborted': 'A_Move_Reset' }) StateMachine.add('A_Move_Reset', ArmModeState(ArmModeState.Arm_Mode_Init), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) # Run state machine introspection server for smach viewer intro_server = IntrospectionServer('tinker_mission_manipulation', state, '/tinker_mission_manipulation') intro_server.start() outcome = state.execute() rospy.spin() intro_server.stop()
def __init__(self): rospy.init_node('carry_smach_move', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (0.0, 3 * pi / 2, pi / 2, 3 * pi / 2, pi / 2, 3 * pi / 2, 3 * pi / 2, 0.0, pi, 0.0, pi, 3 * pi / 2) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, -3.0, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(2.0, -2.0, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(2.9, 1.0, 0.0), quaternions[2])) self.waypoints.append(Pose(Point(3.0, 1.2, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[4])) self.waypoints.append(Pose(Point(4.23, 1.1, 0.0), quaternions[5])) self.waypoints.append(Pose(Point(4.4, -0.6, 0.0), quaternions[6])) self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[7])) self.waypoints.append(Pose(Point(2.3, 1.7, 0.0), quaternions[8])) self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[9])) self.waypoints.append(Pose(Point(-1.3, 0.7, 0.0), quaternions[10])) self.waypoints.append(Pose(Point(-1.6, 0.1, 0.0), quaternions[11])) # State machine #self.sm_out_main = StateMachine(outcomes=['succeeded','aborted','preempted']) #self.sm_out_main.userdata.goal = self.waypoints[2]; #with self.sm_out_main: # StateMachine.add('GO_OUT_MAIN_ROOM', Nav2Waypoint(), # transitions={'succeeded':'succeeded', # 'aborted':'aborted'}) # Concurrent State machine self.sm_in_main_room = Concurrence( outcomes=[ 'succeeded', 'aborted', 'preempted', 'go_kitchen', 'go_bedroom', 'go_sleep' ], default_outcome='succeeded', child_termination_cb=self.in_main_room_child_termination_cb, outcome_cb=self.in_main_room_outcome_cb) self.sm_in_main_room.userdata.goal = self.waypoints[1] with self.sm_in_main_room: Concurrence.add( 'GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb)) Concurrence.add( 'GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb)) Concurrence.add( 'GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb)) #Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[1])) # Concurrent State machine self.sm_sleep = Concurrence( outcomes=[ 'succeeded', 'aborted', 'preempted', 'go_kitchen', 'go_bedroom', 'go_main_room' ], default_outcome='succeeded', child_termination_cb=self.in_sleep_child_termination_cb, outcome_cb=self.in_sleep_outcome_cb) self.sm_sleep.userdata.goal = self.waypoints[0] with self.sm_sleep: Concurrence.add( 'GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb)) Concurrence.add( 'GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb)) Concurrence.add( 'GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb)) #Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[0])) # Concurrent State machine self.sm_in_kitchen = Concurrence( outcomes=[ 'succeeded', 'aborted', 'preempted', 'go_main_room', 'go_bedroom', 'go_sleep' ], default_outcome='succeeded', child_termination_cb=self.in_kitchen_child_termination_cb, outcome_cb=self.in_kitchen_outcome_cb) self.sm_in_kitchen.userdata.goal = self.waypoints[6] with self.sm_in_kitchen: Concurrence.add( 'GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb)) Concurrence.add( 'GO_TO_BEDROOM', MonitorState("/CARRY/go_bedroom", Empty, self.empty_cb)) Concurrence.add( 'GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb)) #Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[6])) # Concurrent State machine self.sm_in_bedroom = Concurrence( outcomes=[ 'succeeded', 'aborted', 'preempted', 'go_main_room', 'go_kitchen', 'go_sleep' ], default_outcome='succeeded', child_termination_cb=self.in_bedroom_child_termination_cb, outcome_cb=self.in_bedroom_outcome_cb) self.sm_in_bedroom.userdata.goal = self.waypoints[11] with self.sm_in_bedroom: Concurrence.add( 'GO_TO_MAIN_ROOM', MonitorState("/CARRY/go_main_room", Empty, self.empty_cb)) Concurrence.add( 'GO_TO_KITCHEN', MonitorState("/CARRY/go_kitchen", Empty, self.empty_cb)) Concurrence.add( 'GO_TO_SLEEP', MonitorState("/CARRY/go_sleep", Empty, self.empty_cb)) #Concurrence.add('GO_TO_POINT', Nav2Waypoint(self.waypoints[11])) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('IN_MAIN_ROOM', self.sm_in_main_room, transitions={ 'succeeded': 'IN_MAIN_ROOM', 'go_kitchen': 'NAV_M2K_M', 'go_sleep': 'IN_SLEEP', 'go_bedroom': 'NAV_M2B_M' }) StateMachine.add('IN_KITCHEN', self.sm_in_kitchen, transitions={ 'succeeded': 'succeeded', 'go_main_room': 'NAV_K2M_K', 'go_sleep': 'NAV_K2S_K', 'go_bedroom': 'NAV_K2B_K' }) StateMachine.add('IN_BEDROOM', self.sm_in_bedroom, transitions={ 'succeeded': 'succeeded', 'go_kitchen': 'NAV_B2K_B', 'go_sleep': 'NAV_B2S_B', 'go_main_room': 'NAV_B2M_B' }) StateMachine.add('IN_SLEEP', self.sm_sleep, transitions={ 'succeeded': 'succeeded', 'go_kitchen': 'NAV_S2K_M', 'go_main_room': 'IN_MAIN_ROOM', 'go_bedroom': 'NAV_S2B_M' }) StateMachine.add('NAV_M2K_M', Nav2Waypoint(self.waypoints[2]), transitions={ 'succeeded': 'NAV_M2K_K', 'aborted': 'aborted' }) StateMachine.add('NAV_M2K_K', Nav2Waypoint(self.waypoints[5]), transitions={ 'succeeded': 'NAV_M2K_END', 'aborted': 'aborted' }) StateMachine.add('NAV_M2K_END', Nav2Waypoint(self.waypoints[6]), transitions={ 'succeeded': 'IN_KITCHEN', 'aborted': 'aborted' }) StateMachine.add('NAV_K2M_K', Nav2Waypoint(self.waypoints[4]), transitions={ 'succeeded': 'NAV_K2M_M', 'aborted': 'aborted' }) StateMachine.add('NAV_K2M_M', Nav2Waypoint(self.waypoints[3]), transitions={ 'succeeded': 'NAV_K2M_END', 'aborted': 'aborted' }) StateMachine.add('NAV_K2M_END', Nav2Waypoint(self.waypoints[1]), transitions={ 'succeeded': 'IN_MAIN_ROOM', 'aborted': 'aborted' }) StateMachine.add('NAV_M2B_M', Nav2Waypoint(self.waypoints[2]), transitions={ 'succeeded': 'NAV_M2B_C', 'aborted': 'aborted' }) StateMachine.add('NAV_M2B_C', Nav2Waypoint(self.waypoints[8]), transitions={ 'succeeded': 'NAV_M2B_B', 'aborted': 'aborted' }) StateMachine.add('NAV_M2B_B', Nav2Waypoint(self.waypoints[10]), transitions={ 'succeeded': 'NAV_M2B_END', 'aborted': 'aborted' }) StateMachine.add('NAV_M2B_END', Nav2Waypoint(self.waypoints[11]), transitions={ 'succeeded': 'IN_BEDROOM', 'aborted': 'aborted' }) StateMachine.add('NAV_K2S_K', Nav2Waypoint(self.waypoints[4]), transitions={ 'succeeded': 'NAV_K2S_M', 'aborted': 'aborted' }) StateMachine.add('NAV_K2S_M', Nav2Waypoint(self.waypoints[3]), transitions={ 'succeeded': 'NAV_K2S_END', 'aborted': 'aborted' }) StateMachine.add('NAV_K2S_END', Nav2Waypoint(self.waypoints[0]), transitions={ 'succeeded': 'IN_SLEEP', 'aborted': 'aborted' }) StateMachine.add('NAV_K2B_K', Nav2Waypoint(self.waypoints[4]), transitions={ 'succeeded': 'NAV_K2B_C', 'aborted': 'aborted' }) StateMachine.add('NAV_K2B_C', Nav2Waypoint(self.waypoints[8]), transitions={ 'succeeded': 'NAV_K2B_B', 'aborted': 'aborted' }) StateMachine.add('NAV_K2B_B', Nav2Waypoint(self.waypoints[10]), transitions={ 'succeeded': 'NAV_K2B_END', 'aborted': 'aborted' }) StateMachine.add('NAV_K2B_END', Nav2Waypoint(self.waypoints[11]), transitions={ 'succeeded': 'IN_BEDROOM', 'aborted': 'aborted' }) StateMachine.add('NAV_B2K_B', Nav2Waypoint(self.waypoints[9]), transitions={ 'succeeded': 'NAV_B2K_C', 'aborted': 'aborted' }) StateMachine.add('NAV_B2K_C', Nav2Waypoint(self.waypoints[7]), transitions={ 'succeeded': 'NAV_B2K_K', 'aborted': 'aborted' }) StateMachine.add('NAV_B2K_K', Nav2Waypoint(self.waypoints[5]), transitions={ 'succeeded': 'NAV_B2K_END', 'aborted': 'aborted' }) StateMachine.add('NAV_B2K_END', Nav2Waypoint(self.waypoints[6]), transitions={ 'succeeded': 'IN_KITCHEN', 'aborted': 'aborted' }) StateMachine.add('NAV_B2S_B', Nav2Waypoint(self.waypoints[9]), transitions={ 'succeeded': 'NAV_B2S_C', 'aborted': 'aborted' }) StateMachine.add('NAV_B2S_C', Nav2Waypoint(self.waypoints[7]), transitions={ 'succeeded': 'NAV_B2S_M', 'aborted': 'aborted' }) StateMachine.add('NAV_B2S_M', Nav2Waypoint(self.waypoints[3]), transitions={ 'succeeded': 'NAV_B2S_END', 'aborted': 'aborted' }) StateMachine.add('NAV_B2S_END', Nav2Waypoint(self.waypoints[0]), transitions={ 'succeeded': 'IN_SLEEP', 'aborted': 'aborted' }) StateMachine.add('NAV_B2M_B', Nav2Waypoint(self.waypoints[9]), transitions={ 'succeeded': 'NAV_B2M_C', 'aborted': 'aborted' }) StateMachine.add('NAV_B2M_C', Nav2Waypoint(self.waypoints[7]), transitions={ 'succeeded': 'NAV_B2M_M', 'aborted': 'aborted' }) StateMachine.add('NAV_B2M_M', Nav2Waypoint(self.waypoints[3]), transitions={ 'succeeded': 'NAV_B2M_END', 'aborted': 'aborted' }) StateMachine.add('NAV_B2M_END', Nav2Waypoint(self.waypoints[1]), transitions={ 'succeeded': 'IN_MAIN_ROOM', 'aborted': 'aborted' }) StateMachine.add('NAV_S2B_M', Nav2Waypoint(self.waypoints[2]), transitions={ 'succeeded': 'NAV_S2B_C', 'aborted': 'aborted' }) StateMachine.add('NAV_S2B_C', Nav2Waypoint(self.waypoints[8]), transitions={ 'succeeded': 'NAV_S2B_B', 'aborted': 'aborted' }) StateMachine.add('NAV_S2B_B', Nav2Waypoint(self.waypoints[10]), transitions={ 'succeeded': 'NAV_S2B_END', 'aborted': 'aborted' }) StateMachine.add('NAV_S2B_END', Nav2Waypoint(self.waypoints[11]), transitions={ 'succeeded': 'IN_BEDROOM', 'aborted': 'aborted' }) StateMachine.add('NAV_S2K_M', Nav2Waypoint(self.waypoints[2]), transitions={ 'succeeded': 'NAV_S2K_K', 'aborted': 'aborted' }) StateMachine.add('NAV_S2K_K', Nav2Waypoint(self.waypoints[5]), transitions={ 'succeeded': 'NAV_S2K_END', 'aborted': 'aborted' }) StateMachine.add('NAV_S2K_END', Nav2Waypoint(self.waypoints[6]), transitions={ 'succeeded': 'IN_KITCHEN', 'aborted': 'aborted' }) # Create and start the SMACH introspection server intro_server = IntrospectionServer('carry_sm', self.sm_top, '/SM_CARRY_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): rospy.init_node('petit_smach_ai', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi/2, pi, 3*pi/2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist) self.stopping = False self.recharging = False self.robot_side = 1 # State machine for Action1 self.sm_action1 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action1.userdata.mandibles_sleep = 0.1 with self.sm_action1: StateMachine.add('OPEN_MANDIBLES', OpenMandibles(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'aborted'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'UPDATE_DROPCUBE_OBJ', 'aborted':'aborted'}) # StateMachine.add('UPDATE_DROPCUBE_OBJ', UpdateObjectiveDropCubes(), # transitions={'succeeded':'CLOSE_MANDIBLES', # 'aborted':'aborted'}) StateMachine.add('UPDATE_DROPCUBE_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioCube_cb, response_cb=self.updatePrioCube_cb, output_keys=['waypoint_out'], input_keys=['robot_side']), transitions={'succeeded':'HCLOSE_MANDIBLES', 'aborted':'HCLOSE_MANDIBLES'}, remapping={'waypoint_out':'patrol_waypoint'}) StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action2 self.sm_action2 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action2.userdata.speed = -0.1; self.sm_action2.userdata.distance = 0.3; self.sm_action2.userdata.mandibles_sleep = 0.1 with self.sm_action2: StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'OPEN_MANDIBLES', 'aborted':'aborted'}) StateMachine.add('OPEN_MANDIBLES', OpenMandibles(), transitions={'succeeded':'MOVE_BACKWARD', 'aborted':'aborted'}) StateMachine.add('MOVE_BACKWARD', MoveForward(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action3 self.sm_action3 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action3.userdata.mandibles_sleep = 0.1 self.sm_action3.userdata.speed = -0.1; self.sm_action3.userdata.distance = 0.2; with self.sm_action3: StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'MOVE_PUSH', 'aborted':'aborted'}) StateMachine.add('MOVE_PUSH', MovePush(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action4 self.sm_action4 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action4.userdata.mandibles_sleep = 0.1 self.sm_action4.userdata.speed_x = 0.1 self.sm_action4.userdata.speed_y = 0.1 self.sm_action4.userdata.distance = 0.5; with self.sm_action4: StateMachine.add('CLOSE_MANDIBLES', CloseMandibles(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'aborted'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'SLIDE', 'aborted':'aborted'}) StateMachine.add('SLIDE', Slide(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action5 self.sm_action5 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action5.userdata.mandibles_sleep = 0.1 with self.sm_action5: StateMachine.add('OPEN_MANDIBLES', OpenMandibles(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'aborted'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'UPDATE_DROPSHELL_OBJ', 'aborted':'aborted'}) # StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(), # transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES', # 'aborted':'aborted'}) StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb, output_keys=['waypoint_out'], input_keys=['robot_side']), transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES', 'aborted':'ALMOSTCLOSE_MANDIBLES'}) StateMachine.add('ALMOSTCLOSE_MANDIBLES', AlmostCloseMandibles(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action6 self.sm_action6 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action6.userdata.mandibles_sleep = 0.1 self.sm_action6.userdata.speed = 0.1; self.sm_action6.userdata.distance = 0.2; with self.sm_action6: StateMachine.add('OPEN_MANDIBLES', OpenMandibles(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'aborted'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'FORWARD', 'aborted':'aborted'}) StateMachine.add('FORWARD', MoveForward(), transitions={'succeeded':'UPDATE_DROPSHELL_OBJ', 'aborted':'aborted'}) # StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(), # transitions={'succeeded':'HCLOSE_MANDIBLES', # 'aborted':'aborted'}) StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb, output_keys=['waypoint_out'], input_keys=['robot_side']), transitions={'succeeded':'HCLOSE_MANDIBLES', 'aborted':'HCLOSE_MANDIBLES'}) StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Action7 self.sm_action7 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out']) self.sm_action7.userdata.mandibles_sleep = 0.1 with self.sm_action7: StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}) # State machine for Actions self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted']) self.sm_actions.userdata.waypoints = self.waypoints with self.sm_actions: StateMachine.add('PICK_WAYPOINT', ServiceState('/PETIT/get_objective', GetObjective, response_cb=self.objective_cb, output_keys=['waypoint_out'], outcomes=['action1','action2','action3','action4','action5','action6','action7','aborted','succeeded','preempted']), transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','action7':'SM_ACTION7','aborted':'SM_ACTION1'}, remapping={'waypoint_out':'patrol_waypoint'}) #StateMachine.add('PICK_WAYPOINT', PickWaypoint(), # transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'}, # remapping={'waypoint_out':'patrol_waypoint'}) StateMachine.add('SM_ACTION1', self.sm_action1, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION2', self.sm_action2, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION3', self.sm_action3, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION4', self.sm_action4, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION5', self.sm_action5, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION6', self.sm_action6, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('SM_ACTION7', self.sm_action7, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'}, remapping={'waypoint_in':'patrol_waypoint', 'waypoint_out':'remove_waypoint'}) StateMachine.add('REMOVE_OBJECTIVE', RemoveObjective(), transitions={'succeeded':'succeeded', 'aborted':'aborted'}, remapping={'waypoint_in':'remove_waypoint'}) # State machine with concurrence self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_actions machine and a battery MonitorState to the nav_patrol machine with self.sm_concurrent: Concurrence.add('SM_ACTIONS', self.sm_actions) Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb)) Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: # @smach.cb_interface() # def requestPrioCube_cb(userdata, request): # update_request = UpdatePriority().Request # update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) # update_request.goal.pose.position.y = 0.800 # update_request.prio = 100 # return update_request StateMachine.add('WAIT_COLOR', MonitorState("/GENERAL/color", Int32, self.color_cb), transitions={'valid':'WAIT_START', 'preempted':'WAIT_START', 'invalid':'WAIT_START'}) StateMachine.add('WAIT_START', MonitorState("/GENERAL/start", Empty, self.start_cb), transitions={'valid':'CONCURRENT', 'preempted':'CONCURRENT', 'invalid':'CONCURRENT'}) StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', 'stop':'STOP'}) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): rospy.init_node('Pi_Smach', anonymous=False) rospy.on_shutdown(self.shutdown) setup_environment(self) self.last_nav_state = None self.recharging = False nav_states = list() for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, exec_timeout=rospy.Duration(60.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, exec_timeout=rospy.Duration(60.0), server_wait_timeout=rospy.Duration(10.0)) self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) self.sm_nav.userdata.X_input = 0 self.sm_nav.userdata.Y_input = 0 with self.sm_nav: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={ 'succeeded': 'NAV_STATE_1', 'aborted': 'NAV_STATE_1' }) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={ 'succeeded': 'INPUTXY', 'aborted': 'INPUTXY' }) StateMachine.add('INPUTXY', InputXY(), transitions={'succeeded': 'GOCHOSEWAY'}, remapping={ 'X_output': 'X_input', 'Y_output': 'Y_input' }) StateMachine.add('GOCHOSEWAY', ChoseWay(), transitions={ 'succeeded': 'MOVE_ARM', 'aborted': 'MOVE_ARM' }, remapping={ 'X': 'X_input', 'Y': 'Y_input' }) StateMachine.add('MOVE_ARM', Move_Arm(), transitions={'succeeded': 'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={ 'succeeded': '', 'aborted': '' }) self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add( 'MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): # init node rospy.init_node('pool_patrol', anonymous=False) # Set the shutdown fuction (stop the robot) rospy.on_shutdown(self.shutdown) # Initilalize the mission parameters and variables setup_task_environment(self) # A variable to hold the last/current mission goal/state self.last_mission_state = None # A flag to indicate whether or not we are recharging self.recharging = False # Turn the target locations into SMACH MoveBase and LosPathFollowing action states nav_terminal_states = {} nav_transit_states = {} # DP controller for target in self.pool_locations.iterkeys(): nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'odom' nav_goal.target_pose.pose = self.pool_locations[target] move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.nav_result_cb, exec_timeout=self.nav_timeout, server_wait_timeout=rospy.Duration(10.0)) nav_terminal_states[target] = move_base_state # Path following for target in self.pool_locations.iterkeys(): nav_goal = LosPathFollowingGoal() #nav_goal.prev_waypoint = navigation.vehicle_pose.position nav_goal.next_waypoint = self.pool_locations[target].position nav_goal.forward_speed.linear.x = self.transit_speed nav_goal.desired_depth.z = self.search_depth nav_goal.sphereOfAcceptance = self.search_area_size los_path_state = SimpleActionState( 'los_path', LosPathFollowingAction, goal=nav_goal, result_cb=self.nav_result_cb, exec_timeout=self.nav_timeout, server_wait_timeout=rospy.Duration(10.0)) nav_transit_states[target] = los_path_state """ Create individual state machines for assigning tasks to each target zone """ # Create a state machine container for the orienting towards the gate subtask(s) self.sm_gate_tasks = StateMachine(outcomes=[ 'found', 'unseen', 'missed', 'passed', 'aborted', 'preempted' ]) # Then add the subtask(s) with self.sm_gate_tasks: # if gate is found, pass pixel info onto TrackTarget. If gate is not found, look again StateMachine.add('SCANNING_OBJECTS', SearchForTarget('gate'), transitions={ 'found': 'CAMERA_CENTERING', 'unseen': 'BROADEN_SEARCH', 'passed': '', 'missed': '' }, remapping={ 'px_output': 'object_px', 'fx_output': 'object_fx', 'search_output': 'object_search', 'search_confidence_output': 'object_confidence' }) StateMachine.add('CAMERA_CENTERING', TrackTarget('gate', self.pool_locations['gate'].position), transitions={'succeeded': 'SCANNING_OBJECTS'}, remapping={ 'px_input': 'object_px', 'fx_input': 'object_fx', 'search_input': 'object_search', 'search_confidence_input': 'object_confidence' }) StateMachine.add('BROADEN_SEARCH', TrackTarget('gate', self.pool_locations['gate'].position), transitions={'succeeded': 'SCANNING_OBJECTS'}, remapping={ 'px_input': 'object_px', 'fx_input': 'object_fx', 'search_input': 'object_search', 'search_confidence_input': 'object_confidence' }) # Create a state machine container for returning to dock self.sm_docking = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add states to container with self.sm_docking: StateMachine.add('RETURN_TO_DOCK', nav_transit_states['docking'], transitions={ 'succeeded': 'DOCKING_SECTOR', 'aborted': '', 'preempted': 'RETURN_TO_DOCK' }) StateMachine.add('DOCKING_SECTOR', ControlMode(POSE_HEADING_HOLD), transitions={ 'succeeded': 'DOCKING_PROCEEDURE', 'aborted': '', 'preempted': '' }) StateMachine.add('DOCKING_PROCEEDURE', nav_terminal_states['docking'], transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) """ Assemble a Hierarchical State Machine """ # Initialize the HSM self.sm_mission = StateMachine(outcomes=[ 'succeeded', 'aborted', 'preempted', 'passed', 'missed', 'unseen', 'found' ]) # Build the HSM from nav states and target states with self.sm_mission: """ Navigate to GATE in TERMINAL mode """ StateMachine.add('TRANSIT_TO_GATE', nav_transit_states['gate'], transitions={ 'succeeded': 'GATE_SEARCH', 'aborted': 'DOCKING', 'preempted': 'TRANSIT_TO_GATE' }) """ When in GATE sector""" StateMachine.add('GATE_SEARCH', self.sm_gate_tasks, transitions={ 'passed': 'GATE_PASSED', 'missed': 'TRANSIT_TO_GATE', 'aborted': 'DOCKING' }) """ Transiting to gate """ StateMachine.add('GATE_PASSED', ControlMode(OPEN_LOOP), transitions={ 'succeeded': 'TRANSIT_TO_POLE', 'aborted': 'DOCKING', 'preempted': 'TRANSIT_TO_GATE' }) StateMachine.add('TRANSIT_TO_POLE', nav_transit_states['pole'], transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) """ When in POLE sector""" #StateMachine.add('POLE_PASSING_TASK', sm_pole_tasks, transitions={'passed':'POLE_PASSING_TASK','missed':'RETURN_TO_DOCK','aborted':'RETURN_TO_DOCK'}) """ When aborted, return to docking """ StateMachine.add('DOCKING', self.sm_docking, transitions={'succeeded': ''}) # Register a callback function to fire on state transitions within the sm_mission state machine self.sm_mission.register_transition_cb(self.mission_transition_cb, cb_args=[]) # Initialize the recharge state machine self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add states to the container with self.sm_recharge: """ To be able to dock we have to be in the recharging area """ StateMachine.add('DOCKING', self.sm_docking, transitions={'succeeded': 'RECHARGE_BATTERY'}) """ Batteru is recharged by using the set_battery_level service to 100 percent battery level """ StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': 'RECHARGE_FINISHED'}) """ when recharge is finished, we have to set the DP controller in open loop so we can use the transit controller """ StateMachine.add('RECHARGE_FINISHED', ControlMode(OPEN_LOOP), transitions={'succeeded': ''}) # Create the sm_concurrence state machine using a concurrence container self.sm_concurrence = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_mission and a battery MonitorState to the sm_concurrence container with self.sm_concurrence: Concurrence.add('SM_MISSION', self.sm_mission) Concurrence.add( 'MONITOR_BATTERY', MonitorState("/manta/battery_level", Float32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add sm_concurrence, sm_recharge and a Stop() objective to sm_top with self.sm_top: StateMachine.add('PATROL', self.sm_concurrence, transitions={ 'succeeded': '', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # Create and start the SMACH Introspection server intro_server = IntrospectionServer(str(rospy.get_name()), self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine hsm_outcome = self.sm_top.execute() intro_server.stop()
def __init__(self): rospy.init_node('patrol_smach_concurrence', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables setup_task_environment(self) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # A variable to hold the last/current navigation goal self.last_nav_state = None # A flag to indicate whether or not we are rechargin self.recharging = False # A list to hold then navigation goa nav_states = list() location_goal = MoveBaseGoal() result_goal = MoveBaseGoal() #recognize_goal = VisionGoal() #flag = 1 class wait(State): def __init__(self): State.__init__(self, outcomes=['succeeded', 'aborted']) def execute(self, userdata): if flag == 0: rospy.loginfo('waitting') return 'aborted' else: return 'succeeded' class MoveItDemo(State): # spin() simply keeps python from exiting until this node is stopped def __init__(self): # Initialize the move_group API State.__init__(self, outcomes=['succeeded', 'aborted']) def execute(self, userdata): moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('moveit_ik') #rospy.Subscriber("chatter", Pose, callback) # Initialize the move group for the right arm right_arm = moveit_commander.MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.11) right_arm.set_goal_orientation_tolerance(0.15) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_arm_init') right_arm.go() rospy.sleep(2) # Set the target pose. This particular pose has the gripper oriented horizontally # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of # the center of the robot base. target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() #global a,b,c,d,e,f,g target_pose.pose.position.x = 0.3 target_pose.pose.position.y = -0.45 target_pose.pose.position.z = 1.35 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = 0 target_pose.pose.orientation.w = 1 #Set the start state to the current state right_arm.set_start_state_to_current_state() #print a # Set the goal pose of the end effector to the stored pose right_arm.set_pose_target(target_pose, end_effector_link) # Plan the trajectory to the goal traj = right_arm.plan() # Execute the planned trajectory right_arm.execute(traj) # Pause for a second rospy.sleep(5) #right_arm.set_named_target('right_arm_init') right_arm.go() return 'succeeded' class MoveItDemo1(State): # spin() simply keeps python from exiting until this node is stopped def __init__(self): # Initialize the move_group API State.__init__(self, outcomes=['succeeded', 'aborted']) def execute(self, userdata): moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('moveit_ik') #rospy.Subscriber("chatter", Pose, callback) # Initialize the move group for the right arm right_arm = moveit_commander.MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) right_arm.set_named_target('right_arm_init') right_arm.go() return 'succeeded' # Turn the waypoints into SMACH states for waypoint in self.waypoints: nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = waypoint move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(20.0)) nav_states.append(move_base_state) # Create a MoveBaseAction state for the docking station nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(10.0)) pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 0.1 pose_target.position.x = 0.7 pose_target.position.y = -0.0 pose_target.position.z = 1.1 result_goal.target_pose.header.frame_id = 'map' result_goal.target_pose.pose = (Pose(Point(0.5, 1.5, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))) # Initialize the navigation state machine self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add the nav states to the state machine with the appropriate transitions with self.sm_nav: StateMachine.add('WAITTING', wait(), transitions={ 'succeeded': 'NAV_STATE_0', 'aborted': 'WAITTING' }) StateMachine.add('NAV_STATE_0', nav_states[0], transitions={ 'succeeded': 'NAV_STATE_1', 'aborted': 'NAV_STATE_1' }) StateMachine.add('NAV_STATE_1', SimpleActionState('move_base', MoveBaseAction, goal=location_goal), transitions={ 'succeeded': 'ARM', 'aborted': 'ARM' }) #StateMachine.add('VISION', SimpleActionState('drv_action', VisionAction, goal=recognize_goal), #transitions={'succeeded': 'ARM', 'aborted': 'ARM'}) StateMachine.add('ARM', MoveItDemo(), transitions={ 'succeeded': 'NAV_STATE_2', 'aborted': 'NAV_STATE_2' }) StateMachine.add('NAV_STATE_2', nav_states[0], transitions={ 'succeeded': 'ARM1', 'aborted': 'ARM1' }) StateMachine.add('ARM1', MoveItDemo1(), transitions={ 'succeeded': '', 'aborted': '' }) # StateMachine.add('NAV_STATE_2', SimpleActionState('move_base', MoveBaseAction, goal_slots=['target_pose']), # transitions={'succeeded': 'NAV_STATE_0'}, remapping={'target_pose': 'user_data'}) # Register a callback function to fire on state transitions within the sm_nav state machine self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # Initialize the recharge state machine self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) # Create the nav_patrol state machine using a Concurrence container self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) # Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) Concurrence.add( 'LOCATION_GOAL', MonitorState("nav_location_goal", Pose, self.nav_location_goal_cb)) Concurrence.add( 'RECOGNIZE_GOAL', MonitorState("/comm/msg/control/recognize_goal", String, self.recognize_goal_cb)) #Concurrence.add('RESULT', MonitorState("/drv_action/result", VisionActionResult, self.result_goal_cb)) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) rospy.loginfo('=============ce shi=============') time.sleep(5) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): rospy.init_node('explain_history_concurrence', anonymous=False) # 设置关闭机器人函数(stop the robot) rospy.on_shutdown(self.shutdown) # 初始化一些参数和变量 setup_task_environment(self) # 跟踪到达目标位置的成功率 self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # 保存上一个或者当前的导航目标点的变量 self.last_nav_state = None # 指示是否正在充电的标志 self.recharging = False # 保存导航目标点的列表 nav_states = {} # 把waypoints变成状态机的状态 for waypoint in self.room_locations.iterkeys(): nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.room_locations[waypoint] move_base_state = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0), server_wait_timeout=rospy.Duration(10.0)) # nav_states.append(move_base_state) nav_states[waypoint] = move_base_state # 为扩展底座(docking station)创建一个MoveBaseAction state nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(10.0)) # 为written words子任务创建一个状态机 sm_written_words = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_written_words: StateMachine.add('EXPLAIN_HISTORY', WrittenWords('written_words', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 为rule子任务创建一个状态机 sm_rule = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_rule: StateMachine.add('EXPLAIN_HISTORY', Rule('rule', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 为life子任务创建一个状态机 sm_life = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_life: StateMachine.add('EXPLAIN_HISTORY', Life('life', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 为faith子任务创建一个状态机 sm_faith = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) # 然后添加子任务 with sm_faith: StateMachine.add('EXPLAIN_HISTORY', Faith('faith', 5), transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 初始化导航的状态机 self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 使用transitions将导航的状态添加到状态机 with self.sm_nav: StateMachine.add('START', nav_states['explanatory_text'], transitions={ 'succeeded': 'WRITTEN_WORDS', 'aborted': 'WRITTEN_WORDS', 'preempted': 'WRITTEN_WORDS' }) ''' Add the living room subtask(s) ''' StateMachine.add('WRITTEN_WORDS', nav_states['explanatory_text'], transitions={ 'succeeded': 'WRITTEN_WORDS_TASKS', 'aborted': 'RULE', 'preempted': 'RULE' }) # 当任务完成时, 继续进行kitchen任务 StateMachine.add('WRITTEN_WORDS_TASKS', sm_written_words, transitions={ 'succeeded': 'RULE', 'aborted': 'RULE', 'preempted': 'RULE' }) ''' Add the kitchen subtask(s) ''' StateMachine.add('RULE', nav_states['explain_the_rule'], transitions={ 'succeeded': 'RULE_TASKS', 'aborted': 'LIFE', 'preempted': 'LIFE' }) # 当任务完成时, 继续进行bathroom任务 StateMachine.add('RULE_TASKS', sm_rule, transitions={ 'succeeded': 'LIFE', 'aborted': 'LIFE', 'preempted': 'LIFE' }) ''' Add the bathroom subtask(s) ''' StateMachine.add('LIFE', nav_states['explain_life'], transitions={ 'succeeded': 'LIFE_TASKS', 'aborted': 'FAITH', 'preempted': 'FAITH' }) # 当任务完成时, 继续进行hallway任务 StateMachine.add('LIFE_TASKS', sm_life, transitions={ 'succeeded': 'FAITH', 'aborted': 'FAITH', 'preempted': 'FAITH' }) ''' Add the hallway subtask(s) ''' StateMachine.add('FAITH', nav_states['explain_faith'], transitions={ 'succeeded': 'FAITH_TASKS', 'aborted': '', 'preempted': '' }) # 当任务完成时, stop StateMachine.add('FAITH_TASKS', sm_faith, transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) # 在sm_nav状态机中注册一个回调函数以启动状态转换(state transitions) self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # 初始化充电的状态机 self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState( 'battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) # 使用并发容器(Concurrence container)创建nav_patrol状态机 self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # 将sm_nav machine和battery MonitorState添加到nav_patrol状态机里面 with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add( 'MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb)) # 创建顶层状态机 self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # 将nav_patrol,sm_recharge和Stop添加到sm_top状态机 with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # 创建并开始SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # 运行状态机 sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def __init__(self): rospy.init_node('patrol_smach_concurrence', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) self.docking_station_pose = (Pose(Point(-0.5, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))) self.waypoints = list() nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose.position.x = 0.7 nav_goal.target_pose.pose.position.y = 0.0 nav_goal.target_pose.pose.position.z = 0.0 nav_goal.target_pose.pose.orientation.w = 1.0 self.waypoints.append(nav_goal) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose.position.x = 2.51611566544 nav_goal.target_pose.pose.position.y = 0.100562250018 nav_goal.target_pose.pose.position.z = 0.0 nav_goal.target_pose.pose.orientation.w = 1.0 self.waypoints.append(nav_goal) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose.position.x = 2.94330668449 nav_goal.target_pose.pose.position.y = -0.77200148106 nav_goal.target_pose.pose.position.z = 0.0 nav_goal.target_pose.pose.orientation.z = -0.585479230542 nav_goal.target_pose.pose.orientation.w = 0.81068740622 self.waypoints.append(nav_goal) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose.position.x = 2.94330668449 nav_goal.target_pose.pose.position.y = -1.67200148106 nav_goal.target_pose.pose.position.z = 0.0 nav_goal.target_pose.pose.orientation.z = -0.585479230542 nav_goal.target_pose.pose.orientation.w = 0.81068740622 self.waypoints.append(nav_goal) # A variable to hold the last/current navigation goal self.last_nav_state = None # A flag to indicate whether or not we are rechargin self.recharging = False # A list to hold then navigation goa nav_states = list() # Create a MoveBaseAction state for the docking station nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.docking_station_pose nav_docking_station = SimpleActionState( 'move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(20.0), server_wait_timeout=rospy.Duration(10.0)) # Initialize the navigation state machine self.sm_nav = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add the nav states to the state machine with the appropriate transitions self.sm_nav.userdata.waypoints = self.waypoints with self.sm_nav: StateMachine.add('PICK_WAYPOINT', PickWaypoint(), transitions={'succeeded': 'NAV_WAYPOINT'}, remapping={'waypoint_out': 'patrol_waypoint'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={ 'succeeded': 'PICK_WAYPOINT', 'aborted': 'PICK_WAYPOINT', }, remapping={'waypoint_in': 'patrol_waypoint'}) # Register a callback function to fire on state transitions within the sm_nav state machine self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[]) # Initialize the recharge state machine self.sm_recharge = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.sm_recharge: StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded': 'RECHARGE_BATTERY'}) StateMachine.add('RECHARGE_BATTERY', ServiceState('battery/set_battery', SetBattery, 100, response_cb=self.recharge_cb), transitions={'succeeded': ''}) # Create the nav_patrol state machine using a Concurrence container self.nav_patrol = Concurrence( outcomes=['succeeded', 'recharge', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.nav_patrol: Concurrence.add('SM_NAV', self.sm_nav) Concurrence.add( 'MONITOR_BATTERY', MonitorState("battery/battery_level", Float32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('PATROL', self.nav_patrol, transitions={ 'succeeded': 'PATROL', 'recharge': 'RECHARGE', 'stop': 'STOP' }) StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded': 'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()
def main(): rospy.init_node('smach_usecase_executive') rospy.Subscriber("turtle2/pose", turtlesim.msg.Pose, turtle2pose_cb) sm_root = smach.StateMachine( outcomes=['succeeded', 'preempted', 'aborted']) #input_keys = ['my_awesome_goal'], #output_keys= ['some_awesome_result']) with sm_root: #add state to call ROS service by turtlesim to reset sim smach.StateMachine.add('RESET', ServiceState('reset', std_srvs.srv.Empty), transitions={'succeeded': 'SPAWN'}) #add state to spawn new turtle at 0,0 called turtle2 smach.StateMachine.add('SPAWN', ServiceState('spawn', turtlesim.srv.Spawn, request=turtlesim.srv.SpawnRequest( 0, 0, 0, 'turtle2')), transitions={'succeeded': 'TELEPORT1'}) smach.StateMachine.add( 'TELEPORT1', ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute, request=turtlesim.srv.TeleportAbsoluteRequest( 5.0, 1.0, 0)), transitions={'succeeded': 'DRAW_SHAPES'}) # Create DRAW SHAPES container sm_draw_shapes = smach.Concurrence( outcomes=['succeeded', 'preempted', 'aborted'], default_outcome='aborted', outcome_map={ 'succeeded': { 'BIG': 'succeeded', 'SMALL': 'succeeded' } }) smach.StateMachine.add('DRAW_SHAPES', sm_draw_shapes, transitions={'succeeded': 'succeeded'}) with sm_draw_shapes: smach.Concurrence.add( 'BIG', SimpleActionState('turtle_shape1', turtle_actionlib.msg.ShapeAction, goal=turtle_actionlib.msg.ShapeGoal( edges=11, radius=4.0))) #Create SMALL container sm_small = smach.StateMachine( outcomes=['succeeded', 'preempted', 'aborted']) smach.Concurrence.add('SMALL', sm_small) with sm_small: smach.StateMachine.add( 'TELEPORT2', ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute, request=turtlesim.srv.TeleportAbsoluteRequest( 9.0, 5.0, 0)), transitions={'succeeded': 'DRAW_WITH_MONITOR'}) smach.StateMachine.add('WAIT_FOR_CLEAR', MonitorState("turtle1/pose", turtlesim.msg.Pose, wait_for_clear_callback), transitions={ 'invalid': 'TELEPORT2', 'valid': 'WAIT_FOR_CLEAR' }) #Create DRAW WITH MONITOR sm_draw_with_monitor = smach.Concurrence( outcomes=[ 'succeeded', 'preempted', 'aborted', 'interrupted' ], default_outcome='aborted', child_termination_cb=child_term_cb, outcome_map={ 'succeeded': { 'DRAW': 'succeeded', 'MONITOR': 'valid' }, 'preempted': { 'DRAW': 'preempted', 'MONITOR': 'preempted' }, 'interrupted': { 'MONITOR': 'invalid' } }) smach.StateMachine.add( 'DRAW_WITH_MONITOR', sm_draw_with_monitor, transitions={'interrupted': 'WAIT_FOR_CLEAR'}) with sm_draw_with_monitor: smach.Concurrence.add( 'DRAW', SimpleActionState('turtle_shape2', turtle_actionlib.msg.ShapeAction, goal=turtle_actionlib.msg.ShapeGoal( edges=6, radius=0.5))) smach.Concurrence.add( 'MONITOR', MonitorState("turtle1/pose", turtlesim.msg.Pose, monitor_callback)) # Commented out to run as actionlib server #start introspection server to use smach_viewer.py sis = smach_ros.IntrospectionServer('server_name', sm_root, '/SM_ROOT') sis.start() # Execute SMACH tree in a separate thread so that we can ctrl-c the script smach_thread = threading.Thread(target=sm_root.execute) smach_thread.start() #outcome = sm_root.execute() rospy.spin() sis.stop() sm_root.request_preempt()
def __init__(self): rospy.init_node('random_patrol', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables #setup_task_environment(self) # Create a list to hold the target quaternions (orientations) quaternions = list() # First define the corner orientations as Euler angles euler_angles = (pi / 2, pi, 3 * pi / 2, 0) # Then convert the angles to quaternions for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) # Create a list to hold the waypoint poses self.waypoints = list() self.square_size = 1.0 # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) self.waypoints.append( Pose(Point(self.square_size, 0.0, 0.0), quaternions[0])) self.waypoints.append( Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1])) self.waypoints.append( Pose(Point(0.0, self.square_size, 0.0), quaternions[2])) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist) self.stopping = False self.recharging = False # Initialize the patrol state machine self.sm_patrol = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Set the userdata.waypoints variable to the pre-defined waypoints self.sm_patrol.userdata.waypoints = self.waypoints # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('PICK_WAYPOINT', PickWaypoint(), transitions={ 'succeeded': 'NAV_WAYPOINT', 'aborted': 'NAV_WAYPOINT' }, remapping={'waypoint_out': 'patrol_waypoint'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={ 'succeeded': '', 'aborted': '' }, remapping={'waypoint_in': 'patrol_waypoint'}) # Create the nav_patrol state machine using a Concurrence container self.sm_concurrent = Concurrence( outcomes=['succeeded', 'stop'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) # Add the sm_nav machine and a battery MonitorState to the nav_patrol machine with self.sm_concurrent: Concurrence.add('SM_NAV', self.sm_patrol) Concurrence.add( 'MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb)) #Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb)) # Create the top level state machine self.sm_top = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) # Add nav_patrol, sm_recharge and a Stop() machine to sm_top with self.sm_top: StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={ 'succeeded': 'CONCURRENT', 'stop': 'STOP' }) #StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'}) StateMachine.add('STOP', Stop(), transitions={'succeeded': ''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_top.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop()