def __init__(self): rospy.init_node('move_to_and_inspect_point_sm', anonymous=False) hsm = StateMachine(outcomes=['finished statemachine']) with hsm: StateMachine.add( 'GO_TO_POINT', SimpleActionState( 'pid_global', MoveAction, makeMoveGoal("pid_global_plan", -3.0, 0, -0.5, radius_of_acceptance = 2.0)), transitions = { "succeeded": 'INSPECT_POINT', "preempted": 'INSPECT_POINT', "aborted": 'INSPECT_POINT' }) StateMachine.add( 'INSPECT_POINT', SimpleActionState( 'inspect_point', MoveAction, makeMoveGoal("inspect_point", -3.0, 0.0, -0.5, radius_of_acceptance=2.0)), transitions = { 'succeeded': 'INSPECT_POINT', "preempted": 'INSPECT_POINT', "aborted": 'INSPECT_POINT' }) intro_server = IntrospectionServer(str(rospy.get_name()), hsm,'/SM_ROOT') intro_server.start() hsm.execute() #patrol.execute() print("State machine execute finished") intro_server.stop()
def main(): rospy.init_node('update_userdata_from_old_state') state_a = UpdateName('name_topic') state_b = PrintName(duration=4.0) state_c = PrintName(duration=2.0) top = StateMachine(outcomes=['success']) top.userdata.my_name = "Joseph" with top: StateMachine.add('A', state_a, transitions={'success': 'B'}, remapping={'my_name': 'my_name'}) StateMachine.add('B', state_b, transitions={'success': 'C'}, remapping={'my_name': 'my_name'}) StateMachine.add('C', state_c, transitions={'success': 'A'}, remapping={'my_name': 'my_name'}) top.execute()
class test(): def __init__(self): rospy.init_node('go_and_follow') self.Test = StateMachine(outcomes=['succeeded', 'aborted', 'error']) self.Test.userdata.PT_LIST = {} self.Test.userdata.mission = {} with self.Test: self.Test.userdata.targets = list() self.Test.userdata.actions = list() self.Test.userdata.people_condition = {} self.Test.userdata.command = 2 # StateMachine.add('GET_TARGET', # CheckStop2(), # transitions={'stop': 'succeeded', 'aborted': 'GET_TARGET', 'remeber':'GET_TARGET'}) StateMachine.add( 'GET_TASK', GeneralAnswer(), transitions={ 'succeeded': 'GET_TASK', 'aborted': 'aborted' }, remapping={ 'targets': 'targets', 'actions': 'actions', #'task_num':'tasksNum' }) self.Test.execute()
def main(): global rate global is_tape global center_tape global pub rospy.init_node('tape_master') rate = rospy.Rate(20) is_tape_sub = rospy.Subscriber('tape_detect_msg', Bool, is_tape_callback) center_tape_sub = rospy.Subscriber('tape_center_percent_msg', Float32, center_tape_callback) pub = rospy.Publisher('vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=1) tape = StateMachine(outcomes=['decide_tape', 'no_tape', 'follow_tape']) with tape: StateMachine.add('DECIDE', decision(), transitions={ 'decide_tape': 'DECIDE', 'no_tape': 'STOP', 'follow_tape': 'FOLLOW' }) StateMachine.add('STOP', stop(), transitions={'decide_tape': 'DECIDE'}) StateMachine.add('FOLLOW', follow(), transitions={'decide_tape': 'DECIDE'}) tape.execute()
def PatrolStateMachine(listOfWaypoints): tempWaypoint = () waypoints = [] #convert waypoints to ros topic format #waypoints.append([str(0), (listOfWaypoints[-1][1],listOfWaypoints[-1][0]), (0.0, 0.0, 0.0, 1.0)]) for i in range(0, len(listOfWaypoints), 1): tempWaypoint = (listOfWaypoints[i][1], listOfWaypoints[i][0]) waypoints.append([str(i), tempWaypoint, (0.0, 0.0, 0.0, 1.0)]) #start state machine patrol = StateMachine(['succeeded', 'aborted', 'preempted']) with patrol: for i, w in enumerate(waypoints): goal_pose = MoveBaseGoal() goal_pose.target_pose.header.frame_id = 'map' goal_pose.target_pose.pose.position.x = w[1][0] goal_pose.target_pose.pose.position.y = w[1][1] goal_pose.target_pose.pose.position.z = 0.0 goal_pose.target_pose.pose.orientation.x = w[2][0] goal_pose.target_pose.pose.orientation.y = w[2][1] goal_pose.target_pose.pose.orientation.z = w[2][2] goal_pose.target_pose.pose.orientation.w = w[2][3] StateMachine.add(w[0], SimpleActionState('move_base', MoveBaseAction, goal=goal_pose), transitions={'succeeded': waypoints[(i + 1) % \ len(waypoints)][0]}) patrol.execute()
def main(): rospy.init_node('move_gripper_client') parser = argparse.ArgumentParser( description='Client for Korus` move gripper action server') parser.add_argument('gripper_angle', type=float, help='gripper angle') args = parser.parse_args() sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) sm.userdata.gripper_angle = args.gripper_angle with sm: def MoveGripperResultCb(userdata, status, result): rospy.loginfo('action finished in state ' + str(status)) rospy.loginfo('action result: ') rospy.loginfo(str(result)) smach.StateMachine.add( 'MoveGripper', SimpleActionState('/korus/move_gripper', MoveGripperAction, goal_slots=['gripper_angle'], result_cb=MoveGripperResultCb)) sm.execute()
class turn(): def __init__(self): rospy.init_node('turn') rospy.logwarn('ssssss') self.Turn = StateMachine(outcomes=['succeeded', 'aborted', 'error']) with self.Turn: self.Turn.userdata.degree = pi self.Turn.userdata.sentences = 'I want play a riddle game' StateMachine.add('SPEAK', Speak(), transitions={ 'succeeded': 'turn', 'aborted': 'aborted', 'error': 'error' }, remapping={'sentences': 'sentences'}) StateMachine.add('turn', TurnDegree(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }, remapping={'degree': 'degree'}) self.Turn.execute()
class test(): def __init__(self): rospy.init_node('test') rospy.on_shutdown(self.shutdown) self.top = StateMachine(outcomes=['succeeded', 'aborted', 'error']) with self.top: self.top.userdata.go_point = Point(1, 0, 0) self.top.userdata.degree = 3.14 StateMachine.add('sm1', LinearDisplacement(), transitions={ 'succeeded': 'sm2', 'preempted': 'sm2', 'error': 'error' }, remapping={'displacementVec': 'go_point'}) StateMachine.add('sm2', TurnDegree(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }, remapping={'degree': 'degree'}) self.top.execute() def shutdown(self): rospy.logerr('done')
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 main(msg): if (msg.data == True): print('Creating SMACH') sm = StateMachine(['complete', 'failed']) sis = smach_ros.IntrospectionServer('smach_introspect', sm, '/SM_ROOT') with sm: StateMachine.add('MANUAL_OVERRIDE', MANUAL_OVERRIDE(), transitions={ 'success': 'GET_HOME', 'failed': 'failed' }) StateMachine.add('GET_HOME', GET_HOME(), transitions={ 'success': 'START_GMAPPING', 'failed': 'failed' }) StateMachine.add('START_GMAPPING', START_GMAPPING(), transitions={ 'success': 'GET_POINTS', 'failed': 'failed' }) StateMachine.add('GET_POINTS', GET_POINTS(), transitions={ 'success': 'MAKE_MAP', 'failed': 'failed' }) StateMachine.add('MAKE_MAP', MAKE_MAP(), transitions={ 'success': 'DISPLAY_MAP', 'failed': 'failed' }) StateMachine.add('DISPLAY_MAP', DISPLAY_MAP(), transitions={ 'success': 'USER_CONFIRMATION', 'failed': 'failed' }) StateMachine.add('USER_CONFIRMATION', USER_CONFIRMATION(), transitions={ 'confirm': 'complete', 'update': 'UPDATE_POINTS', 'failed': 'failed' }) StateMachine.add('UPDATE_POINTS', UPDATE_POINTS(), transitions={ 'success': 'MAKE_MAP', 'failed': 'failed' }) sis.start() sm.execute()
def turnOnStateMachine(request): sm = StateMachine(outcomes=['success']) sm.userdata.direction = request.direction with sm: StateMachine.add('A', A(), transitions={ '1': 'B', '0': 'D' }, remapping={ 'input': 'direction', 'output': '' }) StateMachine.add('B', B(), transitions={ '1': 'C', '0': 'success' }, remapping={ 'input': 'direction', 'output': '' }) StateMachine.add('C', C(), transitions={ '1': 'D', '0': 'B' }, remapping={ 'input': 'direction', 'output': '' }) StateMachine.add('D', D(), transitions={ '1': 'success', '0': 'C' }, remapping={ 'input': 'direction', 'output': '' }) ## sis = smach_os.IntrospectionServer('server_name', sm, '/SM_ROOT') ## sis.start() sm.execute() ## sis.stop() response = jinko_service_msgResponse() response.success = True return response
class ArGraspTask(): 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 position_cb(self,userdata,msg): if msg.markers is not None: obj_pos = PointStamped() obj_pos.point = msg.markers[0].pose.pose.position obj_pos.header.frame_id ='camera_Link' userdata.obj_pos = obj_pos return True else: return False def callback(self,req): self.arm_point = req.goal_position self.arm_mode = req.action sm_result = self.sm_ArTags.execute(parent_ud ={'arm_point':self.arm_point,'arm_mode':self.arm_mode}) res = xm_PickOrPlaceResponse() if sm_result == 'succeeded': rospy.logwarn('arm_stack succeeded') res.result =True else: rospy.logwarn('moveit failed, please justfy the position of robot') res.result =False return res
def callback(data): #difine state when normal which is state one class one(state): def _init_(self): state._init_(self, outcomes=['emergency']) def execute(self, userdata): rospy.loginfo('Executing state normal') if battery_state > 10: m = data.data.split() print(m) if (int(m[0]) > 100): bot.drive_direct(-35, 35) elif (int(m[1]) > 100): bot.drive_direct(-35, 35) elif (int(m[2]) > 100): bot.drive_direct(-35, 35) elif (int(m[3]) > 100): bot.drive_direct(35, -35) elif (int(m[4]) > 100): bot.drive_direct(35, -35) elif (int(m[5]) > 100): bot.drive_direct(35, -35) else: bot.drive_straight(35) else: return 'emergency' class two(state): #difine state when emergancy which is state two def __init__(self): State.__init__(self, outcomes=['normal']) def execute(self, userdata): rospy.loginfo('Executing state emergency') bot.drive_straight(0) if battery_state > 10: return 'normal' if __name__ == '__main__': # Create a SMACH state machine sm = StateMachine(outcomes=['normal', 'emergency']) # Open the container with sm: # Add states to the container StateMachine.add('ONE', one(), transitions={'emergency': 'TWO'}) StateMachine.add('TWO', two(), transitions={'normal': 'ONE'}) # Execute SMACH plan sm.execute()
class EnterRoom(): def __init__(self): rospy.init_node('enter_room') rospy.on_shutdown(self.shutdown) self.sm_enter = StateMachine(outcomes= ['succeeded','aborted','preempted']) self.nav_states_ = WayPoint().nav_states_ with self.sm_enter : # navpose is the pose of xm_arm self.sm_enter.userdata.mode = 0 StateMachine.add('NAV_POSE', ArmCmd(), transitions={'succeeded':'WAIT1','aborted':'NAV_POSE'}, remapping = {'mode':'mode'}) # this time (rec =1) may need to justfy to satisfy the scene self.sm_enter.userdata.delay_time =1.0 StateMachine.add('WAIT1', Wait(), transitions={'succeeded':'DOOR'}, remapping = {'rec':'delay_time'}) StateMachine.add('DOOR', DoorDetect().door_detect_, transitions={'invalid':'NAV1','valid':'DOOR'}) StateMachine.add('NAV1', self.nav_states_[0], transitions={'succeeded':'TURN_POSE','aborted':'NAV1'}) pt = Point() pt.x =0 pt.y=0 pt.z= pi/8 self.sm_enter.userdata.turn_pose = pt StateMachine.add('TURN_POSE', SimpleMove(), transitions={'succeeded':'SPEAK','aborted':'SPEAK'}, remapping = {'turn_pose':'turn_pose'}) self.sm_enter.userdata.sentences = 'please look at me' StateMachine.add('SPEAK', Speak(), transitions={'succeeded':'succeeded'}, remapping ={'sentences':'sentences'}) self.smach_bool = False self.sm_enter.execute() self.smach_bool =True def shutdown(self): if self.smach_bool ==True: rospy.loginfo('xm completes the whole tasks , >3<') else: rospy.loginfo('Stopping..., you may write the wrong code , but this is not the fault of xm #_ #')
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 main(): rospy.init_node('follow_waypoints') sm = StateMachine(outcomes=['success']) with sm: StateMachine.add('GET_PATH', GetPath(), transitions={'success': 'FOLLOW_PATH', 'killed': 'success'}, remapping={'waypoints': 'waypoints'}) StateMachine.add('FOLLOW_PATH', FollowPath(), transitions={'success': 'PATH_COMPLETE'}, remapping={'waypoints': 'waypoints'}) StateMachine.add('PATH_COMPLETE', PathComplete(), transitions={'success': 'GET_PATH'}) sm.execute()
def main(): rospy.init_node('smach_usecase_executive') sm_root = StateMachine(outcomes=['preempted', 'aborted', 'succeeded']) with sm_root: StateMachine.add('RESET', ServiceState('/reset', Empty, request=EmptyRequest()), transitions={'succeeded': 'SPAWN'}) StateMachine.add( 'SPAWN', ServiceState('/spawn', Empty, request=EmptyRequest()), #request=SpawnRequest(1.0, 1.0, 2.0, 'kkk')), transitions={'aborted': 'aborted'}) # transitions={ # 'preemtped': 'preemtped', # 'aborted': 'aborted', # 'succeeded': 'succeeded'}) sis = IntrospectionServer('exmaple', sm_root, '/USE_CASE') sis.start() outcome = sm_root.execute() rospy.spin()
def main(): rospy.init_node('smach_usecase_step_02') # 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'))) # Attach a SMACH introspection server sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE') sis.start() # Execute SMACH tree outcome = sm0.execute() # Signal ROS shutdown (kill threads in background) rospy.spin() sis.stop()
def main(): rospy.init_node('smach_example_hierarchical_state_machine') # Create the top level SMACH state machine sm_top = StateMachine(outcomes=['outcome5']) # Open the container with sm_top: StateMachine.add('BAS', Bas(), transitions={'outcome3': 'SUB'}) # Create the sub SMACH state machine sm_sub = StateMachine(outcomes=['outcome4']) # Open the container with sm_sub: # Add state to the container StateMachine.add('FOO', Foo(), transitions={ 'outcome1': 'BAR', 'outcome2': 'outcome4' }) StateMachine.add('BAR', Bar(), transitions={'outcome1': 'FOO'}) StateMachine.add('SUB', sm_sub, transitions={'outcome4': 'outcome5'}) sis = IntrospectionServer('example', sm_top, '/SM_STATEMACHINE') sis.start() # Execute SMACH plan outcome = sm_top.execute() rospy.spin() sis.stop()
def test_service_cb(self): """Test calling a service with a callback.""" srv = rospy.Service('/empty', std_srvs.Empty, empty_server) sm = StateMachine(['succeeded', 'aborted', 'preempted', 'done']) with sm: def foo_response_cb(userdata, response): userdata.foo_var_out = 'foo!' return 'succeeded' StateMachine.add('FOO', ServiceState('/empty', std_srvs.Empty, response_cb=foo_response_cb, output_keys=['foo_var_out']), remapping={'foo_var_out': 'sm_var'}, transitions={'succeeded': 'done'}) outcome = sm.execute() rospy.logwarn("OUTCOME: " + outcome) assert outcome == 'done'
def main(): #Create a rospy node for the state machine rospy.init_node('IRA_statemachine') #Creating a state machine for banking scenario sm_bank = StateMachine(outcomes=['Stop']) #Create Smach containers for each state with sm_bank: StateMachine.add('Face Recognition', frn.Face_recognition(), transitions={'Detected':'Welcome_speech','Not_Detected':'Face Recognition'},remapping={'Face_recognition_out':'speech_in'}) StateMachine.add('Welcome_speech', ss1.welcome_speech(), transitions={'completed':'Hotword_detection'}) StateMachine.add('Hotword_detection', hwd.Hotword_detection(), transitions={'Detected':'Speech_Recognition','Not_Detected':'Stop'}) StateMachine.add('Speech_Recognition', sr.Speech_Recognition(), transitions={'Completed':'Hotword_detection'}) #Code for initiating the smach_viewer Monitor = smach_ros.IntrospectionServer('IRA', sm_bank, '/IRA_FLOW') Monitor.start() #Execute the state machine outcome = sm_bank.execute() #Stop the smach_viewer if state machine stops Monitor.stop
def main(): #Create a rospy node for the state machine rospy.init_node('IRA_statemachine') #Creating a state machine for banking scenario sm_bank = StateMachine(outcomes=['Stop']) #Create Smach containers for each state with sm_bank: StateMachine.add('Face Recognition', frn.Face_recognition(), transitions={'Detected':'Speech_Gesture','Not_Detected':'Face Recognition'},remapping={'Face_recognition_out':'speech_in'}) sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Speech1':'Completed','Gesture1':'Completed'}}) with sm_con: smach.Concurrence.add('Speech1',ss1.speech()) smach.Concurrence.add('Gesture1',g1.gesture()) smach.StateMachine.add('Speech_Gesture', sm_con, transitions={'Waiting':'Stop', 'Done':'Face Recognition'}) StateMachine.add('Hotword_detection', hwdc.Hotword_detection(), transitions={'Detected':'Speech_Recognition','Not_Detected':'Face Recognition'}) StateMachine.add('Speech_Recognition', src.Speech_recognition(), transitions={'Completed':'Speech_database','Not_Completed':'Face Recognition'},remapping={'speech_recognition_out':'Database_in'}) StateMachine.add('Speech_database', sdc.Speech_database(), transitions={'Completed':'CON','Not_Completed':'Face Recognition'},remapping={'speech_database_out':'Text_in'}) sm_con = smach.Concurrence(outcomes=['Waiting','Done'],default_outcome='Waiting',outcome_map={'Done':{'Text_speech':'Completed'}},input_keys=['Text_in']) with sm_con: smach.Concurrence.add('Text_speech',ts.Text_speech()) #smach.Concurrence.add('ui1',ui1.Text_reply()) StateMachine.add('CON', sm_con, transitions={'Waiting':'Stop','Done':'Hotword_detection'}) #Code for initiating the smach_viewer Monitor = smach_ros.IntrospectionServer('IRA', sm_bank, '/IRA_FLOW') Monitor.start() #Execute the state machine outcome = sm_bank.execute() #Stop the smach_viewer if state machine stops Monitor.stop
def __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()
class TestGripperSmach: def __init__(self): rospy.init_node('test_gripper_smach', anonymous=False) gripper_goal = xm_GripperCommandGoal() gripper_goal.command.position = 0.10 gripper_goal.command.torque = 0.5 gripper_state = SimpleActionState( 'xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10)) self.gripper_smach = StateMachine( outcomes=['succeeded', 'aborted', 'preempted']) with self.gripper_smach: StateMachine.add('GRIPPER_OPEN', gripper_state, transitions={ 'succeeded': '', 'aborted': '', 'preempted': '' }) gripper_outcome = self.gripper_smach.execute() rospy.loginfo('State Machine Outcome: ' + str(gripper_outcome)) def gripper_state_cb(self, userdata, status, result): if result: rospy.loginfo("Complete!")
def main(): rospy.init_node('smach_example_state_machine') sm = StateMachine(['exit']) with sm: for key, (x, y, next_point) in WAYPOINTS.items(): goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y goal.target_pose.pose.orientation.w = 1. StateMachine.add(key, SimpleActionState('move_base', MoveBaseAction, goal=goal), transitions={'succeeded': next_point, 'aborted': 'exit', 'preempted': 'exit'}) # Create and start the introspection server sis = IntrospectionServer('strat', sm, '/SM_ROOT') sis.start() # Execute the state machine outcome = sm.execute() # Wait for ctrl-c to stop the application rospy.spin() sis.stop()
class WaypointStateMachine(object): def __init__(self): self.sm = StateMachine(outcomes=["success", "failure", "preempted"]) self.outcome = None self.action_server = None with self.sm: StateMachine.add("GOTO_WAYPOINT", GoToWaypointState(), transitions={ "success": "GOTO_WAYPOINT", "finished": "success", "failure": "failure", "preempted": "preempted" }, remapping={ "waypoints": "sm_waypoints", "waypoint_index_in": "sm_waypoint_index", "waypoint_index_out": "sm_waypoint_index", "action_server": "sm_action_server", }) def execute(self, waypoints, action_server): rospy.loginfo( "To cancel the waypoint follower, run: 'rostopic pub -1 /dodobot/follow_path/cancel actionlib_msgs/GoalID -- {}'" ) rospy.loginfo( "To cancel the current goal, run: 'rostopic pub -1 /move_base/cancel actionlib_msgs/GoalID -- {}'" ) self.sm.userdata.sm_waypoint_index = 0 self.sm.userdata.sm_waypoints = waypoints self.sm.userdata.sm_action_server = action_server self.outcome = self.sm.execute() return self.outcome
class TestGripperSmach: def __init__(self): rospy.init_node("test_gripper_smach", anonymous=False) gripper_goal = xm_GripperCommandGoal() gripper_goal.command.position = 0.10 gripper_goal.command.torque = 0.5 gripper_state = SimpleActionState( "xm_move_gripper", xm_GripperCommandAction, goal=gripper_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10), ) self.gripper_smach = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with self.gripper_smach: StateMachine.add( "GRIPPER_OPEN", gripper_state, transitions={"succeeded": "", "aborted": "", "preempted": ""} ) gripper_outcome = self.gripper_smach.execute() rospy.loginfo("State Machine Outcome: " + str(gripper_outcome)) def gripper_state_cb(self, userdata, status, result): if result: rospy.loginfo("Complete!")
class TestFollow(): def __init__(self): rospy.init_node('TestFollow') rospy.on_shutdown(self.shutdown) rospy.logerr('Follow start') self.smach_bool = False self.newFollow = StateMachine(outcomes = ['succeeded' , 'aborted','error']) with self.newFollow: StateMachine.add('RUNNODE' , RunNode() , transitions = {'succeeded':'FOLLOW' , 'aborted':'RUNNODE', }) StateMachine.add('FOLLOW' ,FollowTest() , transitions = {'succeeded':'succeeded' , 'error':'error', 'aborted':'aborted'} ) intro_server = IntrospectionServer('sm_gpsr',self.newFollow,'/SM_ROOT') intro_server.start() out = self.newFollow.execute() rospy.logerr(out) intro_server.stop() self.smach_bool = True def shutdown(self): if self.smach_bool ==True: rospy.loginfo('smach succeeded') else: rospy.loginfo('smach error')
def main(): rospy.init_node('tinker_mission_follow') rospy.loginfo(colored('starting follow and guide task ...', 'green')) # Main StateMachine state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with state: StateMachine.add('Start_Button', MonitorStartButtonState(), transitions={'valid': 'Start_Button', 'invalid': '1_Start'}) StateMachine.add('1_Start', MonitorKinectBodyState(), transitions={'valid':'1_Start', 'invalid':'Sequence'}) sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded') with sequence: follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[]) with follow_concurrence: Concurrence.add('FollowMe', FollowMeState()) Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop')) Sequence.add('Follow_concurrence', follow_concurrence) StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'}) # Run state machine introspection server for smach viewer intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation') intro_server.start() outcome = state.execute() rospy.spin() intro_server.stop()
def main(): rospy.init_node('smach_example_state_machine') # Create a SMACH state machine sm = StateMachine(outcomes=['outcome4']) sm.userdata.sm_counter = 0 sis = IntrospectionServer('example', sm, '/SM_ROOT') sis.start() # Open the container with sm: # Add states to the container StateMachine.add('FOO', Foo(), transitions={ 'outcome1': 'BAR', 'outcome2': 'outcome4' }, remapping={ 'foo_counter_in': 'sm_counter', 'foo_counter_out': 'sm_counter' }) StateMachine.add('BAR', Bar(), transitions={'outcome1': 'FOO'}, remapping={'bar_counter_in': 'sm_counter'}) # Execute SMACH plan outcome = sm.execute() rospy.spin()
def test_group(self): """Test adding a bunch of states with group args.""" class DoneState(State): def __init__(self): State.__init__(self, outcomes=['done']) def execute(self, ud=None): return 'done' sm = StateMachine(['succeeded', 'done']) with sm: StateMachine.add('FAILSAUCE', DoneState()) transitions = {'aborted': 'FAILSAUCE', 'preempted': 'FAILSAUCE'} with sm: StateMachine.add( 'FIRST', SimpleActionState('reference_action', TestAction, goal=g1), transitions) StateMachine.add( 'SECOND', SimpleActionState('reference_action', TestAction, goal=g2), transitions) StateMachine.add( 'THIRD', SimpleActionState('reference_action', TestAction, goal=g1), transitions) outcome = sm.execute() assert outcome == 'done'
def test_userdata_nesting(self): """Test serial manipulation of userdata.""" sm = StateMachine(['done', 'preempted', 'aborted']) with sm: StateMachine.add('SETTER', Setter(), {'done': 'GETTER'}) StateMachine.add('GETTER', Getter(), {'done': 'NEST'}) sm2 = StateMachine(['done', 'aborted', 'preempted']) sm2.register_input_keys(['a']) with sm2: StateMachine.add( 'ASSERTER', ConditionState(lambda ud: 'a' in ud, input_keys=['a']), { 'true': 'done', 'false': 'aborted' }) StateMachine.add('NEST', sm2) outcome = sm.execute() assert outcome == 'done' assert 'a' in sm.userdata assert 'b' in sm.userdata assert sm.userdata.a == 'A' assert sm.userdata.b == 'A'
class TestTurn(): def __init__(self): rospy.init_node('turn_test', anonymous=False) rospy.on_shutdown(self.shutdown) rospy.loginfo('started test the turn') self.test_bool = False self.test_turn = StateMachine( outcomes=['succeeded', 'aborted', 'error']) with self.test_turn: self.test_turn.userdata.degree = -3.1415926 / 8 StateMachine.add('TURN', TurnDegree(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }, remapping={'degree': 'degree'}) out = self.test_turn.execute() rospy.loginfo(out) if out == 'succeeded': self.test_bool = True def shutdown(self): if self.test_bool == True: rospy.logerr('test succeeded') else: rospy.logerr('test failed')
def __init__(self): global OBJECT_ID rospy.init_node("test") sm=StateMachine(outcomes=["succeeded","aborted","preempted","valid","invalid"]) self.lift=actionlib.SimpleActionClient('xm_move_arm_height', xm_ArmHeightAction) arm_goal = xm_ArmHeightGoal() arm_goal.distance_x = 1 arm_goal.distance_z = -0.2 self.arm_state=SimpleActionState('xm_move_arm_height',xm_ArmHeightAction,goal=arm_goal, result_cb=self.arm_state_cb, exec_timeout=rospy.Duration(60), server_wait_timeout=rospy.Duration(60)) gripper_goal = xm_GripperCommandGoal() gripper_goal.command.position = 0.0 # gripper_goal.command.torque = 0.5 # gripper_goal.header.frame gripper_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal, result_cb=self.wrist_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10)) wrist_goal=xm_WristPositionGoal() wrist_goal.angle=0 wrist_state=SimpleActionState("xm_move_wrist", xm_WristPositionAction,goal=wrist_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10)) self.monitor_times=1 sm.userdata.ob=0 with sm: # StateMachine.add("FIND_OBJECT1", Find_Object(), # transitions={"succeeded": "", 'aborted': ''}, ) # StateMachine.add("angle", ChangeAngular(angle=0.2), transitions={"succeeded":""}) # StateMachine.add("INIT",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"prepare",response_cb=self.responce_cb),transitions={'succeeded':''}) # StateMachine.add("HEIGHT",self.arm_state,transitions={"succeeded":''}) # StateMachine.add("GRIP",gripper_state,transitions={"succeeded":''}) # StateMachine.add("AR_TARGET2", MonitorState("ar_pose_marker", AlvarMarkers,self.get_mark_pos_cb), transitions={"valid":"AR_TARGET2","invalid":""}) # StateMachine.add("WRIST", wrist_state, transitions={"succeeded":""}) StateMachine.add("CHANGE_HEIGHT1", Lift_HEIGHT(height=0.0), transitions={"succeeded":""}) # StateMachine.add("changemode", ChangeMode("arm"), transitions={"succeeded":""},) # for i in range(4): sm.execute() # OBJECT_ID+=1 rospy.sleep(1)
def main(): rospy.init_node('tinker_mission_navigation') rospy.loginfo(colored('starting navigation task ...', 'green')) # load waypoints from xml pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r')) for pose in pose_list.pose: WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose) # Main StateMachine state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with state: StateMachine.add('Start_Button', MonitorStartButtonState(), transitions={'valid': 'Start_Button', 'invalid': 'Sequence'}) sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded') with sequence: # Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'}) # Sequence.add('ArriveWaypoint1', SpeakState('I have arrived at way point one')) # Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'), # transitions={'succeeded': 'ArriveWaypoint2', 'aborted': 'Obstacle'}) # Sequence.add('Obstacle', SpeakState('Obstacle in front of me')) # Sequence.add('ObstacleDelay', DelayState(10), # transitions={'succeeded': 'GoToWaypoin2'}) # Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two')) Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'}) Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three')) Sequence.add('StopCommandAndGo', SpeakState('Please GO. If you want to stop, say stop tinker')) Sequence.add('Train_human', FollowTrainState()) follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[]) with follow_concurrence: Concurrence.add('FollowMe', FollowMeState()) # Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop')) Sequence.add('Follow_concurrence', follow_concurrence) Sequence.add('GoToWaypoin3Again', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3Again'}) Sequence.add('ArriveWaypoint3Again', SpeakState('I am back at way point three')) Sequence.add('GoOut', WayPointGoalState('out'), transitions={'aborted': 'GoOut'}) StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'}) # Run state machine introspection server for smach viewer intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation') intro_server.start() outcome = state.execute() rospy.spin() intro_server.stop()
class RandomPatrol(): 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) # 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'}, remapping={'waypoint_out':'patrol_waypoint'}) StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'PICK_WAYPOINT', 'aborted':'PICK_WAYPOINT', 'preempted':'PICK_WAYPOINT'}, remapping={'waypoint_in':'patrol_waypoint'}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_patrol.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
def main(): rospy.init_node('tinker_mission_person_recognition') rospy.loginfo(colored('starting person recognition task ...', 'green')) # Main StateMachine state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with state: StateMachine.add('0_wait_for_start', MonitorStartButtonState(), transitions={'valid': '0_wait_for_start','invalid': '0_arm_mode'}) StateMachine.add('0_arm_mode', ArmModeState(ArmModeState.Arm_Mode_Kinect), transitions={'succeeded':'0_wait_for_human'}) StateMachine.add('0_wait_for_human', SpeakState('I am tinker , I am waiting for operator'), transitions={'succeeded': '1_Start'}) StateMachine.add('1_Start', MonitorKinectBodyState(), transitions={'valid':'1_Start', 'invalid':'Sequence'}) sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded') with sequence: Sequence.add('2_1_train_speak', SpeakState('please look at the camera')) Sequence.add('2_2_train', TrainPersonState()) Sequence.add('2_3_train_finish', SpeakState('I have memorized you, thanks')) Sequence.add('3_wait', DelayState(10)) Sequence.add('4_turn_back', ChassisSimpleMoveState(theta=3.23)) Sequence.add('4_1_turn_back', ChassisSimpleMoveState(x=-1.0)) Sequence.add('5_1_find_operator', FindPersonState()) Sequence.add('5_2_point_operator', MoveArmState(offset=Point(0, 0, 0)), remapping={'target':'person_pose'}) Sequence.add('5_3_say', SpeakState('I have found you')) Sequence.add('5_4_wait', DelayState(5)) StateMachine.add('Sequence', sequence, {'succeeded': 'Sequence_reset', 'aborted': 'Sequence_reset'}) sequence_reset = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded') with sequence_reset: Sequence.add('6_1_arm_init', ArmModeState(ArmModeState.Arm_Mode_Init)) Sequence.add('6_2_report', GenerateReportState(image='human_result.png', text='human_result.txt')) Sequence.add('6_3_finish', SpeakState('task finished')) StateMachine.add('Sequence_reset', sequence_reset, {'succeeded': 'succeeded', 'aborted': 'aborted'}) # Run state machine introspection server for smach viewer intro_server = IntrospectionServer('tinker_mission_person_recognition', state, '/tinker_mission_person_recognition') intro_server.start() outcome = state.execute() rospy.spin() intro_server.stop()
def main(): rospy.init_node('tinker_RIPS') rospy.loginfo(colored('starting RIPS task ...', 'green')) # load waypoints from xml pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r')) for pose in pose_list.pose: WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose) # Main StateMachine state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with state: StateMachine.add('Start_Button', MonitorStartButtonState(), transitions={'valid': 'Start_Button', 'invalid': 'Sequence'}) sequence_1 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded') with sequence_1: Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'}) Sequence.add('ArriveWaypoint1', SpeakState('Wait for continue')) StateMachine.add('Sequence', sequence_1, {'succeeded': 'Continue', 'aborted': 'aborted'}) StateMachine.add('Continue', MonitorStartButtonState(), transitions={'valid': 'Continue', 'invalid': 'Sequence_2'}) sequence_2 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded') with sequence_2: Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'), transitions={'aborted': 'GoToWaypoin2'}) Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two')) Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'}) Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three')) StateMachine.add('Sequence_2', sequence_2, {'succeeded': 'succeeded', 'aborted': 'aborted'}) # Run state machine introspection server for smach viewer intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation') intro_server.start() outcome = state.execute() rospy.spin() intro_server.stop()
def __init__(self): global target rospy.init_node('object', anonymous=False) rospy.on_shutdown(self.shutdown) grasp_sm=StateMachine(outcomes=['succeeded','aborted','preempted']) self.target=Point gripper_goal = xm_GripperCommandGoal() gripper_goal.command.position = 0.10 gripper_goal.command.torque = 0.5 # gripper_goal.header.frame gripper_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10)) # # self.lift=actionlib.SimpleActionClient('xm_move_arm_height', xm_ArmHeightAction) # arm_goal = xm_ArmHeightGoal() # arm_goal.distance_x = 0.5 # arm_goal.distance_z = 0.1 # self.arm_state=SimpleActionState('xm_move_gripper',xm_ArmHeightAction,goal=arm_goal, # result_cb=self.arm_state_cb, exec_timeout=rospy.Duration(10), # server_wait_timeout=rospy.Duration(10)) change2arm=ChangeMode("arm") change2base=ChangeMode("base") rospy.loginfo("ready") with grasp_sm: # StateMachine.add("INIT",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"init",response_cb=self.responce_cb),transitions={'succeeded':'FIND_OBJECT'}) StateMachine.add("FIND_OBJECT", ServiceState("Find_Object", xm_ObjectDetect,1, response_cb=self.find_object_cb), transitions={"succeeded":"BACK",'aborted':'FIND_OBJECT'}) StateMachine.add("BACK", Forword_BacK(dis= -0.5), transitions={'succeeded':'CHANGE_2_ARM1'}) StateMachine.add("CHANGE_2_ARM1",change2arm,transitions={'succeeded':"READY"}) StateMachine.add("READY",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"prepare",response_cb=self.responce_cb),transitions={'succeeded':'CHANGE_2_BASE1'}) StateMachine.add("CHANGE_2_BASE1",change2base,transitions={"succeeded":"ALIGN_Y"}) StateMachine.add("ALIGN_Y",Left_Right(dis=target.y-0),transitions= {"succeeded" :"CHANGE_2_ARM2"}) StateMachine.add("CHANGE_2_ARM2",change2arm,transitions={'succeeded':"OPEN_GRIPPER"}) StateMachine.add("OPEN_GRIPPER",gripper_state,transitions={'succeeded':'CHANGE_2_BASE2'}) StateMachine.add("CHANGE_2_BASE2",change2base,transitions={'succeeded':""}) outcome=grasp_sm.execute("FIND_OBJECT")
self.goal.target_pose.pose.position.x = position[0] self.goal.target_pose.pose.position.y = position[1] self.goal.target_pose.pose.position.z = 0.0 self.goal.target_pose.pose.orientation.x = orientation[0] self.goal.target_pose.pose.orientation.y = orientation[1] self.goal.target_pose.pose.orientation.z = orientation[2] self.goal.target_pose.pose.orientation.w = orientation[3] def execute(self, userdata): # Send goal and wait self.client.send_goal(self.goal) self.client.wait_for_result() return 'success' if __name__ == '__main__': rospy.init_node('patrol') # Create an SM to lookup the waypoints, going through them sequentially and continuously patrol = StateMachine(outcomes=['success']) with patrol: for i,w in enumerate(waypoints): StateMachine.add(w[0], Waypoint(w[1], w[2]), transitions={'success':waypoints[(i + 1) % len(waypoints)][0]}) patrol.execute()
class Patrol(): def __init__(self): rospy.init_node('patrol_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables self.setup_task_environment() #self.setup_initial_pose() # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 # A list to hold then navigation waypoints 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(30.0), server_wait_timeout=rospy.Duration(10.0)) nav_states.append(move_base_state) # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted']) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'}) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'}) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_4', nav_states[4], transitions={'succeeded':'NAV_STATE_5','aborted':'NAV_STATE_5','preempted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_5', nav_states[5], transitions={'succeeded':'NAV_STATE_6','aborted':'NAV_STATE_6','preempted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_6', nav_states[6], transitions={'succeeded':'','aborted':'','preempted':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT') intro_server.start() sm_outcome = self.sm_patrol.execute() rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def setup_initial_pose(self): # rostopic pub /initialpose geometry_msgs/PoseWithCovarianceSmped '{ header: { frame_id: "map" }, pose: { pose: { position: { x: 2.6731004715, y: 2.44490814209, z: 0 }, orientation: { x: 0 , y: 0, z: 0, w: 1 } } } }' self.initial_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, latch=True) mypose = PoseWithCovarianceStamped() mypose.header.frame_id = 'map' mypose.pose.pose = self.waypoints[1] self.initial_pose_pub.publish(mypose) def move_base_result_cb(self, userdata, status, result): if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("n_succeeded "+ str(self.n_succeeded)) rospy.loginfo("n_aborted "+ str(self.n_aborted)) rospy.loginfo("n_preempted "+ str(self.n_preempted)) rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def setup_task_environment(self): # How big is the square we want the robot to patrol? self.square_size = rospy.get_param("~square_size", 1.0) # meters # Set the low battery threshold (between 0 and 100) self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50) # How many times should we execute the patrol loop self.n_patrols = rospy.get_param("~n_patrols", 2) # meters # How long do we have to get to each waypoint? self.move_base_timeout = rospy.get_param("~move_base_timeout", 10) #seconds # Initialize the patrol counter self.patrol_count = 0 # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move_base action server") # Orientation list quaternions = list() quaternions.append(Quaternion(0.0, 0.0, 0.0, 1.0)) quaternions.append(Quaternion(0.0, 0.0, 1.0, 6.12303176911)) quaternions.append(Quaternion(0.0, 0.0, 0.707106781187, 0.707106781187)) quaternions.append(Quaternion(0.0, 0.0, 1.0, -0.0034963161226)) quaternions.append(Quaternion(0.0, 0.0, -0.707106781187, 0.707106781187)) quaternions.append(Quaternion(0.0, 0.0, 0.707106781187, 0.707106781187)) quaternions.append(Quaternion(0.0, 0.0, 0.707106781187, 0.707106781187)) # Position list points = list() points.append(Point(2.6731004715, 2.44490814209, 0)) points.append(Point(2.6731004715, 0.81360769272, 0)) points.append(Point(0.4786823391, 0.81360769272, 0)) points.append(Point(1.6281396152, 2.44490814209, 0)) points.append(Point(-0.6823855638, 2.44490814209, 0)) points.append(Point(1.7326359748, -0.06299890577, 0)) points.append(Point(-0.7520495653, 0.01827591657, 0)) # 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(points[0], quaternions[0]) ) self.waypoints.append( Pose(points[1], quaternions[1]) ) self.waypoints.append( Pose(points[2], quaternions[2]) ) self.waypoints.append( Pose(points[3], quaternions[3]) ) self.waypoints.append( Pose(points[4], quaternions[4]) ) self.waypoints.append( Pose(points[5], quaternions[5]) ) self.waypoints.append( Pose(points[6], quaternions[6]) ) # Where is the docking station? # let's consider docking station is our kitchen self.docking_station_pose = (Pose(points[0], quaternions[0])) # Initialize markers for the waypoints for RViz self.init_waypoint_markers() # Set a visualization marker at each waypoint for waypoint in self.waypoints[1:]: p = Point() p = waypoint.position self.waypoint_markers.points.append(p) # Initialize a marker for the docking station for RViz self.init_docking_station_marker() # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) rospy.loginfo("Starting Tasks") # Publish the waypoint markers self.marker_pub.publish(self.waypoint_markers) rospy.sleep(1) self.marker_pub.publish(self.waypoint_markers) # <- this is weird # Publish the docking station marker self.docking_station_marker_pub.publish(self.docking_station_marker) rospy.sleep(1) def init_waypoint_markers(self): # Set up our waypoint markers marker_scale = 0.2 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = 0 marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} # Define a marker publisher. self.marker_pub = rospy.Publisher('waypoint_markers', Marker) # Initialize the marker points list. self.waypoint_markers = Marker() self.waypoint_markers.ns = marker_ns self.waypoint_markers.id = marker_id self.waypoint_markers.type = Marker.CUBE_LIST self.waypoint_markers.action = Marker.ADD self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime) self.waypoint_markers.scale.x = marker_scale self.waypoint_markers.scale.y = marker_scale self.waypoint_markers.color.r = marker_color['r'] self.waypoint_markers.color.g = marker_color['g'] self.waypoint_markers.color.b = marker_color['b'] self.waypoint_markers.color.a = marker_color['a'] self.waypoint_markers.header.frame_id = 'odom' self.waypoint_markers.header.stamp = rospy.Time.now() self.waypoint_markers.points = list() def init_docking_station_marker(self): # Define a marker for the charging station marker_scale = 0.3 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = 0 marker_color = {'r': 0.7, 'g': 0.7, 'b': 0.0, 'a': 1.0} self.docking_station_marker_pub = rospy.Publisher('docking_station_marker', Marker) self.docking_station_marker = Marker() self.docking_station_marker.ns = marker_ns self.docking_station_marker.id = marker_id self.docking_station_marker.type = Marker.CYLINDER self.docking_station_marker.action = Marker.ADD self.docking_station_marker.lifetime = rospy.Duration(marker_lifetime) self.docking_station_marker.scale.x = marker_scale self.docking_station_marker.scale.y = marker_scale self.docking_station_marker.scale.z = 0.02 self.docking_station_marker.color.r = marker_color['r'] self.docking_station_marker.color.g = marker_color['g'] self.docking_station_marker.color.b = marker_color['b'] self.docking_station_marker.color.a = marker_color['a'] self.docking_station_marker.header.frame_id = 'odom' self.docking_station_marker.header.stamp = rospy.Time.now() self.docking_station_marker.pose = self.docking_station_pose def shutdown(self): rospy.loginfo("Stopping the robot...") #self.sm_patrol.request_preempt() #self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
def __init__(self): rospy.init_node("navgation_test") rospy.on_shutdown(self.shutdown) self.waypoints=[]; Location= (Point(3.099,-1.796,0), Point(6.732,-3.260,0), Point(7.058,-0.119,0), Point(-0.595,0.069,0),) quaternions=[]; euler_angles=[0.039,0.056,-3.093,-3.133]; for angle in euler_angles: q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') q = Quaternion(*q_angle) quaternions.append(q) for i in range(4): self.waypoints.append(Pose(Location[i], quaternions[i])) point_locations = (('Point1', self.waypoints[0]), ('Point2', self.waypoints[1]), ('Point3', self.waypoints[2]), ('Point4', self.waypoints[3])) self.room_locations = OrderedDict(point_locations) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) rospy.loginfo("Connected to move_base action server") nav_states={} self.people_in_sight=0 # Create a navigation task for each room for room in self.room_locations.iterkeys(): nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.header.stamp = rospy.Time.now() nav_goal.target_pose.pose = self.room_locations[room] move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(100.0), server_wait_timeout=rospy.Duration(100.0)) nav_states[room] = move_base_state sm_nav1= StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"]) with sm_nav1: StateMachine.add('START',MonitorState("task_comming", xm_Task, self.start_cb), transitions={'invalid':'WAIT4DOOR_OPEN', 'valid':'START', 'preempted':'WAIT4DOOR_OPEN'}) StateMachine.add('WAIT4DOOR_OPEN',MonitorState("doormsg",door_msg,self.door_is_open_cb), transitions={'invalid':'INIT', 'valid':'WAIT4DOOR_OPEN', 'preempted':'INIT'}) StateMachine.add("INIT", ServiceState("pan_srv", xm_Pan,True, response_cb=self.init_cb),transitions={"succeeded":"NAV_1","aborted":"NAV_1"}) StateMachine.add("NAV_1",nav_states['Point1'],transitions={'succeeded':'','aborted':'','preempted':''}) sm_nav2=StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_nav2: StateMachine.add("NAV_2",nav_states['Point2'],transitions={'succeeded':'','aborted':'Check_People','preempted':''}) StateMachine.add("Check_People",MonitorState("tracker/tracks_smoothed",TrackArray,self.check_people_cb), transitions={'valid':'WAIT', 'invalid':'WAIT', 'preempted':'WAIT'}) StateMachine.add('WAIT',wait(time=5,people_pass=self.people_in_sight), transitions={'succeeded':'NAV_2_END'}) StateMachine.add("NAV_2_END",nav_states['Point2'],transitions={'succeeded':'','aborted':'','preempted':''}) sm_nav3=StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_nav3: # StateMachine.add("NAV_3",nav_states['Point3'],transitions={'succeeded':'WAIT_4_CMD','aborted':'NAV_3','preempted':'NAV_3'}) sm_follow=Concurrence(outcomes=['succeeded', 'aborted'], default_outcome='succeeded', child_termination_cb=self.concurrence_child_termination_cb, outcome_cb=self.concurrence_outcome_cb) with sm_follow: Concurrence.add('START_FOLLOW',MonitorState("task_comming", xm_Task, self.follow_stop_cb)) co_follow=Concurrence(outcomes=['succeeded','aborted'], default_outcome='succeeded', child_termination_cb=self.co_follow_cb, outcome_cb=self.co_follow_outcome_cb) with co_follow: Concurrence.add('mo_L',MonitorState('people_tf_position',xm_Tracker, self.pos_M_cb)) Concurrence.add('nav', Nav2Waypoint()) Concurrence.add('FOLLOW',co_follow) StateMachine.add('WAIT_4_CMD',MonitorState("task_comming", xm_Task, self.follow_start_cb), transitions={'invalid':'FOLLOW_MODE', 'valid':'FOLLOW_MODE', 'preempted':'FOLLOW_MODE'}) StateMachine.add("FOLLOW_MODE",sm_follow,transitions={'succeeded':'','aborted':''}) sm_nav4=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"]) with sm_nav4: StateMachine.add("WAIT_4_BACK",MonitorState("task_comming",xm_Task,self.cmd_back_cb),transitions={"invalid":"NAV_4","valid":"WAIT_4_BACK","preempted":""}) StateMachine.add("NAV_4",nav_states['Point4'],transitions={'succeeded':'','aborted':'','preempted':''}) sm_nav_test=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"]) with sm_nav_test: # StateMachine.add("NAV_1",sm_nav1,transitions={'succeeded':'NAV_2','aborted':'NAV_2','preempted':'NAV_2'}) # StateMachine.add("NAV_2",sm_nav2,transitions={'succeeded':'NAV_3','aborted':'NAV_3','preempted':'NAV_3'}) StateMachine.add("NAV_3",sm_nav3,transitions={'succeeded':'NAV_4','aborted':'NAV_4','preempted':'NAV_4'}) StateMachine.add("NAV_4",sm_nav4,transitions={'succeeded':'','aborted':'','preempted':''}) sm_outcome = sm_nav_test.execute()
class main(): def __init__(self): rospy.init_node('find_treasure', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # How long do we have to get to each waypoint? self.move_base_timeout = rospy.get_param("~move_base_timeout", 10) #seconds # Initialize the patrol counter self.patrol_count = 0 # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move_base action server") # 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() # Append each of the four waypoints to the list. Each waypoint # is a pose consisting of a position and orientation in the map frame. # -0.163200, 0.044660, 0.193186 self.waypoints.append(Pose(Point(0.9579, 1.8710, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(0.7555, 0.0692, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(-0.72511, 0.4952, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(0.167730, 2.18168, 0.0), quaternions[2])) position_locations = list() position_locations.append(('position1', self.waypoints[0])) position_locations.append(('position2', self.waypoints[1])) position_locations.append(('position3', self.waypoints[2])) position_locations.append(('position4', self.waypoints[3])) self.position_locations = OrderedDict(position_locations) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) rospy.loginfo("Starting Tasks") # Initialize a number of parameters and variables # setup_task_environment(self) RVizUtils.get_instance().init_primary_waypoint_markers(self.waypoints) ''' Create individual state machines for assigning tasks to each position ''' # Initialize the overall state machine self.sm_find_treasure = StateMachine(outcomes=['succeeded','aborted','preempted']) # Build the find treasure state machine from the nav states and treasure finding states with self.sm_find_treasure: # First State Machine sm_nav = StateMachine(outcomes=['succeeded','aborted','preempted']) sm_nav.userdata.waypoints = self.waypoints sm_nav.userdata.waypoints_primary_count = len(self.waypoints) rospy.loginfo(sm_nav.userdata.waypoints) with sm_nav: # StateMachine.add('PICK_WAYPOINT', PickWaypoint(), # transitions={'succeeded':'NAV_WAYPOINT','aborted':'','preempted':''}, # remapping={'waypoint_out':'patrol_waypoint', # 'waypoints_primary_count':'waypoints_primary_count'}) StateMachine.add('NAV_WAYPOINT', NavState(), transitions={'succeeded':'NAV_WAYPOINT', 'aborted':'', 'preempted':''}, remapping={'waypoint_in':'patrol_waypoint', 'waypoints_primary_count':'waypoints_primary_count'}) # Second State Machine sm_read_tags = StateMachine(outcomes=['valid','invalid','preempted']) sm_read_tags.userdata.waypoints = self.waypoints with sm_read_tags: StateMachine.add('read_tag', ReadTagState('ar_pose_marker', AlvarMarkers), transitions={'invalid':'read_tag', 'valid':'read_tag', 'preempted':''}) # Third State Machine sm_detect_faces = StateMachine(outcomes=['valid','invalid','preempted']) with sm_detect_faces: StateMachine.add('detect_face', FaceDetectState("/camera/rgb/image_color", Image), transitions={'invalid':'detect_face', 'valid':'detect_face', 'preempted':''}) # Forth State Machine sm_recognize_faces = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_recognize_faces: StateMachine.add('recognize_face', FaceRecognitionState(), transitions={'succeeded':'', 'aborted':'', 'preempted':''}) # goal = FaceRecognitionGoal() # goal.order_id = 1 # goal.order_argument = '' # StateMachine.add('recognize_face', SimpleActionState('face_recognition', # FaceRecognitionAction, goal=goal), transitions={'succeeded':'', 'aborted':'', 'preempted':''}) sm_con = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],default_outcome='succeeded', child_termination_cb=self.child_term_cb, outcome_cb=self.out_cb) with sm_con: Concurrence.add('SM_NAV', sm_nav) Concurrence.add('SM_READ_TAGS', sm_read_tags) # Concurrence.add('SM_DETECT_FACES', sm_detect_faces) Concurrence.add('SM_RECOGNIZE_FACES', sm_recognize_faces) sm_estimate_position = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_estimate_position: StateMachine.add('estimate_pose', PoseEstimation('ar_pose_marker', AlvarMarkers), transitions={'succeeded':'', 'aborted':'estimate_pose', 'preempted':''}) StateMachine.add('POSE_ESTIMATE', sm_estimate_position, transitions={'succeeded':'CON','aborted':'CON','preempted':'CON'}) StateMachine.add('CON',sm_con, transitions={'succeeded':'','aborted':'','preempted':''}) # Create and start the SMACH introspection server sm_find_treasure intro_server = IntrospectionServer('find_treasure', self.sm_find_treasure, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm_find_treasure.execute() rospy.loginfo('the length now is') rospy.loginfo(self.waypoints) intro_server.stop() def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_find_treasure.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1) def out_cb(self,outcome_map): return 'succeeded' def child_term_cb(self,outcome_map): rospy.loginfo(outcome_map) if 'SM_NAV' in outcome_map and (outcome_map['SM_NAV'] == 'succeeded' or outcome_map['SM_NAV'] == 'aborted'): rospy.loginfo('It visited all points') return True return False
Drive(1), transitions={'success':'TURN_{0}'.format(i + 1)}) # Add all the turns for i in xrange(sides - 1): StateMachine.add('TURN_{0}'.format(i + 1), Turn(360.0 / sides), transitions={'success':'SIDE_{0}'.format(i + 2)}) # Add the final side StateMachine.add('SIDE_{0}'.format(sides), Drive(1), transitions={'success':'success'}) return polygon # END PART_1 if __name__ == '__main__': # BEGIN PART_2 triangle = polygon(3) square = polygon(4) # END PART_2 shapes = StateMachine(outcomes=['success']) with shapes: StateMachine.add('TRIANGLE', triangle, transitions={'success':'SQUARE'}) StateMachine.add('SQUARE', square, transitions={'success':'success'}) shapes.execute()
def __init__(self): rospy.init_node('clean_house', 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) # Turn the room locations into SMACH move_base action states nav_states = {} for room in self.room_locations.iterkeys(): nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.room_locations[room] move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(15.0), server_wait_timeout=rospy.Duration(10.0)) nav_states[room] = move_base_state ''' Create individual state machines for assigning tasks to each room ''' # Create a state machine for the living room subtask(s) sm_living_room = StateMachine(outcomes=['succeeded','aborted','preempted']) # Then add the subtask(s) with sm_living_room: StateMachine.add('VACUUM_FLOOR', VacuumFloor('living_room', 5), transitions={'succeeded':'','aborted':'','preempted':''}) # Create a state machine for the kitchen subtask(s) sm_kitchen = StateMachine(outcomes=['succeeded','aborted','preempted']) # Then add the subtask(s) with sm_kitchen: StateMachine.add('MOP_FLOOR', MopFloor('kitchen', 5), transitions={'succeeded':'','aborted':'','preempted':''}) # Create a state machine for the bathroom subtask(s) sm_bathroom = StateMachine(outcomes=['succeeded','aborted','preempted']) # Then add the subtasks with sm_bathroom: StateMachine.add('SCRUB_TUB', ScrubTub('bathroom', 7), transitions={'succeeded':'MOP_FLOOR'}) StateMachine.add('MOP_FLOOR', MopFloor('bathroom', 5), transitions={'succeeded':'','aborted':'','preempted':''}) # Create a state machine for the hallway subtask(s) sm_hallway = StateMachine(outcomes=['succeeded','aborted','preempted']) # Then add the subtasks with sm_hallway: StateMachine.add('VACUUM_FLOOR', VacuumFloor('hallway', 5), transitions={'succeeded':'','aborted':'','preempted':''}) # Initialize the overall state machine sm_clean_house = StateMachine(outcomes=['succeeded','aborted','preempted']) # Build the clean house state machine from the nav states and room cleaning states with sm_clean_house: StateMachine.add('START', nav_states['hallway'], transitions={'succeeded':'LIVING_ROOM','aborted':'LIVING_ROOM','preempted':'LIVING_ROOM'}) ''' Add the living room subtask(s) ''' StateMachine.add('LIVING_ROOM', nav_states['living_room'], transitions={'succeeded':'LIVING_ROOM_TASKS','aborted':'KITCHEN','preempted':'KITCHEN'}) # When the tasks are done, continue on to the kitchen StateMachine.add('LIVING_ROOM_TASKS', sm_living_room, transitions={'succeeded':'KITCHEN','aborted':'KITCHEN','preempted':'KITCHEN'}) ''' Add the kitchen subtask(s) ''' StateMachine.add('KITCHEN', nav_states['kitchen'], transitions={'succeeded':'KITCHEN_TASKS','aborted':'BATHROOM','preempted':'BATHROOM'}) # When the tasks are done, continue on to the bathroom StateMachine.add('KITCHEN_TASKS', sm_kitchen, transitions={'succeeded':'BATHROOM','aborted':'BATHROOM','preempted':'BATHROOM'}) ''' Add the bathroom subtask(s) ''' StateMachine.add('BATHROOM', nav_states['bathroom'], transitions={'succeeded':'BATHROOM_TASKS','aborted':'HALLWAY','preempted':'HALLWAY'}) # When the tasks are done, return to the hallway StateMachine.add('BATHROOM_TASKS', sm_bathroom, transitions={'succeeded':'HALLWAY','aborted':'HALLWAY','preempted':'HALLWAY'}) ''' Add the hallway subtask(s) ''' StateMachine.add('HALLWAY', nav_states['hallway'], transitions={'succeeded':'HALLWAY_TASKS','aborted':'','preempted':''}) # When the tasks are done, stop StateMachine.add('HALLWAY_TASKS', sm_hallway, transitions={'succeeded':'','aborted':'','preempted':''}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('clean_house', sm_clean_house, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = sm_clean_house.execute() if len(task_list) > 0: message = "Ooops! Not all chores were completed." message += "The following rooms need to be revisited: " message += str(task_list) else: message = "All chores complete!" rospy.loginfo(message) easygui.msgbox(message, title="Finished Cleaning") intro_server.stop()
def __init__(self): rospy.init_node("deliver_food", anonymous=False) self.initialize_destination() self.lalala = 100 # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 nav_states = {} for room in self.room_locations.iterkeys(): nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = "map" nav_goal.target_pose.pose = self.room_locations[room] move_base_state = SimpleActionState( "move_base", MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(60.0), server_wait_timeout=rospy.Duration(10.0), ) nav_states[room] = move_base_state rospy.loginfo( room + " -> [" + str(round(self.room_locations[room].position.x, 2)) + ", " + str(round(self.room_locations[room].position.y, 2)) + "]" ) sm_rotate_search = Concurrence( outcomes=["find", "not_find"], default_outcome="not_find", child_termination_cb=self.concurrence_child_termination_callback, outcome_cb=self.concurrence_outcome_callback, ) with sm_rotate_search: Concurrence.add("ROTATE", Rotate360(0.4, 2 * pi)) Concurrence.add("SEARCH", SearchTable()) sm_table1 = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with sm_table1: StateMachine.add( "GOTO_TABLE1", nav_states["table1"], transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"}, ) StateMachine.add( "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"} ) StateMachine.add( "DO_STUFFS", DoStuffs(5), transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""} ) sm_table2 = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with sm_table2: StateMachine.add( "GOTO_TABLE2", nav_states["table2"], transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"}, ) StateMachine.add( "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"} ) StateMachine.add( "DO_STUFFS", DoStuffs(5), transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""} ) sm_table3 = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with sm_table3: StateMachine.add( "GOTO_TABLE3", nav_states["table3"], transitions={ "succeeded": "ROTATE_SEARCH", "aborted": "GOTO_CHECKPOINT_2", "preempted": "GOTO_CHECKPOINT_2", }, ) StateMachine.add( "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"} ) StateMachine.add( "DO_STUFFS", DoStuffs(5), transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a"} ) # if something wrong when we tried to go to the table StateMachine.add( "GOTO_CHECKPOINT_2", nav_states["checkpoint2"], transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_CHECKPOINT_1"}, ) StateMachine.add( "GOTO_CHECKPOINT_1", nav_states["checkpoint1"], transitions={"succeeded": "GOTO_CHECKPOINT_2z", "aborted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_CHECKPOINT_2z", nav_states["checkpoint1"], transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_TABLE3_2", nav_states["table3"], transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"}, ) StateMachine.add( "CLEARING_NOISE_GO", Rotate360(0.8, 2 * pi), transitions={"full_rotate": "GOTO_TABLE3_3", "not_full_rotate": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_TABLE3_3", nav_states["table3"], transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"}, ) # if something wrong when we tried to go back to kitchen StateMachine.add( "GOTO_CHECKPOINT_1a", nav_states["checkpoint1"], transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"}, ) StateMachine.add( "GOTO_CHECKPOINT_2a", nav_states["checkpoint2"], transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"}, ) StateMachine.add( "CLEARING_NOISE_BACK_a", Rotate360(0.8, 2 * pi), transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"}, ) StateMachine.add( "GOTO_KITCHEN_2", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"}, ) StateMachine.add( "CLEARING_NOISE_BACK_b", Rotate360(0.8, 2 * pi), transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"} ) sm_table4 = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with sm_table4: StateMachine.add( "GOTO_TABLE4", nav_states["table4"], transitions={ "succeeded": "ROTATE_SEARCH", "aborted": "GOTO_CHECKPOINT_2", "preempted": "GOTO_CHECKPOINT_2", }, ) StateMachine.add( "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"} ) StateMachine.add( "DO_STUFFS", DoStuffs(5), transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"}, ) # if something wrong when we tried to go to the table StateMachine.add( "GOTO_CHECKPOINT_2", nav_states["checkpoint2"], transitions={"succeeded": "GOTO_TABLE4_2", "aborted": "GOTO_CHECKPOINT_1"}, ) StateMachine.add( "GOTO_CHECKPOINT_1", nav_states["checkpoint1"], transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_TABLE4_2", nav_states["table4"], transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"}, ) StateMachine.add( "CLEARING_NOISE_GO", Rotate360(0.4, 2 * pi), transitions={"full_rotate": "GOTO_TABLE4_3", "not_full_rotate": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_TABLE4_3", nav_states["table4"], transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"}, ) # if something wrong when we tried to go back to kitchen StateMachine.add( "GOTO_CHECKPOINT_1a", nav_states["checkpoint1"], transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"}, ) StateMachine.add( "GOTO_CHECKPOINT_2a", nav_states["checkpoint2"], transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"}, ) StateMachine.add( "CLEARING_NOISE_BACK_a", Rotate360(0.8, 2 * pi), transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"}, ) StateMachine.add( "GOTO_KITCHEN_2", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"}, ) StateMachine.add( "CLEARING_NOISE_BACK_b", Rotate360(0.4, 2 * pi), transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"} ) sm_table5 = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with sm_table5: StateMachine.add( "GOTO_TABLE5", nav_states["table5"], transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"}, ) StateMachine.add( "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"} ) StateMachine.add( "DO_STUFFS", DoStuffs(5), transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""} ) sm_table6 = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with sm_table6: StateMachine.add( "GOTO_TABLE6", nav_states["table6"], transitions={ "succeeded": "ROTATE_SEARCH", "aborted": "GOTO_CHECKPOINT_2", "preempted": "GOTO_CHECKPOINT_2", }, ) StateMachine.add( "ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"} ) StateMachine.add( "DO_STUFFS", DoStuffs(5), transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"}, ) # if something wrong when we tried to go to the table StateMachine.add( "GOTO_CHECKPOINT_2", nav_states["checkpoint2"], transitions={"succeeded": "GOTO_TABLE6_2", "aborted": "GOTO_CHECKPOINT_1"}, ) StateMachine.add( "GOTO_CHECKPOINT_1", nav_states["checkpoint1"], transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_TABLE6_2", nav_states["table6"], transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"}, ) StateMachine.add( "CLEARING_NOISE_GO", Rotate360(0.8, 2 * pi), transitions={"full_rotate": "GOTO_TABLE6_3", "not_full_rotate": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_TABLE6_3", nav_states["table6"], transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"}, ) # if something wrong when we tried to go back to kitchen StateMachine.add( "GOTO_CHECKPOINT_1a", nav_states["checkpoint1"], transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"}, ) StateMachine.add( "GOTO_CHECKPOINT_2a", nav_states["checkpoint2"], transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"}, ) StateMachine.add( "CLEARING_NOISE_BACK_a", Rotate360(0.8, 2 * pi), transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"}, ) StateMachine.add( "GOTO_KITCHEN_2", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"}, ) StateMachine.add( "CLEARING_NOISE_BACK_b", Rotate360(0.8, 2 * pi), transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"}, ) StateMachine.add( "GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"} ) # let's initialize the overall state machine sm_deliverfood = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with sm_deliverfood: StateMachine.add("STARTING_TASK", Welcome(), transitions={"succeeded": "COMPUTER_VISION_TASK"}) StateMachine.add( "COMPUTER_VISION_TASK", ComputerVision(), transitions={ "detect1": "TABLE_ONE_TASK", "detect2": "TABLE_TWO_TASK", "detect3": "TABLE_THREE_TASK", "detect4": "TABLE_FOUR_TASK", "detect5": "TABLE_FIVE_TASK", "detect6": "TABLE_SIX_TASK", "preempted": "", }, ) StateMachine.add( "TABLE_ONE_TASK", sm_table1, transitions={ "succeeded": "COMPUTER_VISION_TASK", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN", }, ) StateMachine.add( "TABLE_TWO_TASK", sm_table2, transitions={ "succeeded": "COMPUTER_VISION_TASK", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN", }, ) StateMachine.add( "TABLE_THREE_TASK", sm_table3, transitions={ "succeeded": "COMPUTER_VISION_TASK", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN", }, ) StateMachine.add( "TABLE_FOUR_TASK", sm_table4, transitions={ "succeeded": "COMPUTER_VISION_TASK", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN", }, ) StateMachine.add( "TABLE_FIVE_TASK", sm_table5, transitions={ "succeeded": "COMPUTER_VISION_TASK", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN", }, ) StateMachine.add( "TABLE_SIX_TASK", sm_table6, transitions={ "succeeded": "COMPUTER_VISION_TASK", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN", }, ) StateMachine.add( "GOTO_KITCHEN", nav_states["kitchen"], transitions={ "succeeded": "COMPUTER_VISION_TASK", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN", }, ) # Create and start the SMACH introspection server intro_server = IntrospectionServer("deliver_food", sm_deliverfood, "/SM_ROOT") intro_server.start() # Execute the state machine sm_outcome = sm_deliverfood.execute() rospy.on_shutdown(self.shutdown)
class main: def __init__(self): rospy.init_node("follow") rospy.on_shutdown(self.shutdown) self.state = None self.sm_follow = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) # 总状态机follow with self.sm_follow: self.co_follow = Concurrence( outcomes=["succeeded", "preempted", "aborted"], default_outcome="succeeded", # outcome_cb = self.co_follow_outcome_cb , # child_termination_cb=self.co_follow_child_cb ) with self.co_follow: Concurrence.add("mo_L", MonitorState("people_position_estimation", PositionMeasurement, self.pos_M_cb)) Concurrence.add("nav", Nav2Waypoint()) # self.sm_nav=StateMachine(outcomes=('succeeded','aborted','preempted')) # with self.sm_nav: # # StateMachine.add('wait',MonitorState('sr_data',Int16,self.wait_cb),transitions={'valid':'wait','invalid':'nav_speech','preempted':'nav_speech'}) # # self.speech_nav=Concurrence(outcomes=['get_in','succeeded'], # default_outcome='succeeded', # outcome_cb=self.speech_nav_outcome_cb, # child_termination_cb=self.speech_nav_child_termination_cb # ) # with self.speech_nav: # Concurrence.add('speech',MonitorState('sr_data',Int16,self.nav_speech_cb)) # Concurrence.add('nav', Nav2Waypoint()) # StateMachine.add('nav_speech',self.speech_nav,transitions={'get_in':'Get_in',}) # self.get_in_speech=Concurrence(outcomes=['get_out','succeeded'], # default_outcome='succeeded', # outcome_cb=self.speech_get_in_outcome_cb, # child_termination_cb=self.speech_get_in_child_termination_cb # # ) # with self.get_in_speech: # Concurrence.add('speech',MonitorState('sr_data',Int16,self.get_in_speech_cb)) # Concurrence.add('get_in',Get_in()) # StateMachine.add('Get_in',self.get_in_speech,transitions={'get_out':'Get_out'}) # StateMachine.add('Get_out',Get_out(), transitions={'succeeded':'nav_speech','preempted':'','aborted':'Get_out' }) # Concurrence.add('Nav',self.sm_nav) StateMachine.add("Nav_top", self.co_follow) a = self.sm_follow.execute() def pos_M_cb(self, userdata, msg): global current_people_pose, updata_flag # lock= threading.Lock() # with lock: if msg.pos is not None: current_people_pose = msg.pos updata_flag += 1 return True def wait_cb(self, userdata, msg): global FOLLOW if msg.data == FOLLOW: return False else: return True def nav_speech_cb(self, userdata, msg): global GET_IN, GET_OUT if msg.data == GET_IN: self.state = "get_in" return False else: return True def get_in_speech_cb(self, userdata, msg): global GET_IN, GET_OUT if msg.data == GET_OUT: self.state = "get_out" return False else: return True def speech_nav_outcome_cb(self, outcome_map): if outcome_map["speech"] == "invalid": if self.state == "get_in": return "get_in" def speech_nav_child_termination_cb(self, outcome_map): if outcome_map["speech"] == "invalid": return True def speech_get_in_outcome_cb(self, outcome_map): if outcome_map["speech"] == "invalid": if self.state == "get_out": return "get_out" def speech_get_in_child_termination_cb(self, outcome_map): if outcome_map["speech"] == "invalid": return True def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_follow.request_preempt() rospy.sleep(1) pass
def execute(self, userdata): print 'one' sleep(1) return 'success' # One of the outcomes should be returned class Two(State): def __init__(self): # Call the parent class constructor, passing all the possible outcomes of the state State.__init__(self, outcomes=['success']) # execute method - where the actions are descripted def execute(self, userdata): print 'two' sleep(1) return 'success' # One of the outcomes should be returned if __name__ == '__main__': # Create a StateMachine, passing a list of its possible overall outcomes sm = StateMachine(outcomes=['success']) # Populate the state machine with our states with sm: # (state_name, state_instance, transitions={dict with the transitions}) StateMachine.add('ONE', One(), transitions={'success':'TWO'}) StateMachine.add('TWO', Two(), transitions={'success':'ONE'}) # Execute the state machine sm.execute()
class main(): def __init__(self): rospy.init_node('patrol_smach', anonymous=False) # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # Initialize a number of parameters and variables self.init() # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move_base action server") # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 self.n_patrols = 2 # Turn the waypoints into SMACH states 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, 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) move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(10.0)) # Initialize the top level state machine self.sm = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm: # Initialize the iterator self.sm_patrol_iterator = Iterator(outcomes = ['succeeded','preempted','aborted'], input_keys = [], it = lambda: range(0, self.n_patrols), output_keys = [], it_label = 'index', exhausted_outcome = 'succeeded') with self.sm_patrol_iterator: # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted','continue']) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'}) StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'}) StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'}) StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'}) StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'continue','aborted':'continue','preempted':'continue'}) # Close the sm_patrol machine and add it to the iterator Iterator.set_contained_state('PATROL_STATE', self.sm_patrol, loop_outcomes=['continue']) # Close the top level state machine StateMachine.add('PATROL_ITERATOR', self.sm_patrol_iterator, {'succeeded':'succeeded', 'aborted':'aborted'}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('patrol', self.sm, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = self.sm.execute() rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) intro_server.stop() def move_base_result_cb(self, userdata, status, result): if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def init(self): # How big is the square we want the robot to patrol? self.square_size = rospy.get_param("~square_size", 1.0) # meters # How many times should we execute the patrol loop self.n_patrols = rospy.get_param("~n_patrols", 3) # meters # 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() # 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])) # Initialize the waypoint visualization markers for RViz self.init_waypoint_markers() # Set a visualization marker at each waypoint for waypoint in self.waypoints: p = Point() p = waypoint.position self.waypoint_markers.points.append(p) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) rospy.loginfo("Starting SMACH test") # Publish the waypoint markers self.marker_pub.publish(self.waypoint_markers) rospy.sleep(1) self.marker_pub.publish(self.waypoint_markers) def init_waypoint_markers(self): # Set up our waypoint markers marker_scale = 0.2 marker_lifetime = 0 # 0 is forever marker_ns = 'waypoints' marker_id = 0 marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} # Define a marker publisher. self.marker_pub = rospy.Publisher('waypoint_markers', Marker) # Initialize the marker points list. self.waypoint_markers = Marker() self.waypoint_markers.ns = marker_ns self.waypoint_markers.id = marker_id self.waypoint_markers.type = Marker.CUBE_LIST self.waypoint_markers.action = Marker.ADD self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime) self.waypoint_markers.scale.x = marker_scale self.waypoint_markers.scale.y = marker_scale self.waypoint_markers.color.r = marker_color['r'] self.waypoint_markers.color.g = marker_color['g'] self.waypoint_markers.color.b = marker_color['b'] self.waypoint_markers.color.a = marker_color['a'] self.waypoint_markers.header.frame_id = 'odom' self.waypoint_markers.header.stamp = rospy.Time.now() self.waypoint_markers.points = list() def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class SMACHAI(): 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 time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def start_cb(self, userdata, msg): rospy.loginfo("Start !") return False def color_cb(self, userdata, msg): rospy.loginfo("Color " + str(msg.data)) self.robot_side = msg.data self.sm_action1.userdata.robot_side = self.robot_side self.sm_action2.userdata.robot_side = self.robot_side self.sm_action3.userdata.robot_side = self.robot_side self.sm_action4.userdata.robot_side = self.robot_side self.sm_action5.userdata.robot_side = self.robot_side self.sm_action6.userdata.robot_side = self.robot_side self.sm_action7.userdata.robot_side = self.robot_side self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE return False def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True def objective_cb(self, userdata, response): #objective_response = GetObjective().Response userdata.waypoint_out = response.goal waypoint_type = response.type.data rospy.loginfo("goal: " + str(response.goal)) if(waypoint_type == 1): return 'action1' if(waypoint_type == 2): return 'action2' if(waypoint_type == 3): return 'action3' if(waypoint_type == 4): return 'action4' if(waypoint_type == 5): return 'action5' if(waypoint_type == 6): return 'action6' if(waypoint_type == 7): return 'action7' return 'aborted' def requestPrioCube_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300) update_request.goal.pose.position.y = 1.000 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop cubes update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300) #request.goal.pose.position.y = 1.000 #request.prio = 100 #return request def updatePrioCube_cb(self, userdata, response): rospy.loginfo("Priority Drop cubes updated") return 'aborted' def requestPrioShell_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) update_request.goal.pose.position.y = 0.800 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop shell update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) #request.goal.pose.position.y = 0.800 #request.prio = 100 #return request def updatePrioShell_cb(self, userdata, response): rospy.loginfo("Priority Drop shell updated") return 'aborted' # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_ACTIONS'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("LOW TIME! NEED TO STOP...") return True if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO STOP...") return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ") return 'stop' if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'stop' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_ACTIONS'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) #return 'stop' # Recharge if all else fails else: return 'recharge' def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_actions.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class SMACHAI(): 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 = (0, pi, pi/2, -pi/2, pi/4, 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.4, 0.65, 0.0), quaternions[0])) self.waypoints.append(Pose(Point(-1.0, 0.50, 0.0), quaternions[1])) self.waypoints.append(Pose(Point(-0.4, 0.35, 0.0), quaternions[2])) self.waypoints.append(Pose(Point(-0.8, 0.65, 0.0), quaternions[3])) self.waypoints.append(Pose(Point(-1.2, 0.30, 0.0), quaternions[4])) self.waypoints.append(Pose(Point(-0.4, 0.657, 0.0), quaternions[5])) # 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 Actions self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted']) with self.sm_actions: # Calib X StateMachine.add('CALIBRATE_X', CalibX(), transitions={'succeeded':'CALIBRATE_Y', 'aborted':'aborted'}) # Calib Y StateMachine.add('CALIBRATE_Y', CalibY(), transitions={'succeeded':'MAX_SPEED_1', 'aborted':'aborted'}) # PICK-UP # Change speed StateMachine.add('MAX_SPEED_1', MaxLinearSpeed(70000), transitions={'succeeded':'GOTO_1_1', 'aborted':'aborted'}) # NavPoint 1 StateMachine.add('GOTO_1_1', Nav2Waypoint(self.waypoints[0]), transitions={'succeeded':'MAX_SPEED_2', 'aborted':'aborted'}) # Change speed StateMachine.add('MAX_SPEED_2', MaxLinearSpeed(40000), transitions={'succeeded':'PAUSE_1', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_1', Pause(1.0), transitions={'succeeded':'FORWARD_1', 'aborted':'aborted'}) # Avancer StateMachine.add('FORWARD_1', MoveForward(0.15), transitions={'succeeded':'FORKS_10', 'aborted':'aborted'}) # Monter fourches StateMachine.add('FORKS_10', Forks(90), transitions={'succeeded':'FORKS_11', 'aborted':'aborted'}) StateMachine.add('FORKS_11', Forks(87), transitions={'succeeded':'FORKS_12', 'aborted':'aborted'}) StateMachine.add('FORKS_12', Forks(83), transitions={'succeeded':'BACKWARD_1', 'aborted':'aborted'}) # Reculer StateMachine.add('BACKWARD_1', MoveForward(-0.15), transitions={'succeeded':'ROTATE_1', 'aborted':'aborted'}) # Rotation StateMachine.add('ROTATE_1', MoveRotate(pi), transitions={'succeeded':'MAX_SPEED_3', 'aborted':'aborted'}) # DROP-OFF # Change speed StateMachine.add('MAX_SPEED_3', MaxLinearSpeed(70000), transitions={'succeeded':'GOTO_2_1', 'aborted':'aborted'}) # NavPoint 2 StateMachine.add('GOTO_2_1', Nav2Waypoint(self.waypoints[1]), transitions={'succeeded':'MAX_SPEED_4', 'aborted':'aborted'}) # Change speed StateMachine.add('MAX_SPEED_4', MaxLinearSpeed(40000), transitions={'succeeded':'PAUSE_2', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_2', Pause(1.0), transitions={'succeeded':'FORWARD_2', 'aborted':'aborted'}) # Avancer StateMachine.add('FORWARD_2', MoveForward(0.13), transitions={'succeeded':'FORKS_20', 'aborted':'aborted'}) # Baisser fourches StateMachine.add('FORKS_20', Forks(87), transitions={'succeeded':'FORKS_21', 'aborted':'aborted'}) StateMachine.add('FORKS_21', Forks(91), transitions={'succeeded':'FORKS_22', 'aborted':'aborted'}) StateMachine.add('FORKS_22', Forks(95), transitions={'succeeded':'BACKWARD_2', 'aborted':'aborted'}) # Reculer StateMachine.add('BACKWARD_2', MoveForward(-0.15), transitions={'succeeded':'ROTATE_2', 'aborted':'aborted'}) # Rotation StateMachine.add('ROTATE_2', MoveRotate(0), transitions={'succeeded':'GOTO_3_1', 'aborted':'aborted'}) # NavPoint 3 StateMachine.add('GOTO_3_1', Nav2Waypoint(self.waypoints[2]), transitions={'succeeded':'PAUSE_3', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_3', Pause(2.0), transitions={'succeeded':'GOTO_4_1', 'aborted':'aborted'}) # NavPoint 4 StateMachine.add('GOTO_4_1', Nav2Waypoint(self.waypoints[3]), transitions={'succeeded':'PAUSE_4', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_4', Pause(2.0), transitions={'succeeded':'GOTO_2_2', 'aborted':'aborted'}) # PICK-UP # NavPoint 2 StateMachine.add('GOTO_2_2', Nav2Waypoint(self.waypoints[1]), transitions={'succeeded':'PAUSE_5', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_5', Pause(1.0), transitions={'succeeded':'FORWARD_3', 'aborted':'aborted'}) # Avancer StateMachine.add('FORWARD_3', MoveForward(0.15), transitions={'succeeded':'FORKS_30', 'aborted':'aborted'}) # Monter fourches StateMachine.add('FORKS_30', Forks(90), transitions={'succeeded':'FORKS_31', 'aborted':'aborted'}) StateMachine.add('FORKS_31', Forks(87), transitions={'succeeded':'FORKS_32', 'aborted':'aborted'}) StateMachine.add('FORKS_32', Forks(83), transitions={'succeeded':'BACKWARD_3', 'aborted':'aborted'}) # Reculer StateMachine.add('BACKWARD_3', MoveForward(-0.15), transitions={'succeeded':'ROTATE_3', 'aborted':'aborted'}) # Rotation StateMachine.add('ROTATE_3', MoveRotate(0), transitions={'succeeded':'GOTO_1_2', 'aborted':'aborted'}) # DROP-OFF # NavPoint 1 StateMachine.add('GOTO_1_2', Nav2Waypoint(self.waypoints[5]), transitions={'succeeded':'PAUSE_6', 'aborted':'aborted'}) # Pause StateMachine.add('PAUSE_6', Pause(1.0), transitions={'succeeded':'FORWARD_4', 'aborted':'aborted'}) # Avancer StateMachine.add('FORWARD_4', MoveForward(0.13), transitions={'succeeded':'FORKS_40', 'aborted':'aborted'}) # Baisser fourches StateMachine.add('FORKS_40', Forks(87), transitions={'succeeded':'FORKS_41', 'aborted':'aborted'}) StateMachine.add('FORKS_41', Forks(91), transitions={'succeeded':'FORKS_42', 'aborted':'aborted'}) StateMachine.add('FORKS_42', Forks(95), transitions={'succeeded':'BACKWARD_4', 'aborted':'aborted'}) # Reculer StateMachine.add('BACKWARD_4', MoveForward(-0.15), transitions={'succeeded':'ROTATE_4', 'aborted':'aborted'}) # Rotation StateMachine.add('ROTATE_4', MoveRotate(pi), transitions={'succeeded':'GOTO_5_1', 'aborted':'aborted'}) # NavPoint 5 StateMachine.add('GOTO_5_1', Nav2Waypoint(self.waypoints[4]), transitions={'succeeded':'CALIBRATE_X', '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('ACTIONS', self.sm_actions, transitions={'succeeded':'ACTIONS', 'aborted':'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 time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def start_cb(self, userdata, msg): rospy.loginfo("Start !") return False def color_cb(self, userdata, msg): rospy.loginfo("Color " + str(msg.data)) self.robot_side = msg.data self.sm_action1.userdata.robot_side = self.robot_side self.sm_action2.userdata.robot_side = self.robot_side self.sm_action3.userdata.robot_side = self.robot_side self.sm_action4.userdata.robot_side = self.robot_side self.sm_action5.userdata.robot_side = self.robot_side self.sm_action6.userdata.robot_side = self.robot_side self.sm_action7.userdata.robot_side = self.robot_side self.sm_top.userdata.robot_side = self.robot_side # TODO REMOVE return False def battery_cb(self, userdata, msg): if msg.data < 320: self.recharging = True return False else: self.recharging = False return True def objective_cb(self, userdata, response): #objective_response = GetObjective().Response userdata.waypoint_out = response.goal waypoint_type = response.type.data rospy.loginfo("goal: " + str(response.goal)) if(waypoint_type == 1): return 'action1' if(waypoint_type == 2): return 'action2' if(waypoint_type == 3): return 'action3' if(waypoint_type == 4): return 'action4' if(waypoint_type == 5): return 'action5' if(waypoint_type == 6): return 'action6' if(waypoint_type == 7): return 'action7' return 'aborted' def requestPrioCube_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300) update_request.goal.pose.position.y = 1.000 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop cubes update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 1.300) #request.goal.pose.position.y = 1.000 #request.prio = 100 #return request def updatePrioCube_cb(self, userdata, response): rospy.loginfo("Priority Drop cubes updated") return 'aborted' def requestPrioShell_cb(self, userdata, request): rospy.loginfo("Side : " + str(userdata.robot_side)) update_request = UpdatePriorityRequest() update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) update_request.goal.pose.position.y = 0.800 update_request.prio.data = 100 rospy.loginfo("Request Priority Drop shell update") return update_request #request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300) #request.goal.pose.position.y = 0.800 #request.prio = 100 #return request def updatePrioShell_cb(self, userdata, response): rospy.loginfo("Priority Drop shell updated") return 'aborted' # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_ACTIONS'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("LOW TIME! NEED TO STOP...") return True if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO STOP...") return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_TIME'] == 'invalid': rospy.loginfo("TIME FINISHED !! GOING TO STOP ! ") return 'stop' if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'stop' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_ACTIONS'] == 'succeeded': #self.patrol_count += 1 #rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning #if self.n_patrols == -1 or self.patrol_count < self.n_patrols: #self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' #else: #self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) #return 'stop' # Recharge if all else fails else: return 'recharge' def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_actions.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
class Patrol: def __init__(self): rospy.init_node("patrol_smach", 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 list to hold then navigation waypoints 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) # Initialize the patrol state machine self.sm_patrol = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) # Add the states to the state machine with the appropriate transitions with self.sm_patrol: StateMachine.add( "NAV_STATE_0", nav_states[0], transitions={"succeeded": "NAV_STATE_1", "aborted": "NAV_STATE_1", "preempted": "NAV_STATE_1"}, ) StateMachine.add( "NAV_STATE_1", nav_states[1], transitions={"succeeded": "NAV_STATE_2", "aborted": "NAV_STATE_2", "preempted": "NAV_STATE_2"}, ) StateMachine.add( "NAV_STATE_2", nav_states[2], transitions={"succeeded": "NAV_STATE_3", "aborted": "NAV_STATE_3", "preempted": "NAV_STATE_3"}, ) StateMachine.add( "NAV_STATE_3", nav_states[3], transitions={"succeeded": "NAV_STATE_4", "aborted": "NAV_STATE_4", "preempted": "NAV_STATE_4"}, ) StateMachine.add( "NAV_STATE_4", nav_states[0], transitions={"succeeded": "", "aborted": "", "preempted": ""} ) # Create and start the SMACH introspection server intro_server = IntrospectionServer("patrol", self.sm_patrol, "/SM_ROOT") intro_server.start() # Execute the state machine for the specified number of patrols while (self.n_patrols == -1 or self.patrol_count < self.n_patrols) and not rospy.is_shutdown(): sm_outcome = self.sm_patrol.execute() self.patrol_count += 1 rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) rospy.loginfo("State Machine Outcome: " + str(sm_outcome)) intro_server.stop() def move_base_result_cb(self, userdata, status, result): if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo( "Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted)) ) except: pass def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_patrol.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
def __init__(self): global DIS_CHANGED, ANGLE_CHANGED, OBJECT_DIS, ARM_LENGTH, OBJECT_POS, AR_POS,OBJECT_NAME,OBJECT_ID rospy.init_node("object", anonymous=False) rospy.on_shutdown(self.shutdown) id_list=[3,1,4] self.monitor_times = 0 change2base = ChangeMode("base") change2arm = ChangeMode("arm") wrist_goal = xm_WristPositionGoal() wrist_goal.angle = 0.785 wrist_up_state = SimpleActionState("xm_move_wrist", xm_WristPositionAction, goal=wrist_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(20), server_wait_timeout=rospy.Duration(20)) wrist_goal = xm_WristPositionGoal() wrist_goal.angle = 0 wrist_down_state = SimpleActionState("xm_move_wrist", xm_WristPositionAction, goal=wrist_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(20), server_wait_timeout=rospy.Duration(20)) gripper_goal = xm_GripperCommandGoal() gripper_goal.command.position = 0.15 gripper_open_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10)) # gripper_goal = xm_GripperCommandGoal() gripper_goal.command.position = 0.05 gripper_close_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10)) init_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted","valid","invalid"]) with init_sm: StateMachine.add("CHANGE_2_ARM0", change2arm, transitions={'succeeded': "INIT","aborted":"CHANGE_2_ARM0"}) StateMachine.add("INIT", ServiceState('xm_move_arm_position/move_position', xm_ArmPosition, "init", response_cb=self.responce_cb), transitions={'succeeded': 'WRIST_DOWN0'}) StateMachine.add("WRIST_DOWN0", wrist_down_state, transitions={"succeeded": "CLOSE_GRIPPER0"}) StateMachine.add("CLOSE_GRIPPER0", gripper_close_state, transitions={'succeeded': 'CHANGE_2_BASE0'}) StateMachine.add("CHANGE_2_BASE0", change2base, transitions={"succeeded": "SPEAK_READY","aborted":"CHANGE_2_BASE0"}) # StateMachine.add("WAIT_4_START", MonitorState("task_comming", xm_Task, self.start_cb), # transitions={'invalid': 'SPEAK_READY', # 'valid': 'WAIT_4_START', # 'preempted': ''}) StateMachine.add("SPEAK_READY",Speak(str="I am ready,,test start"),transitions={'succeeded': ''}) init_sm_again=StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with init_sm_again: StateMachine.add("CHANGE_2_ARM0", change2arm, transitions={'succeeded': "INIT","aborted":"CHANGE_2_ARM0"}) StateMachine.add("INIT", ServiceState('xm_move_arm_position/move_position', xm_ArmPosition, "init", response_cb=self.responce_cb), transitions={'succeeded': 'WRIST_DOWN0'}) StateMachine.add("WRIST_DOWN0", wrist_down_state, transitions={"succeeded": "CLOSE_GRIPPER0"}) StateMachine.add("CLOSE_GRIPPER0", gripper_close_state, transitions={'succeeded': 'CHANGE_2_BASE0'}) StateMachine.add("CHANGE_2_BASE0", change2base, transitions={"succeeded": "SPEAK_READY","aborted":"CHANGE_2_BASE0"}) StateMachine.add("SPEAK_READY",Speak(str=" test start again"),transitions={'succeeded': ''}) find_object_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) find_object_sm.userdata.object = 1 with find_object_sm: StateMachine.add("NAV_2_START_POS", Forword_BacK(dis=0.5), transitions={"succeeded": "FIND_OBJECT1"}) StateMachine.add("FIND_OBJECT1", Find_Object(), transitions={"succeeded": "CALCULATE_ANGLE1", 'aborted': 'FIND_OBJECT1'}, ) StateMachine.add("CALCULATE_ANGLE1", Calculate_angle(), transitions={"succeeded": "CHANGE_ANGLE1"}, remapping={'angle': 'angle_out'}) StateMachine.add("CHANGE_ANGLE1", ChangeAngular(), transitions={"succeeded": "FIND_OBJECT2"}, remapping={"angle_in": 'angle_out'}) StateMachine.add("FIND_OBJECT2", Find_Object(), transitions={"succeeded": "SPEAK_OBJECT", 'aborted': 'FIND_OBJECT2'}, ) StateMachine.add("SPEAK_OBJECT",Speak(mode=1),transitions={'succeeded': 'CALCULATE_ANGLE2'}) StateMachine.add("CALCULATE_ANGLE2", Calculate_angle(), transitions={"succeeded": "CHANGE_ANGLE2"}, remapping={'angle': 'angle_out'}) StateMachine.add("CHANGE_ANGLE2", ChangeAngular(), transitions={"succeeded": ""}, remapping={"angle_in": 'angle_out'}) grasp_ready_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with grasp_ready_sm: StateMachine.add("SPEAK_GRASP",Speak(mode=2),transitions={'succeeded': 'BACK'}) StateMachine.add("BACK", Forword_BacK(dis=-0.5), transitions={'succeeded': 'CHANGE_2_ARM1'}) StateMachine.add("CHANGE_2_ARM1", change2arm, transitions={'succeeded': "READY","aborted":"CHANGE_2_ARM1"}) StateMachine.add("READY", ServiceState('xm_move_arm_position/move_position', xm_ArmPosition, "prepare", response_cb=self.responce_cb), transitions={'succeeded': 'OPEN_GRIPER'}) StateMachine.add("OPEN_GRIPER", gripper_open_state, transitions={"succeeded": "CHANGE_HEIGHT1"}) StateMachine.add("CHANGE_HEIGHT1", Lift_HEIGHT(height=-0.06), transitions={"succeeded": "AR_TARGET"}) StateMachine.add("AR_TARGET", MonitorState("ar_pose_marker", AlvarMarkers, self.get_mark_pos_cb), transitions={"valid": "AR_TARGET", "invalid": "CHANGE_2_BASE1"}) StateMachine.add("CHANGE_2_BASE1", change2base, transitions={"succeeded": "CALCULATE_DIS","aborted":"CHANGE_2_BASE1"}) StateMachine.add("CALCULATE_DIS", Calculate_length(), transitions={'succeeded': ''}) grasp_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with grasp_sm: StateMachine.add("FORWORD", Forword_BacK_Global(), transitions={"succeeded": "CLOSE_GRIPPER"}) StateMachine.add("CLOSE_GRIPPER", CLOSE_GRIPPER(), transitions={'succeeded': 'WRIST_UP'}) StateMachine.add("WRIST_UP", wrist_up_state, transitions={"succeeded": "CHANGE_2_BASE2"}) StateMachine.add("CHANGE_2_BASE2", change2base, transitions={'succeeded': "BACK2","aborted":"CHANGE_2_BASE2"}) StateMachine.add("BACK2", Forword_BacK_Global(dis=1), transitions={'succeeded': ''}) place_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with place_sm: StateMachine.add("SPEAK_PLACE",Speak(str='I am try to place it'),transitions={'succeeded': 'CHANGE_2_ARM1'}) StateMachine.add("CHANGE_2_ARM1", change2arm, transitions={'succeeded': "CHANGE_HEIGHT","aborted":"CHANGE_2_ARM1"}) StateMachine.add("CHANGE_HEIGHT", Lift_HEIGHT(height=0.38), transitions={"succeeded": "CHANGE_2_BASE2"}) StateMachine.add("CHANGE_2_BASE2", change2base, transitions={"succeeded": "CALCULATE_DIS2","aborted":"CHANGE_2_BASE2"}) # StateMachine.add("AR_TARGET2", MonitorState("ar_pose_marker", AlvarMarkers, self.get_mark_pos_cb), # transitions={"valid": "AR_TARGET2", "invalid": "CALCULATE_DIS2"}) StateMachine.add("CALCULATE_DIS2", Calculate_length_place(), transitions={"succeeded": "FORWORD2"}) StateMachine.add("FORWORD2", Forword_BacK_Global(), transitions={"succeeded": "CHANGE_2_ARM2"}) StateMachine.add("CHANGE_2_ARM2", change2arm, transitions={'succeeded': "WRIST_DOWN","aborted":"CHANGE_2_ARM2"}) StateMachine.add("WRIST_DOWN", wrist_down_state, transitions={"succeeded": "GRIPER_OPEN"}) StateMachine.add("GRIPER_OPEN", gripper_open_state, transitions={"succeeded": "CHANGE_2_BASE3"}) StateMachine.add("CHANGE_2_BASE3", change2base, transitions={"succeeded": "","aborted":"CHANGE_2_BASE3"}) reinit_sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) with reinit_sm: StateMachine.add("BACK3", Forword_BacK_Global(dis=1), transitions={"succeeded": "INIT_ANGLE"}) StateMachine.add("INIT_ANGLE", ChangeAngularGlobal(), transitions={"succeeded": ""}) sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) sm.userdata.object_id=0 with sm: StateMachine.add("FIND_OBJECT", find_object_sm, transitions={"succeeded": "READY"},remapping={"object":"object_id "}) StateMachine.add("READY", grasp_ready_sm, transitions={"succeeded": "GRASP"}) StateMachine.add("GRASP", grasp_sm, transitions={"succeeded": "PLACE"}) StateMachine.add("PLACE", place_sm, transitions={"succeeded": "REINIT"}) StateMachine.add("REINIT", reinit_sm, transitions={"succeeded": "INIT"}) StateMachine.add("INIT", init_sm_again, transitions={"succeeded": ""}) output=init_sm.execute() rospy.sleep(1) for id in id_list: OBJECT_ID=id output = sm.execute() ANGLE_CHANGED = 0 rospy.sleep(1) rospy.sleep(rospy.Duration(3))
class SMACHAI(): 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 empty_cb(self, userdata, msg): #rospy.loginfo("Empty message received.") return False # Gets called when ANY child state terminates def useless_child_termination_cb(self, outcome_map): rospy.loginfo("useless_child_termination_cb.") return True # Gets called when ALL child states are terminated def useless_outcome_cb(self, outcome_map): rospy.loginfo("useless_outcome_cb.") return 'succeeded' # Gets called when ANY child state terminates def in_main_room_child_termination_cb(self, outcome_map): #rospy.loginfo("daymode_child_termination_cb.") return True # Gets called when ALL child states are terminated def in_main_room_outcome_cb(self, outcome_map): #rospy.loginfo("daymode_outcome_cb.") if outcome_map['GO_TO_KITCHEN'] == 'invalid': rospy.loginfo("Going to the kitchen ") return 'go_kitchen' if outcome_map['GO_TO_BEDROOM'] == 'invalid': rospy.loginfo("Going to the bedroom") return 'go_bedroom' if outcome_map['GO_TO_SLEEP'] == 'invalid': rospy.loginfo("Going to sleep") return 'go_sleep' if outcome_map['GO_TO_POINT'] == 'succeeded': rospy.loginfo("Arrived to point") return 'succeeded' else: return 'aborted' # Gets called when ANY child state terminates def in_sleep_child_termination_cb(self, outcome_map): #rospy.loginfo("daymode_child_termination_cb.") return True # Gets called when ALL child states are terminated def in_sleep_outcome_cb(self, outcome_map): #rospy.loginfo("daymode_outcome_cb.") if outcome_map['GO_TO_KITCHEN'] == 'invalid': rospy.loginfo("Going to the kitchen ") return 'go_kitchen' if outcome_map['GO_TO_BEDROOM'] == 'invalid': rospy.loginfo("Going to the bedroom") return 'go_bedroom' if outcome_map['GO_TO_MAIN_ROOM'] == 'invalid': rospy.loginfo("Going to main room") return 'go_main_room' if outcome_map['GO_TO_POINT'] == 'succeeded': rospy.loginfo("Arrived to point") return 'succeeded' else: return 'aborted' # Gets called when ANY child state terminates def in_bedroom_child_termination_cb(self, outcome_map): #rospy.loginfo("daymode_child_termination_cb.") return True # Gets called when ALL child states are terminated def in_bedroom_outcome_cb(self, outcome_map): #rospy.loginfo("daymode_outcome_cb.") if outcome_map['GO_TO_KITCHEN'] == 'invalid': rospy.loginfo("Going to the kitchen ") return 'go_kitchen' if outcome_map['GO_TO_MAIN_ROOM'] == 'invalid': rospy.loginfo("Going to the main room") return 'go_main_room' if outcome_map['GO_TO_SLEEP'] == 'invalid': rospy.loginfo("Going to sleep") return 'go_sleep' if outcome_map['GO_TO_POINT'] == 'succeeded': rospy.loginfo("Arrived to point") return 'succeeded' else: return 'aborted' # Gets called when ANY child state terminates def in_kitchen_child_termination_cb(self, outcome_map): #rospy.loginfo("daymode_child_termination_cb.") return True # Gets called when ALL child states are terminated def in_kitchen_outcome_cb(self, outcome_map): #rospy.loginfo("daymode_outcome_cb.") if outcome_map['GO_TO_MAIN_ROOM'] == 'invalid': rospy.loginfo("Going to the main room") return 'go_main_room' if outcome_map['GO_TO_BEDROOM'] == 'invalid': rospy.loginfo("Going to the bedroom") return 'go_bedroom' if outcome_map['GO_TO_SLEEP'] == 'invalid': rospy.loginfo("Going to sleep") return 'go_sleep' if outcome_map['GO_TO_POINT'] == 'succeeded': rospy.loginfo("Arrived to point") return 'succeeded' else: return 'aborted' def time_cb(self, userdata, msg): if msg.data < 2: self.stopping = True return False else: self.stopping = False return True def shutdown(self): rospy.loginfo("Stopping carry automation...") #self.sm_day_mode.request_preempt() rospy.sleep(1)
def __init__(self): rospy.init_node('deliver_food', anonymous=False) self.initialize_destination() self.lalala = 100 # Track success rate of getting to the goal locations self.n_succeeded = 0 self.n_aborted = 0 self.n_preempted = 0 nav_states = {} for room in self.room_locations.iterkeys(): nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'map' nav_goal.target_pose.pose = self.room_locations[room] move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(60.0), server_wait_timeout=rospy.Duration(10.0)) nav_states[room] = move_base_state rospy.loginfo(room + " -> [" + str(round(self.room_locations[room].position.x,2)) + ", " + str(round(self.room_locations[room].position.y,2)) + "]" ) sm_rotate_search = Concurrence(outcomes=['find', 'not_find'], default_outcome='not_find', child_termination_cb=self.concurrence_child_termination_callback, outcome_cb=self.concurrence_outcome_callback) with sm_rotate_search: Concurrence.add('ROTATE', Rotate360() ) Concurrence.add('SEARCH', SearchTable() ) sm_table1 = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_table1: StateMachine.add('GOTO_TABLE1', nav_states['table1'], transitions={'succeeded':'ROTATE_SEARCH', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS', 'not_find':'GOTO_KITCHEN'}) StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'', 'aborted':'', 'preempted':''}) sm_table2 = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_table2: StateMachine.add('GOTO_TABLE2', nav_states['table2'], transitions={'succeeded':'ROTATE_SEARCH', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS', 'not_find':'GOTO_KITCHEN'}) StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'', 'aborted':'', 'preempted':''}) sm_table3 = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_table3: StateMachine.add('GOTO_TABLE3', nav_states['table3'], transitions={'succeeded':'ROTATE_SEARCH', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS', 'not_find':'GOTO_KITCHEN'}) StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'', 'aborted':'', 'preempted':''}) sm_table4 = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_table4: StateMachine.add('GOTO_TABLE4', nav_states['table4'], transitions={'succeeded':'ROTATE_SEARCH', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS', 'not_find':'GOTO_KITCHEN'}) StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'', 'aborted':'', 'preempted':''}) sm_table5 = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_table5: StateMachine.add('GOTO_TABLE5', nav_states['table5'], transitions={'succeeded':'ROTATE_SEARCH', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS', 'not_find':'GOTO_KITCHEN'}) StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'', 'aborted':'', 'preempted':''}) sm_table6 = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_table6: StateMachine.add('GOTO_TABLE6', nav_states['table6'], transitions={'succeeded':'ROTATE_SEARCH', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS', 'not_find':'GOTO_KITCHEN'}) StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN', 'aborted':'GOTO_KITCHEN', 'preempted':'GOTO_KITCHEN'}) StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'', 'aborted':'', 'preempted':''}) # let's initialize the overall state machine sm_deliverfood = StateMachine(outcomes=['succeeded','aborted','preempted']) with sm_deliverfood: StateMachine.add('COMPUTER_VISION_TASK', ComputerVision(), transitions={'detect1':'TABLE_ONE_TASK', 'detect2':'TABLE_TWO_TASK', 'detect3':'TABLE_THREE_TASK', 'detect4':'TABLE_FOUR_TASK', 'detect5':'TABLE_FIVE_TASK', 'detect6':'TABLE_SIX_TASK'}) StateMachine.add('TABLE_ONE_TASK',sm_table1, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'}) StateMachine.add('TABLE_TWO_TASK',sm_table2, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'}) StateMachine.add('TABLE_THREE_TASK',sm_table3, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'}) StateMachine.add('TABLE_FOUR_TASK',sm_table4, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'}) StateMachine.add('TABLE_FIVE_TASK',sm_table5, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'}) StateMachine.add('TABLE_SIX_TASK',sm_table6, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'}) StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'}) # Create and start the SMACH introspection server intro_server = IntrospectionServer('deliver_food', sm_deliverfood, '/SM_ROOT') intro_server.start() # Execute the state machine sm_outcome = sm_deliverfood.execute() rospy.on_shutdown(self.shutdown)
service_states.make_bounding_boxes, transitions={'succeeded': 'ANALYZE_CLOUD_COLOR'}) @cb_interface(input_keys=['clusters']) def analyze_color_request_cb(userdata, request): r = AnalyzeCloudColorRequest() r.cloud = userdata.clusters[0] return r StateMachine.add('ANALYZE_CLOUD_COLOR', ServiceState('analyze_cloud_color', AnalyzeCloudColor, request_cb=analyze_color_request_cb, response_slots=['mean', 'median', 'points']), transitions={'succeeded': 'COUNTER'}) StateMachine.add('COUNTER', CounterState(int(args.confirm_every)), transitions={'overflow': 'CONFIRM_OBJECT', 'counting': 'STORE_OBJECT'}) StateMachine.add('CONFIRM_OBJECT', ConfirmState('Is the detected object correct?'), transitions={'yes': 'STORE_OBJECT', 'no': 'ACCUMULATE_CLOUD'}) StateMachine.add('STORE_OBJECT', StoreObject(args.dataset, args.object_id), transitions={'stored': 'ACCUMULATE_CLOUD'}) rospy.loginfo('Starting data collection...') outcome = sm.execute()
def __init__(self): global target rospy.init_node('object', anonymous=False) rospy.on_shutdown(self.shutdown) grasp_sm=StateMachine(outcomes=['succeeded','aborted','preempted']) grasp_sm.userdata.target=Point(0,0,0) grasp_sm.userdata.object=xm_Object gripper_goal = xm_GripperCommandGoal() gripper_goal.command.position = 0.15 # gripper_goal.command.torque = 0.5 # gripper_goal.header.frame gripper_open_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10)) # gripper_goal = xm_GripperCommandGoal() gripper_goal.command.position = 0.06 # gripper_goal.command.torque = 0.5 # gripper_goal.header.frame gripper_close_state = SimpleActionState('xm_move_gripper', xm_GripperCommandAction, goal=gripper_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(10), server_wait_timeout=rospy.Duration(10)) wrist_goal=xm_WristPositionGoal() wrist_goal.angle=-1.047 wrist_state=SimpleActionState("xm_move_wrist", xm_WristPositionAction,goal=wrist_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(20), server_wait_timeout=rospy.Duration(20)) wrist_goal=xm_WristPositionGoal() wrist_goal.angle=0 wrist_state_init=SimpleActionState("xm_move_wrist", xm_WristPositionAction,goal=wrist_goal, result_cb=self.gripper_state_cb, exec_timeout=rospy.Duration(20), server_wait_timeout=rospy.Duration(20)) change2base=ChangeMode("base") change2arm=ChangeMode("arm") rospy.loginfo("ready") grasp_sm.userdata.target=Point(0.779,-0.03,-0.144) grasp_sm.userdata.x1=float(0.6) grasp_sm.userdata.x2=-0.6 grasp_sm.userdata.x3=0.3 # grasp_sm.userdata.distance_in=0.5 grasp_sm.userdata.angle_out=atan2(-0.03, 0.779) grasp_sm.userdata.postions=Point(0.779,-0.03,-0.144) with grasp_sm: StateMachine.add("CHANGE_2_ARM0",change2arm,transitions={'succeeded':"WRIST_DOWN"}) # StateMachine.add("INIT",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"init",response_cb=self.responce_cb),transitions={'succeeded':'WRIST_DOWN'}) StateMachine.add("WRIST_DOWN",wrist_state_init,transitions={"succeeded":"CHANGE_2_BASE0"}) StateMachine.add("CHANGE_2_BASE0",change2base,transitions={"succeeded":"CALCULATE_ANGLE"}) # StateMachine.add("Move_to_Pos", Forword_BacK(), transitions={'succeeded':'CALCULATE_ANGLE'},remapping={"distance_in":'x1'}) # StateMachine.add("FIND_OBJECT", ServiceState("Find_Object", xm_ObjectDetect,request_cb=self.find_object_request_cb, # response_cb=self.find_object_cb,input_keys=['object'],output_keys=['target']), # transitions={"succeeded":"CALCULATE_ANGLE",'aborted':'FIND_OBJECT'}, # remapping={'target':'labal'}) StateMachine.add("CALCULATE_ANGLE",Calculate_angle(),transitions={"succeeded":"CHANGE_ANGLE"},remapping={'waypoint_in':'target','angle':'angle_out'}) StateMachine.add("CHANGE_ANGLE",ChangeAngular(),transitions={"succeeded":"BACK"},remapping={"angle_in":'angle_out'}) StateMachine.add("BACK", Forword_BacK(), transitions={'succeeded':'CHANGE_2_ARM1'},remapping={"distance_in":"x2"}) StateMachine.add("CHANGE_2_ARM1",change2arm,transitions={'succeeded':"READY"}) StateMachine.add("READY",ServiceState('xm_move_arm_position/move_position', xm_ArmPosition,"prepare",response_cb=self.responce_cb),transitions={'succeeded':'OPEN_GRIPER'}) StateMachine.add("OPEN_GRIPER",gripper_open_state,transitions={"succeeded":"CHANGE_HEIGET"}) StateMachine.add("CHANGE_HEIGET",Lift(),transitions={"succeeded":"CHANGE_2_BASE1"},remapping={"position":"target"}) StateMachine.add("CHANGE_2_BASE1",change2base,transitions={"succeeded":"ALIGN_X"}) StateMachine.add("ALIGN_X",Forword_BacK(),transitions= {"succeeded" :"CHANGE_2_ARM2"},remapping={"distance_in":"x3"}) StateMachine.add("CHANGE_2_ARM2",change2arm,transitions={'succeeded':"CLOSE_GRIPPER"}) StateMachine.add("CLOSE_GRIPPER",gripper_close_state,transitions={'succeeded':'WRIST_UP'}) StateMachine.add("WRIST_UP",wrist_state,transitions={"succeeded":"CHANGE_2_BASE2"}) StateMachine.add("CHANGE_2_BASE2",change2base,transitions={'succeeded':"BACK2"}) StateMachine.add("BACK2", Forword_BacK(), transitions={'succeeded':''},remapping={"distance_in":'x2'}) outcome=grasp_sm.execute("CALCULATE_ANGLE") rospy.sleep(rospy.Duration(3))
class Patrol(): 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(40.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(40.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':''}) #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 nav_transition_cb(self, userdata, active_states, *cb_args): self.last_nav_state = active_states # Gets called when ANY child state terminates def concurrence_child_termination_cb(self, outcome_map): # If the current navigation task has succeeded, return True if outcome_map['SM_NAV'] == 'succeeded': return True # If the MonitorState state returns False (invalid), store the current nav goal and recharge if outcome_map['MONITOR_BATTERY'] == 'invalid': rospy.loginfo("LOW BATTERY! NEED TO RECHARGE...") if self.last_nav_state is not None: self.sm_nav.set_initial_state(self.last_nav_state, UserData()) return True else: return False # Gets called when ALL child states are terminated def concurrence_outcome_cb(self, outcome_map): # If the battery is below threshold, return the 'recharge' outcome if outcome_map['MONITOR_BATTERY'] == 'invalid': return 'recharge' # Otherwise, if the last nav goal succeeded, return 'succeeded' or 'stop' elif outcome_map['SM_NAV'] == 'succeeded': self.patrol_count += 1 rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count)) # If we have not completed all our patrols, start again at the beginning if self.n_patrols == -1 or self.patrol_count < self.n_patrols: self.sm_nav.set_initial_state(['NAV_STATE_0'], UserData()) return 'succeeded' # Otherwise, we are finished patrolling so return 'stop' else: self.sm_nav.set_initial_state(['NAV_STATE_4'], UserData()) return 'stop' # Recharge if all else fails else: return 'recharge' def battery_cb(self, userdata, msg): if msg.data < self.low_battery_threshold: self.recharging = True return False else: self.recharging = False return True def recharge_cb(self, userdata, response): return 'succeeded' def move_base_result_cb(self, userdata, status, result): if not self.recharging: if status == actionlib.GoalStatus.SUCCEEDED: self.n_succeeded += 1 elif status == actionlib.GoalStatus.ABORTED: self.n_aborted += 1 elif status == actionlib.GoalStatus.PREEMPTED: self.n_preempted += 1 try: rospy.loginfo("Success rate: " + str(100.0 * self.n_succeeded / (self.n_succeeded + self.n_aborted + self.n_preempted))) except: pass def shutdown(self): rospy.loginfo("Stopping the robot...") self.sm_nav.request_preempt() self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)