def main(): rospy.init_node('drone_state_machine') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['outcome']) sm.userdata.lspeed = 0.5 sm.userdata.rspeed = 1.2 # Open the container with sm: # Add states to the container smach.StateMachine.add('TAKEOFF', CBState(takeoff_cb), { 'finished': 'LAND', 'failed': 'LAND' }) smach.StateMachine.add('LAND', CBState(land_cb), { 'finished': 'outcome', 'failed': 'outcome' }) # Create and start the introspection server sis = smach_ros.IntrospectionServer('drone_server', sm, '/SM_DRONE') sis.start() # Execute SMACH plan outcome = sm.execute() sis.stop()
def construct_sm(): sm = StateMachine(outcomes = ['succeeded','aborted','preempted']) sm.userdata.nums = range(25, 88, 3) sm.userdata.even_nums = [] sm.userdata.odd_nums = [] with sm: ## %Tag(ITERATOR)% tutorial_it = Iterator(outcomes = ['succeeded','preempted','aborted'], input_keys = ['nums', 'even_nums', 'odd_nums'], it = lambda: range(0, len(sm.userdata.nums)), output_keys = ['even_nums', 'odd_nums'], it_label = 'index', exhausted_outcome = 'succeeded') ## %EndTag(ITERATOR)% ## %Tag(CONTAINER)% with tutorial_it: container_sm = StateMachine(outcomes = ['succeeded','preempted','aborted','continue'], input_keys = ['nums', 'index', 'even_nums', 'odd_nums'], output_keys = ['even_nums', 'odd_nums']) with container_sm: #test wether even or odd StateMachine.add('EVEN_OR_ODD', ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2, input_keys=['nums', 'index']), {'true':'ODD', 'false':'EVEN' }) #add even state @smach.cb_interface(input_keys=['nums', 'index', 'even_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def even_cb(ud): ud.even_nums.append(ud.nums[ud.index]) return 'succeeded' StateMachine.add('EVEN', CBState(even_cb), {'succeeded':'continue'}) #add odd state @smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'], output_keys=['odd_nums'], outcomes=['succeeded']) def odd_cb(ud): ud.odd_nums.append(ud.nums[ud.index]) return 'succeeded' StateMachine.add('ODD', CBState(odd_cb), {'succeeded':'continue'}) ## %EndTag(CONTAINER)% ## %Tag(ADDCONTAINER)% #close container_sm Iterator.set_contained_state('CONTAINER_STATE', container_sm, loop_outcomes=['continue']) ## %EndTag(ADDCONTAINER)% ## %Tag(ADDITERATOR)% #close the tutorial_it StateMachine.add('TUTORIAL_IT',tutorial_it, {'succeeded':'succeeded', 'aborted':'aborted'}) ## %EndTag(ADDITERATOR)% return sm
def __init__(self, angle=-math.pi/6): StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], output_keys=['square']) self.turns = 0 self.angle = angle self.has_spoken = False with self: StateMachine.add('FIND_SQUARE1', ReadTopicSquare(), transitions={'succeeded': 'SPEAK_F', 'aborted': 'CHECK_SPEAK'}) text = 'I have not found the marker. I will look around' StateMachine.add('SPEAK_NF', SpeechState(text=text, blocking=False), transitions={'succeeded': 'LOOK_DOWN'}) text = 'I have found the marker!' StateMachine.add('SPEAK_F', SpeechState(text=text, blocking=True), transitions={'succeeded': 'LOOK_FRONT'}) def check_turn(ud): if (self.turns == 1): self.angle = -self.angle _angle = self.angle self.turns += 1 elif (self.turns == 2): self.angle = -self.angle self.turns = 0 _angle = 0 return 'go_back' else: _angle = self.angle self.turns += 1 ud.joint_angles = [_angle] return 'succeeded' StateMachine.add('CHECK_TURN', CBState(check_turn, outcomes=['succeeded', 'go_back'], output_keys=['joint_angles']), transitions={'succeeded': 'LOOK_MIDDLE', 'go_back': 'PRE_GO_BACK'}, remapping={'joint_angles': 'joint_angles'}) StateMachine.add('LOOK_MIDDLE', JointAngleState(['HeadPitch'], [0.0]), transitions={'succeeded': 'LOOK_AROUND'}) StateMachine.add('LOOK_AROUND', JointAngleState(['HeadYaw']), transitions={'succeeded': 'FIND_SQUARE1'}) StateMachine.add('LOOK_DOWN', JointAngleState(['HeadPitch'], [0.5]), transitions={'succeeded': 'FIND_SQUARE2'}) StateMachine.add('FIND_SQUARE2', ReadTopicSquare(), transitions={'succeeded': 'SPEAK_F', 'aborted': 'CHECK_TURN'}) StateMachine.add('LOOK_FRONT', JointAngleState(['HeadYaw'], [0.0]), transitions={'succeeded': 'succeeded'}) StateMachine.add('PRE_GO_BACK', JointAngleState(['HeadPitch', 'HeadYaw'], [0.0, 0.0]), transitions={'succeeded': 'GO_BACK'}) StateMachine.add('GO_BACK', MoveToState(Pose2D(-0.20, 0.0, 0.0)), transitions={'succeeded': 'FIND_SQUARE1'}) def check_speak(ud): if self.has_spoken: return 'no_speak' self.has_spoken = True return 'speak' StateMachine.add('CHECK_SPEAK', CBState(check_speak, outcomes=['speak', 'no_speak']), transitions={'speak': 'SPEAK_NF', 'no_speak': 'LOOK_DOWN'})
def __init__(self, robot, trashbin_id, radius, yaw_offset): StateMachine.__init__(self, outcomes=['done']) @cb_interface(outcomes=['done']) def control_to_pose(_): trash_bin_frame = robot.ed.get_entity(trashbin_id).pose.frame trash_bin_position = trash_bin_frame.p base_frame = robot.base.get_location().frame base_position = base_frame.p trash_bin_to_base_vector = base_position - trash_bin_position trash_bin_to_base_vector.Normalize() desired_base_position = trash_bin_position + radius * trash_bin_to_base_vector goal_pose = PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id="map"), pose=Pose(position=Point(x=desired_base_position.x(), y=desired_base_position.y()), orientation=Quaternion(*quaternion_from_euler( 0, 0, math.atan2(trash_bin_to_base_vector.y(), trash_bin_to_base_vector.x()) - math.pi + yaw_offset)))) ControlToPose( robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.2)).execute({}) return 'done' with self: self.add('CONTROL_TO_TRASH_BIN', CBState(control_to_pose), transitions={'done': 'done'})
def __init__(self, panda_arm, pick_pose, place_pose, hover_distance = 0.0, num_retries=2): self.panda_arm = panda_arm self.num_retries = num_retries self.pick_pose = pick_pose self.place_pose = place_pose #Create a SMACH state machine self.sm = smach.StateMachine(outcomes=['done', 'failed', 'aborted']) self.sm.userdata.sm_pick_object_pose = Pose() self.sm.userdata.sm_place_object_pose = Pose() self.sm.userdata.sm_hover_distance = hover_distance with self.sm: smach.StateMachine.add('SET_PICK_POSE', CBState(self.set_pick_place_pose, cb_args=[self]), transitions={'succeeded':'MOVE_TO_PICK_HOVER_POSE', 'aborted': 'aborted'}, remapping={'pick_object_pose':'sm_pick_object_pose', 'place_object_pose':'sm_place_object_pose', 'hover_distance':'sm_hover_distance'}) smach.StateMachine.add('MOVE_TO_PICK_HOVER_POSE', CBState(self.move_to_pick_hover_pose, cb_args=[self]), transitions={'succeeded':'MOVE_TO_PICK_POSE', 'failed_hover':'failed', 'aborted': 'aborted'}, remapping={'pick_object_pose':'sm_pick_object_pose', 'hover_distance':'sm_hover_distance'}) smach.StateMachine.add('MOVE_TO_PICK_POSE', CBState(self.move_to_pick_pose, cb_args=[self]), transitions={'succeeded':'GRASP_AND_HOVER', 'failed_pick_pose': 'failed', 'aborted': 'aborted'}, remapping={'pick_object_pose':'sm_pick_object_pose'}) smach.StateMachine.add('GRASP_AND_HOVER', CBState(self.grasp_and_hover, cb_args=[self]), transitions={'succeeded':'MOVE_TO_PLACE_HOVER_POSE', 'failed_grasp': 'failed', 'aborted': 'aborted'}, remapping={'hover_distance':'sm_hover_distance'}) smach.StateMachine.add('MOVE_TO_PLACE_HOVER_POSE', CBState(self.move_to_place_hover_pose, cb_args=[self]), transitions={'succeeded':'MOVE_TO_PLACE_POSE', 'failed_hover': 'failed', 'aborted': 'aborted'}, remapping={'place_object_pose':'sm_place_object_pose', 'hover_distance':'sm_hover_distance'}) smach.StateMachine.add('MOVE_TO_PLACE_POSE', CBState(self.move_to_place_pose, cb_args=[self]), transitions={'succeeded':'PLACE_AND_HOVER', 'failed_place_pose': 'failed', 'aborted': 'aborted'}, remapping={'place_object_pose':'sm_place_object_pose'}) smach.StateMachine.add('PLACE_AND_HOVER', CBState(self.place_and_hover, cb_args=[self]), transitions={'succeeded':'done', 'failed_grasp': 'failed', 'aborted': 'aborted'}, remapping={'place_object_pose':'sm_place_object_pose', 'hover_distance':'sm_hover_distance'})
def __init__(self): Concurrence.__init__( self, outcomes=['succeeded', 'aborted', 'preempted'], default_outcome='succeeded', input_keys=['log_mission'], output_keys=['log_mission'], child_termination_cb=self.child_termination_cb, outcome_map={'succeeded': { 'WAITING': 'finished' }}) self.register_start_cb(self.start_pause) self.register_termination_cb(self.termination_pause) _min_time_to_go_recharge = 15.0 @cb_interface(input_keys=['log_mission'], outcomes=['insufficient_time', 'recharge', 'preempted']) def check_waiting_time(ud): navigation_duration = datetime.datetime.now( ) - ud.log_mission['start_time'] if datetime.timedelta(seconds=ud.log_mission['patrol']['min_duration']) - navigation_duration \ > datetime.timedelta(seconds=_min_time_to_go_recharge): return 'recharge' else: return 'succeeded' self.recharge = StateMachine( input_keys=['log_mission'], output_keys=['log_mission'], outcomes=['succeeded', 'aborted', 'preempted']) with self.recharge: StateMachine.add('CHECK_WAITING_TIME', CBState(check_waiting_time), { 'recharge': 'RECHARGE', 'insufficient_time': 'succeeded' }) StateMachine.add('RECHARGE', Recharge()) with self: Concurrence.add('WAITING', Waiting()) Concurrence.add('CONDITIONAL_RECHARGE', self.recharge)
def __init__(self, robot, arm): StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=['position']) @cb_interface(outcomes=['succeeded']) def prepare_grasp(ud): # Open gripper (non-blocking) arm.resolve().send_gripper_goal('open', timeout=0) # Torso up (non-blocking) robot.torso.high() # Arm to position in a safe way arm.resolve().send_joint_trajectory('prepare_grasp', timeout=0) # Open gripper arm.resolve().send_gripper_goal('open', timeout=0.0) return 'succeeded' with self: StateMachine.add( "PREPARE_GRASP", CBState(prepare_grasp), transitions={'succeeded': 'SIMPLE_NAVIGATE_TO_GRASP'}) StateMachine.add("SIMPLE_NAVIGATE_TO_GRASP", SimpleNavigateToGrasp(robot, arm), transitions={ 'unreachable': 'failed', 'arrived': 'SIMPLE_PICKUP', 'goal_not_defined': 'failed' }) StateMachine.add("SIMPLE_PICKUP", SimplePickup(robot, arm), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' })
def __init__(self, robot): StateMachine.__init__(self, outcomes=["succeeded", "failed"]) @cb_interface(outcomes=['succeeded'], output_keys=['position']) def send_point(ud): ud.position = PointStamped( header=Header(frame_id='/amigo/base_link'), point=Point(0.5, 0, 0.7)) return 'succeeded' arm = Designator(robot.leftArm) with self: StateMachine.add("SEND_POINT", CBState(send_point), transitions={'succeeded': 'FIND_CUP'}) StateMachine.add("FIND_CUP", SimpleGrab(robot, arm), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' })
def allign_with_target(target): """ Returns a state that alligns with the specified target. target: string on the form 'GATE' or 'BOUY' The returned state is responsible for chaning the circeling direction when needed. """ # TODO: get target position (x,y) from landmark server # TODO: create a move_goal move_goal = None allignment_attempt = Concurrence( outcomes=['succeeded', 'preempted', 'wrong_direction'], outcome_map={ 'succeeded': { 'ALLIGNMENT_CHECKER': 'alligned' }, 'wrong_direction': { 'ALLIGNMENT_CHECKER': 'wrong_direction' } }, default_outcome=['preempted'], child_termination_cb=None # TODO: should allways terminate ) with allignment_attempt: Concurrence.add( 'CIRCLE_GATE', SimpleActionState('controller/move', MoveAction, goal=move_goal) # TODO ) Concurrence.add('ALLIGNMENT_CHECKER', CBState(allignment_checker))
def __init__(self, dist_m_to_square=0.50, min_x_dist=0.25, negative_vel=False): StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted']) self.ALMOST_ZERO = 0.005 self.negative_vel = negative_vel with self: StateMachine.add('FIND_SQUARE', FindSquare(), transitions={'succeeded': 'PREPARE_OBJ'}, remapping={'square': 'square'}) def put_obj(ud): transf_square = transform_pose(Pose2D(ud.square.z, ud.square.x, 0.0)) if not self.negative_vel: y_mov = transf_square.y else: y_mov = -1*abs(transf_square.y) x_mov = min(min_x_dist, abs(transf_square.x)-dist_m_to_square) print '------------------ ud.square', ud.square print '------------------ transf_square', transf_square print '------------------ x_mov', x_mov print '------------------ min_x_dist', min_x_dist print '------------------ dist_m_to_square', dist_m_to_square #if x_mov <= self.ALMOST_ZERO and ud.square.x <= self.ALMOST_ZERO: obj = Pose2D(x_mov, y_mov, 0.0) ud.objective = obj print '------------------ objective', obj if x_mov < min_x_dist: return 'one_step_left' else: return 'succeeded' StateMachine.add('PREPARE_OBJ', CBState(put_obj, outcomes=['succeeded', 'one_step_left'], input_keys=['square'], output_keys=['objective']), transitions={'succeeded': 'MOVE_TO_SQ', 'one_step_left': 'MOVE_TO_FINAL'}, remapping={'objective': 'objective'}) StateMachine.add('MOVE_TO_SQ', MoveToState(), transitions={'succeeded': 'FIND_SQUARE'}, remapping={'objective': 'objective'}) StateMachine.add('MOVE_TO_FINAL', MoveToState(), transitions={'succeeded': 'succeeded'}, remapping={'objective': 'objective'})
def setup_statemachine(robot): state_machine = StateMachine(outcomes=['done']) state_machine.userdata['item_picked'] = None with state_machine: # Intro # StateMachine.add('START_CHALLENGE_ROBUST', # Initialize(robot), # transitions={'initialized': 'SAY_START', 'abort': 'done'}) StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, "initial_pose"), # ToDo: in knowledge transitions={'Done': 'SAY_START', 'Aborted': 'done', 'Failed': 'SAY_START'}) StateMachine.add('SAY_START', Say(robot, "Let's set the table baby! If there are any chairs near the kitchen_table, please remove them", block=False), transitions={'spoken': 'NAVIGATE_AND_OPEN_CUPBOARD'}) # The pre-work StateMachine.add('NAVIGATE_AND_OPEN_CUPBOARD', NavigateToAndOpenCupboard(robot, "kitchen_cabinet", "in_front_of"), transitions={'succeeded': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER', 'failed': 'SAY_OPEN_FAILED'}) StateMachine.add('SAY_OPEN_FAILED', Say(robot, "I failed to open the cupboard drawer"), transitions={'spoken': 'WAIT_OPEN'}) StateMachine.add('WAIT_OPEN', WaitTime(robot, 5), transitions={'waited': 'SAY_OPEN_THANKS', 'preempted': 'done'}) StateMachine.add('SAY_OPEN_THANKS', Say(robot, "Thank you darling"), transitions={'spoken': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'}) # The loop StateMachine.add('NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER', NavigateToAndPickItemFromCupboardDrawer(robot, "kitchen_cabinet", "in_front_of", required_items), transitions={'succeeded': 'PLACE_ITEM_ON_TABLE', 'failed': 'CHECK_IF_WE_HAVE_IT_ALL'}) StateMachine.add('PLACE_ITEM_ON_TABLE', NavigateToAndPlaceItemOnTable(robot, "kitchen_table", "right_of", "right_of_close"), transitions={'succeeded': 'CHECK_IF_WE_HAVE_IT_ALL', 'failed': 'WAIT'}) StateMachine.add('WAIT', WaitTime(robot, 2), transitions={'waited': 'CHECK_IF_WE_HAVE_IT_ALL', 'preempted': 'done'}) StateMachine.add('CHECK_IF_WE_HAVE_IT_ALL', CBState(check_if_we_have_it_all, cb_args=[robot]), transitions={'we_have_it_all': 'SAY_END_CHALLENGE', 'keep_going': 'NAVIGATE_AND_PICK_ITEM_FROM_CUPBOARD_DRAWER'}) # Outro StateMachine.add('SAY_END_CHALLENGE', Say(robot, "That was all folks, have a nice meal!"), transitions={'spoken': 'done'}) StateMachine.add('NAVIGATE_AND_CLOSE_CUPBOARD', NavigateToAndCloseCupboard(robot, "cupboard", "in_front_of"), transitions={'succeeded': 'done', 'failed': 'done'}) return state_machine
# Start for RecognitionThread # try: # rec_and_show = RecognitionThread(0,config.getboolean('GAPL','show_flag'), config.getboolean('GAPL','streaming_flag'),config.getboolean('GAPL','recording_flag')) # except: # print 'camera 1 ERROR' # exit(1) position_memory = Position_Memory() # Create and start the introspection server sis = smach_ros.IntrospectionServer('drone01_server', sm, '/SM_DRONE01') sis.start() # Open the container with sm: # Add states to the container smach.StateMachine.add('MISSION', CBState(loadMission_cb), transitions={'finished': 'TAKEOFF', 'failed':'LAND'}) smach.StateMachine.add('TAKEOFF', CBState(takeoff_cb), transitions={'finished': 'MISSION_SEARCHING', 'failed':'LAND'}) smach.StateMachine.add('MISSION_SEARCHING', CBState(auto_cb), transitions={'finished': 'RTL', 'reaching': 'MISSION_REACHING', 'failed':'RTL'}, remapping={'target_dx': 'target_dx','target_dy': 'target_dy','target_loc': 'target_loc','errX_pix': 'errX_pix','errY_pix': 'errY_pix'}) smach.StateMachine.add('MISSION_REACHING', CBState(reaching_cb), transitions={'finished': 'MISSION_ACTION', 'GPS': 'MISSION_REACHINGgps', 'failed':'RTL'}, remapping={'target_dx': 'target_dx','target_dy': 'target_dy','target_loc': 'target_loc','errX_pix': 'errX_pix','errY_pix': 'errY_pix'}) smach.StateMachine.add('MISSION_REACHINGgps', CBState(reachingGPS_cb), transitions={'finished': 'MISSION_SEARCHING','reaching': 'MISSION_REACHING','failed':'RTL'}, remapping={'target_loc': 'target_loc'}) smach.StateMachine.add('MISSION_ACTION', CBState(action_cb), transitions={'finished': 'MISSION_SEARCHING', 'failed':'RTL'}) smach.StateMachine.add('RTL', CBState(RTL_cb),
return 'timeouted' if __name__ == "__main__": rospy.init_node('readtopicstatetest') from smach import StateMachine, CBState from std_msgs.msg import String sm = StateMachine(outcomes=['succeeded']) with sm: StateMachine.add('READTOPIC', ReadTopicState('/test', String, output_key_name='test_key', timeout=10), transitions={ 'succeeded': 'succeeded', 'timeouted': 'SAYTIMEOUT' }) def cb(ud): rospy.logwarn( 'READ TOPIC STATE TIMEOUTED WITHOUT RECEIVING MESSAGES') return 'succeeded' StateMachine.add('SAYTIMEOUT', CBState(cb, outcomes=['succeeded']), transitions={'succeeded': 'READTOPIC'}) sm.execute() rospy.loginfo('Data received is: ' + sm.userdata.test_key.data)
return 'finished' #else: #return 'failed' if __name__ == '__main__': rospy.init_node('drone_state_machine') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['outcome']) # Open the container with sm: # Stand UP state smach.StateMachine.add('STAND_UP', CBState(stand_up_cb), { 'finished': 'STOP', 'failed': 'outcome' }) # Stop state smach.StateMachine.add( 'STOP', CBState(stop_cb), { 'finished_1': 'ALTERN_TRIPOD', 'finished_2': 'TETRAPOD', 'finished_3': 'STAIR_CLIMB', 'failed': 'outcome' }) # Altern tripod state smach.StateMachine.add('ALTERN_TRIPOD', CBState(altern_tripod_cb), {
def __init__(self): StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted']) self.turn_rad = REORIENT_RAD with self: StateMachine.add('MOVE_LEFT', MoveToState(Pose2D(0.0, LEFT_MOVE, 0.0)), transitions={'succeeded': 'MOVE_TO_FRONT'}) StateMachine.add('MOVE_TO_FRONT', MoveToState(Pose2D(FRONT_MOVE, 0.0, 0.0)), transitions={'succeeded': 'SAY_CHECKING_FIRES'}) text = 'I will check if any fire is lit.' StateMachine.add('SAY_CHECKING_FIRES', SpeechState(text=text, blocking=False), transitions={'succeeded': 'LOOK_AT_STOVE'}) StateMachine.add('LOOK_AT_STOVE', JointAngleState(['HeadPitch', 'RElbowRoll', 'LElbowRoll'], [0.35, 0.05, -0.05]), transitions={'succeeded': 'CHECK_FIRE'}) #StateMachine.add('DISABLE_ARM_WALK', SetArmsWalkingState(leftArmEnabled=False, rightArmEnabled=False), # transitions={'succeeded': 'LATERAL_TO_FIRE'}) #StateMachine.add('LATERAL_TO_FIRE', MoveToState(Pose2D(0.0, DISTANCE_MARKER_TO_FIRE, 0.0)), transitions={'succeeded': 'CHECK_FIRE'}) StateMachine.add('CHECK_FIRE', ReadTopicFire(), transitions={'succeeded': 'PREPARE_TEXT_AND_MOVEMENT', 'aborted': 'REORIENT_FIRE'}, remapping={'plate': 'plate'}) StateMachine.add('REORIENT_FIRE', MoveToState(Pose2D(0.0, 0.0, REORIENT_FIRE)), transitions={'succeeded': 'CHECK_FIRE'}) def prep_text(ud): ud.out_text = "The %s fire is lit! I will put it off." % ud.plate.lower() if ud.plate == "UPPER": ud.objective = Pose2D(0.0, -DISTANCE_FIRE_UPPER_BUTTON, 0.0) else: ud.objective = Pose2D(0.0, -DISTANCE_FIRE_LOWER_BUTTON, 0.0) return 'succeeded' StateMachine.add('PREPARE_TEXT_AND_MOVEMENT', CBState(prep_text, outcomes=['succeeded'], input_keys=['plate'], output_keys=['out_text', 'objective']), transitions={'succeeded':'SAY_FIRE_LIT'}, remapping={'out_text':'text', 'objective': 'objective'}) StateMachine.add('SAY_FIRE_LIT', SpeechState(blocking=False), remapping={'text': 'text'}, transitions={'succeeded': 'PREPARE_EXECUTE'}) def prep_execute_putoff(ud): if ud.plate == "UPPER": return 'upper' else: return 'lower' return 'succeeded' StateMachine.add('PREPARE_EXECUTE', CBState(prep_execute_putoff, outcomes=['upper', 'lower', 'succeeded'], input_keys=['plate']), transitions={'succeeded':'SAY_NO_FIRE_LIT', 'lower': 'PUT_OFF_LOWER_FIRE_MOVEMENT', 'upper': 'PUT_OFF_UPPER_FIRE_MOVEMENT'}, #transitions={'lower': 'succeeded', 'upper':'succeeded'}, remapping={'out_text':'text', 'objective': 'objective'}) #StateMachine.add('MOVE_TO_BUTTON', MoveToState(), transitions={'succeeded': 'PUT_OFF_FIRE_MOVEMENT'}, # remapping={'objective': 'objective'}) StateMachine.add('PUT_OFF_UPPER_FIRE_MOVEMENT', ExecuteBehavior(behavior_name='putoff_upper_fire'), transitions={'succeeded':'RE_CHECK'}) StateMachine.add('PUT_OFF_LOWER_FIRE_MOVEMENT', ExecuteBehavior(behavior_name='putoff_lower_fire'), transitions={'succeeded':'RE_CHECK'}) StateMachine.add('RE_CHECK', ReadTopicFire(), transitions={'succeeded': 'SAY_FAILED', 'aborted': 'SAY_FINISH'}, remapping={'plate': 'plate'}) StateMachine.add('SAY_FAILED', SpeechState(text='Oh I failed!', blocking=False), transitions={'succeeded': 'PREPARE_REORIENT'}) def prep_reorientcheck(ud): ud.objective = Pose2D(0.0, 0.0, self.turn_rad) self.turn_rad = -self.turn_rad return 'succeeded' StateMachine.add('PREPARE_REORIENT', CBState(prep_reorientcheck, outcomes=['succeeded'], output_keys=['objective']), transitions={'succeeded': 'REORIENT'}) StateMachine.add('REORIENT', MoveToState(), transitions={'succeeded': 'PUT_OFF_UPPER_FIRE_MOVEMENT'}) StateMachine.add('REPUTOFF', ExecuteBehavior(behavior_name='putoff_upper_fire'), transitions={'succeeded':'LAST_CHECK'}) StateMachine.add('LAST_CHECK', ReadTopicFire(), transitions={'succeeded': 'SAY_EVACUATE', 'aborted': 'SAY_FINISH'}, remapping={'plate': 'plate'}) StateMachine.add('SAY_EVACUATE', SpeechState(text='I could not put off the fire. Alarm! Please evacuate the room.', blocking=False), transitions={'succeeded': 'GO_BACK'}) StateMachine.add('SAY_NO_FIRE_LIT', SpeechState(text='I am done as no fire is lit.', blocking=False), transitions={'succeeded': 'GO_BACK'}) text = 'I am done, there is no need to call the fire department!' StateMachine.add('SAY_FINISH', SpeechState(text=text, blocking=False), transitions={'succeeded': 'GO_BACK'}) StateMachine.add('GO_BACK', MoveToState(Pose2D(-DISTANCE_BACK_FROM_FIRE, 0.0, 0.0)), transitions={'succeeded': 'succeeded'})
def __init__(self, heritage): # Creation of State Machine and definition of its outcomes self.agent_sm = StateMachine(outcomes=['completed', 'failed']) with self.agent_sm: # Initialization of the dictionary containing every Action Service Wrapper self.asw_dicc = {} ### ACTION SERVER ADVERTISING ### # Add a state where drone does nothing but wait to receive a service order StateMachine.add( 'action_server_advertiser', CBState(self.action_server_advertiser_stcb, cb_kwargs={ 'heritage': heritage, 'asw_dicc': self.asw_dicc }), {'completed': 'completed'}) # Every action service wrapper is similar following a structure defined by SMACH. # Each wrapper is defined and added to # To understand what they do, see callbacks below ### TAKE-OFF STATE MACHINE & WRAPPER ### self.take_off_sm = StateMachine(outcomes=['completed', 'failed'], input_keys=['action_goal']) self.asw_dicc['take_off'] = ActionServerWrapper( '/magna/GS_Agent_{}/take_off_command'.format(heritage.ID), TakeOffAction, self.take_off_sm, ['completed'], ['failed'], ['preempted'], goal_key='action_goal', result_key='action_result') with self.take_off_sm: StateMachine.add( 'take_off', CBState(self.take_off_stcb, input_keys=['action_goal'], cb_kwargs={'heritage': heritage}), {'completed': 'completed'}) ### LAND STATE MACHINE & WRAPPER ### self.land_sm = StateMachine(outcomes=['completed', 'failed'], input_keys=['action_goal']) self.asw_dicc['land'] = ActionServerWrapper( '/magna/GS_Agent_{}/land_command'.format(heritage.ID), LandAction, self.land_sm, ['completed'], ['failed'], ['preempted'], goal_key='action_goal', result_key='action_result') with self.land_sm: StateMachine.add( 'land', CBState(self.land_stcb, input_keys=['action_goal'], cb_kwargs={'heritage': heritage}), {'completed': 'completed'}) ### BASIC MOVE STATE MACHINE & WRAPPER ### self.basic_move_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['basic_move'] = ActionServerWrapper( '/magna/GS_Agent_{}/basic_move_command'.format(heritage.ID), BasicMoveAction, self.basic_move_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.basic_move_sm: StateMachine.add( 'basic_move', CBState(self.basic_move_stcb, input_keys=['action_goal', 'action_result'], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) ### SET MISSION STATE MACHINE & WRAPPER ### self.set_mission_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['set_mission'] = ActionServerWrapper( '/magna/GS_Agent_{}/set_mission_command'.format(heritage.ID), SetMissionAction, self.set_mission_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.set_mission_sm: StateMachine.add( 'set_mission', CBState(self.set_mission_stcb, input_keys=[ 'action_goal', 'action_result', '_preempt_requested' ], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) ### FOLLOW PATH STATE MACHINE & WRAPPER ### self.follow_path_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['follow_path'] = ActionServerWrapper( '/magna/GS_Agent_{}/follow_path_command'.format(heritage.ID), FollowPathAction, self.follow_path_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.follow_path_sm: StateMachine.add( 'follow_path', CBState(self.follow_path_stcb, input_keys=[ 'action_goal', 'action_result', '_preempt_requested' ], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) # StateMachine.add('to_wp', self.follow_path_sm, # {'completed':'action_server_advertiser'}) ### FOLLOW Agent AD STATE MACHINE & WRAPPER### self.follow_agent_ad_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['follow_agent_ad'] = ActionServerWrapper( '/magna/GS_Agent_{}/follow_agent_ad_command'.format( heritage.ID), FollowAgentADAction, self.follow_agent_ad_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.follow_agent_ad_sm: StateMachine.add( 'follow_agent_ad', CBState(self.follow_agent_ad_stcb, input_keys=[ 'action_goal', 'action_result', '_preempt_requested' ], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) ### FOLLOW Agent AP STATE MACHINE & WRAPPER### self.follow_agent_ap_sm = StateMachine( outcomes=[ 'completed', 'failed', 'collision', 'low_battery', 'GS_critical_event' ], input_keys=['action_goal', 'action_result']) self.asw_dicc['follow_agent_ap'] = ActionServerWrapper( '/magna/GS_Agent_{}/follow_agent_ap_command'.format( heritage.ID), FollowAgentAPAction, self.follow_agent_ap_sm, ['completed'], ['failed'], ['collision', 'low_battery', 'GS_critical_event'], goal_key='action_goal', result_key='action_result') with self.follow_agent_ap_sm: StateMachine.add( 'follow_agent_ap', CBState(self.follow_agent_ap_stcb, input_keys=[ 'action_goal', 'action_result', '_preempt_requested' ], cb_kwargs={'heritage': heritage}), { 'completed': 'completed', 'collision': 'collision', 'low_battery': 'low_battery', 'GS_critical_event': 'GS_critical_event' }) if heritage.smach_view == True: sis = smach_ros.IntrospectionServer( 'magna/Agent_{}_introspection'.format(heritage.ID), self.agent_sm, '/Agent_{}'.format(heritage.ID)) sis.start()
def main(): # The autonomy_sm handles the full autonomy sequence for the competition run autonomy_sm = StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) with autonomy_sm: # Add states for setup (these run once each) for i in range(len(setup_sequence)): name, action_type = get_name_and_type(setup_sequence[i]) func_to_run = None if action_type == 'localize': func_to_run = localization_cb elif action_type == 'move': # action instead of function func_to_run = 'action' else: raise NotImplementedError('Only localize is supported rn') # if this is the last setup state if i == len(loop_sequence) - 1: # tie the last setup state to the first loop state first_loop_name = get_name_and_type(loop_sequence[0]) if func_to_run == 'action': action_state = create_action_state(name, action_type) StateMachine.add( name, action_state, transitions={'succeeded': first_loop_name}) else: StateMachine.add(name, CBState(func_to_run), {'succeeded': first_loop_name}) else: # add_auto adds this to the state machine and automatically # ties the next and previous states together if func_to_run == 'action': action_state = create_action_state(name, action_type) StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded']) else: StateMachine.add_auto(name, CBState(func_to_run), connector_outcomes=['succeeded']) names = [] # store names to check and handle dupes for i in range(len(loop_sequence)): name, action_type = get_name_and_type(loop_sequence[i]) action_state = create_action_state(name, action_type) # "move" states all have the same name and smach requires unique # state names, so check and add a number to the name if needed if name in names: name = name + "2" while name in names: name = name[:-1] + str( int(name[-1]) + 1) # increase num by 1 names.append(name) if i == len(loop_sequence) - 1: # tie the last state to the first one, so they just keep looping StateMachine.add(name, action_state, transitions={'succeeded': names[0]}) else: # add_auto adds this to the state machine and automatically # ties the next and previous states together StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded']) # Create the concurrence container for the fully autonomy sequence. This # runs the state machine for the competition run. It also concurrently runs # a state that listens to the /click_start_button topic. If this is # triggered place us into the teleop state. autonomy_concurrence = Concurrence( outcomes=['enter_teleop', 'stay', 'aborted'], default_outcome='enter_teleop', child_termination_cb=autonomy_child_term_cb, outcome_cb=autonomy_out_cb) with autonomy_concurrence: # state that runs full autonomy state machine Concurrence.add('AUTONOMY', autonomy_sm) # state that listens for toggle message Concurrence.add( 'TOGGLE_LISTEN', MonitorState('/click_start_button', Empty, start_btn_cb)) teleop_concurrence = Concurrence( outcomes=['enter_autonomy', 'stay', 'done'], default_outcome='enter_autonomy', child_termination_cb=teleop_child_term_cb, outcome_cb=teleop_out_cb) with teleop_concurrence: Concurrence.add( 'TOGGLE_LISTEN', MonitorState('/click_start_button', Empty, start_btn_cb)) Concurrence.add( 'EXIT_LISTEN', MonitorState('/click_select_button', Empty, select_btn_cb)) # Top level state machine, containing the autonomy and teleop machines. top_sm = StateMachine(outcomes=['DONE']) with top_sm: StateMachine.add('TELEOP_MODE', teleop_concurrence, transitions={ 'enter_autonomy': 'AUTONOMY_MODE', 'stay': 'TELEOP_MODE', 'done': 'DONE' }) StateMachine.add('AUTONOMY_MODE', autonomy_concurrence, transitions={ 'enter_teleop': 'TELEOP_MODE', 'stay': 'AUTONOMY_MODE', 'aborted': 'DONE' }) #StateMachine.add('TELEOP_MODE', MonitorState('/click_start_button', Empty, monitor_cb), # transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'}) sis = IntrospectionServer('smach_introspection_server', top_sm, '/COMPETITION_SMACH') sis.start() top_sm.execute() rospy.spin() sis.stop()
def __init__(self, robot, table_id, placement_height): StateMachine.__init__(self, outcomes=['succeeded', 'failed'], input_keys=["item_picked"]) arm = robot.get_arm()._arm self.placement_height = placement_height def send_joint_goal(position_array, wait_for_motion_done=True): arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0)) if wait_for_motion_done: arm.wait_for_motion_done() def send_joint_trajectory(goal_array, wait_for_motion_done=True): arm._send_joint_trajectory(goal_array, timeout=rospy.Duration(0)) if wait_for_motion_done: arm.wait_for_motion_done() def send_gripper_goal(open_close_string): arm.send_gripper_goal(open_close_string) rospy.sleep(1.0) # Does not work with motion_done apparently @cb_interface(outcomes=['done'], input_keys=["item_picked"]) def _pre_place(user_data): rospy.loginfo("Preplacing...") robot.head.look_up() robot.head.wait_for_motion_done() item_name = user_data["item_picked"] send_joint_goal([0.69, 0, 0, 0, 0]) if item_name in ['plate', 'napkin']: send_joint_goal(pboven) else: send_joint_goal([0.69, -1.2, 0, -1.57, 0]) return 'done' @cb_interface(outcomes=['done'], input_keys=["item_picked"]) def _align_with_table(user_data): item_placement_vector = item_vector_dict[user_data["item_picked"]] item_frame = item_vector_to_item_frame(item_placement_vector) goal_pose = item_frame_to_pose(item_frame, table_id) rospy.loginfo("Placing {} at {}".format(user_data["item_picked"], goal_pose)) robot.head.look_down() robot.head.wait_for_motion_done() ControlToPose( robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.02, 0.1)).execute({}) return 'done' @cb_interface(outcomes=['done'], input_keys=["item_picked"]) def _place_and_retract(user_data): rospy.loginfo("Placing...") item_name = user_data["item_picked"] if item_name in ['plate', 'napkin']: # TODO: Do a different joint goal/trajectory send_joint_goal(pleg) # send_joint_goal(pweg) else: send_joint_goal([self.placement_height, -1.57, 0, -1.57, 0]) rospy.loginfo("Dropping...") send_gripper_goal("open") send_joint_goal(pweg) robot.head.look_up() robot.head.wait_for_motion_done() rospy.loginfo("Retract...") send_joint_goal([0.69, 0, -1.57, 0, 0]) send_gripper_goal("close") robot.base.force_drive( -0.1, 0, 0, 3) # Drive backwards at 0.1m/s for 3s, so 30cm arm.send_joint_goal("carrying_pose") return 'done' @cb_interface(outcomes=['done'], input_keys=["item_picked"]) def _ask_user(user_data): robot.head.look_up() robot.head.wait_for_motion_done() send_joint_goal([0, 0, 0, 0, 0]) item_name = user_data["item_picked"] robot.speech.speak( "Please put the {} on the table.".format(item_name)) robot.speech.speak("Watch out, I will open my gripper now") send_gripper_goal("open") rospy.sleep(5.0) robot.speech.speak("Thanks for that!", block=False) send_gripper_goal("open") arm.send_joint_goal("carrying_pose", timeout=0) return 'done' with self: self.add_auto('PRE_PLACE', CBState(_pre_place), ['done']) self.add_auto('ALIGN_WITH_TABLE', CBState(_align_with_table), ['done']) self.add('PLACE_AND_RETRACT', CBState(_place_and_retract), transitions={'done': 'succeeded'})
def get_smach_state(self): return CBState(takeoff_cb)
def __init__(self, robot, room_id): StateMachine.__init__(self, outcomes=['done']) @cb_interface(outcomes=['done']) def detect_persons(_): global PERSON_DETECTIONS global NUM_LOOKS # with open('/home/rein/mates/floorplan-2019-07-05-11-06-52.pickle', 'r') as f: # PERSON_DETECTIONS = pickle.load(f) # rospy.loginfo("Loaded %d persons", len(PERSON_DETECTIONS)) # # # return "done" look_angles = np.linspace(-np.pi / 2, np.pi / 2, 8) # From -pi/2 to +pi/2 to scan 180 degrees wide head_goals = [kdl_conversions.VectorStamped(x=100 * math.cos(angle), y=100 * math.sin(angle), z=1.5, frame_id="/%s/base_link" % robot.robot_name) for angle in look_angles] sentences = deque([ "Hi there mates, where are you, please look at me!", "I am looking for my mates! Dippi dee doo! Pew pew!", "You are all looking great today! Keep looking at my camera. I like it when everybody is staring at me!" ]) while len(PERSON_DETECTIONS) < 4 and not rospy.is_shutdown(): for _ in range(NUM_LOOKS): sentences.rotate(1) robot.speech.speak(sentences[0], block=False) for head_goal in head_goals: robot.speech.speak("please look at me", block=False) robot.head.look_at_point(head_goal) robot.head.wait_for_motion_done() now = time.time() rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo() try: persons = robot.perception.detect_person_3d(rgb, depth, depth_info) except Exception as e: rospy.logerr(e) rospy.sleep(2.0) else: for person in persons: if person.face.roi.width > 0 and person.face.roi.height > 0: try: PERSON_DETECTIONS.append({ "map_ps": robot.tf_listener.transformPoint("map", PointStamped( header=rgb.header, point=person.position )), "person_detection": person, "rgb": rgb }) except Exception as e: rospy.logerr("Failed to transform valid person detection to map frame") rospy.loginfo("Took %.2f, we have %d person detections now", time.time() - now, len(PERSON_DETECTIONS)) rospy.loginfo("Detected %d persons", len(PERSON_DETECTIONS)) return 'done' @cb_interface(outcomes=['done', 'failed']) def _data_association_persons_and_show_image_on_screen(_): global PERSON_DETECTIONS try: with open(os.path.expanduser('~/floorplan-{}.pickle'.format(datetime.now().strftime("%Y-%m-%d-%H-%M-%S"))), 'w') as f: pickle.dump(PERSON_DETECTIONS, f) except: pass room_entity = robot.ed.get_entity(id=room_id) # type: Entity room_volume = room_entity.volumes["in"] min_corner = room_entity.pose.frame * room_volume.min_corner max_corner = room_entity.pose.frame * room_volume.max_corner shrink_x = 0.5 shrink_y = 0.3 min_corner_shrinked = PyKDL.Vector(min_corner.x() + shrink_x, min_corner.y() + shrink_y, 0) max_corner_shrinked = PyKDL.Vector(max_corner.x() - shrink_x, max_corner.y() - shrink_y, 0) rospy.loginfo('Found %d person detections', len(PERSON_DETECTIONS)) def _get_clusters(): def _in_room(p): return min_corner_shrinked.x() < p.x < max_corner_shrinked.x() and min_corner_shrinked.y() < p.y < max_corner_shrinked.y() in_room_detections = [d for d in PERSON_DETECTIONS if _in_room(d['map_ps'].point)] rospy.loginfo("%d in room before clustering", len(in_room_detections)) clusters = cluster_people(in_room_detections, np.array([6, 0])) return clusters # filter in room and perform clustering until we have 4 options try: person_detection_clusters = _get_clusters() except ValueError as e: rospy.logerr(e) robot.speech.speak("Mates, where are you?", block=False) return "failed" floorplan = cv2.imread( os.path.join(rospkg.RosPack().get_path('challenge_find_my_mates'), 'img/floorplan.png')) floorplan_height, floorplan_width, _ = floorplan.shape bridge = CvBridge() c_map = color_map(N=len(person_detection_clusters), normalized=True) for i, person_detection in enumerate(person_detection_clusters): image = bridge.imgmsg_to_cv2(person_detection['rgb'], "bgr8") roi = person_detection['person_detection'].face.roi roi_image = image[roi.y_offset:roi.y_offset + roi.height, roi.x_offset:roi.x_offset + roi.width] desired_height = 150 height, width, channel = roi_image.shape ratio = float(height) / float(desired_height) calculated_width = int(float(width) / ratio) resized_roi_image = cv2.resize(roi_image, (calculated_width, desired_height)) x = person_detection['map_ps'].point.x y = person_detection['map_ps'].point.y x_image_frame = 9.04 - x y_image_frame = 1.58 + y pixels_per_meter = 158 px = int(pixels_per_meter * x_image_frame) py = int(pixels_per_meter * y_image_frame) cv2.circle(floorplan, (px, py), 3, (0,0,255), 5) try: px_image = min(max(0, px - calculated_width / 2), floorplan_width - calculated_width - 1) py_image = min(max(0, py - desired_height / 2), floorplan_height - desired_height - 1) if px_image >= 0 and py_image >= 0: #could not broadcast input array from shape (150,150,3) into shape (106,150,3) floorplan[py_image:py_image + desired_height, px_image:px_image + calculated_width] = resized_roi_image cv2.rectangle(floorplan, (px_image, py_image), (px_image + calculated_width, py_image + desired_height), (c_map[i, 2] * 255, c_map[i, 1] * 255, c_map[i, 0] * 255), 10) else: rospy.logerr("bound error") except Exception as e: rospy.logerr("Drawing image roi failed: {}".format(e)) label = "female" if person_detection['person_detection'].gender else "male" label += ", " + str(person_detection['person_detection'].age) cv2.putText(floorplan, label, (px_image, py_image + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2, cv2.LINE_AA) # cv2.circle(floorplan, (px, py), 3, (0, 0, 255), 5) filename = os.path.expanduser('~/floorplan-{}.png'.format(datetime.now().strftime("%Y-%m-%d-%H-%M-%S"))) cv2.imwrite(filename, floorplan) robot.hmi.show_image(filename, 120) return "done" with self: self.add_auto('DETECT_PERSONS', CBState(detect_persons), ['done']) self.add('DATA_ASSOCIATION_AND_SHOW_IMAGE_ON_SCREEN', CBState(_data_association_persons_and_show_image_on_screen), transitions={'done': 'done', 'failed': 'DETECT_PERSONS'})
sm_one = smach.StateMachine(outcomes=[ 'FINISHED', 'ERROR', 'CONDITION_PREEMPTED', 'ACTION_PREEMPTED', 'ACTION_ERROR' ]) with sm_one: smach.StateMachine.add( 'CONDITION_30', smach_ros.MonitorState('/ros_cle_simulation/status', String, condition_30_cb), { 'valid': 'CONDITION_30', 'invalid': 'ACTION_30', 'preempted': 'CONDITION_PREEMPTED' }) smach.StateMachine.add('ACTION_30', CBState(action_30_cb), {'finished': 'FINISHED'}) sm_two = smach.StateMachine(outcomes=[ 'FINISHED', 'ERROR', 'CONDITION_PREEMPTED', 'ACTION_PREEMPTED', 'ACTION_ERROR' ]) with sm_two: smach.StateMachine.add( 'CONDITION_SPIKE', smach_ros.MonitorState( '/monitoring/left_wheel_neuron_rate_monitor', String, condition_spike_cb), { 'valid': 'CONDITION_SPIKE', 'invalid': 'ACTION_SPIKE',
def __init__(self, robot, dishwasher_id): StateMachine.__init__(self, outcomes=['succeeded', 'failed']) base_y_position_door = 0 base_y_position_rack = -0.75 base_y_position_rack2 = -0.65 base_yaw_position_rack = 3.7 @cb_interface(outcomes=['done']) def _pre_grab_handle(ud): robot.rightArm.send_gripper_goal("open", timeout=0) robot.rightArm._send_joint_trajectory([[0, 0.2519052373022729913, 0.7746500794619434, 1.3944848321343395, -1.829999276180074, 0.6947045024700284, 0.1889253710114966]], timeout=rospy.Duration(0)) robot.rightArm.wait_for_motion_done() return 'done' @cb_interface(outcomes=['done']) def _grab_handle(ud): robot.rightArm.wait_for_motion_done() robot.speech.speak('I hope this goes right!', block=False) fs = frame_stamped("dishwasher", 0.42, 0, 0.8, roll=math.pi / 2, pitch=0, yaw=math.pi) robot.rightArm.send_goal(fs.projectToFrame(robot.robot_name + "/base_link", robot.tf_listener)) robot.rightArm.send_gripper_goal("close") return 'done' @cb_interface(outcomes=['done']) def _align_with_dishwasher(ud): goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = dishwasher_id goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi)) goal_pose.pose.position.x = 0.85 goal_pose.pose.position.y = base_y_position_door ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01, 0.1)).execute({}) return 'done' @cb_interface(outcomes=['done']) def _drive_to_open_dishwasher(ud): # Open the dishwasher goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = dishwasher_id goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi)) goal_pose.pose.position.x = 1.4 goal_pose.pose.position.y = base_y_position_door robot.torso.low() ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.1, 0.05, 0.5)).execute({}) return 'done' @cb_interface(outcomes=['done']) def _fully_open_dishwasher_door(ud): # Stand in front of the door and rotate robot.torso.high() robot.rightArm._send_joint_trajectory([[0, 0, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0)) goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = dishwasher_id goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, - math.pi / 2)) goal_pose.pose.position.x = 1.4 goal_pose.pose.position.y = base_y_position_door ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.5, 0.5, 0.5, 0.05, 0.1)).execute({}) robot.torso.wait_for_motion_done() robot.rightArm.wait_for_motion_done() # Move arm to the other side of the door robot.rightArm._send_joint_trajectory([[0, 1.57, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0)) robot.rightArm.wait_for_motion_done() robot.rightArm._send_joint_trajectory([[-1.5, 1.57, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0)) robot.rightArm.wait_for_motion_done() robot.rightArm._send_joint_trajectory([[-1.5, 0, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0)) robot.rightArm.wait_for_motion_done() # Go down robot.torso._send_goal([0.1]) robot.torso.wait_for_motion_done() robot.rightArm._send_joint_trajectory([[-0.8, 0, 0, 0, 0, 0, 0]], timeout=rospy.Duration(0)) robot.rightArm.wait_for_motion_done() # Drive away from the door so we open the door with our stretched arm goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = dishwasher_id goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, - math.pi / 2)) goal_pose.pose.position.x = 1.9 goal_pose.pose.position.y = base_y_position_door ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.1, 0.1, 0.1, 0.05, 0.1)).execute({}) robot.rightArm.reset() robot.rightArm.wait_for_motion_done() return 'done' @cb_interface(outcomes=['done']) def _align_with_dishwasher_to_open_rack(ud): robot.torso.low() # Drive sideways around the open door goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = dishwasher_id goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, math.pi)) goal_pose.pose.position.x = 1.4 goal_pose.pose.position.y = base_y_position_rack - 0.15 ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.1, 0.1, 0.1, 0.02, 0.2)).execute({}) # Arm in the right position so we can drive in the dishwasher with the arm robot.rightArm._send_joint_trajectory([[-1.48, 0, 0, 0.5, 1.57, 0, -0.1]], timeout=rospy.Duration(0)) robot.rightArm.wait_for_motion_done() robot.torso.wait_for_motion_done() # Drive aside the open door, with arm in the dishwasher goal_pose.pose.position.x = 0.8 goal_pose.pose.position.y = base_y_position_rack2 goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, base_yaw_position_rack)) ControlToPose(robot, goal_pose, ControlParameters(0.5, 0.5, 0.1, 0.1, 0.1, 0.02, 0.2)).execute({}) return 'done' @cb_interface(outcomes=['done']) def _arm_in_rack(ud): robot.rightArm._send_joint_trajectory([[-1.4, 0, 0, 0.45, 1.57, 0.8, -0.1]], timeout=rospy.Duration(0)) robot.rightArm.wait_for_motion_done() return 'done' @cb_interface(outcomes=['done']) def _drive_to_open_rack(ud): # Open the dishwasher rack goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = dishwasher_id goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, base_yaw_position_rack)) goal_pose.pose.position.x = 1.25 goal_pose.pose.position.y = base_y_position_rack2 ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.1, 0.05, 0.2)).execute({}) return 'done' @cb_interface(outcomes=['done']) def _retract_arm_from_rack(ud): robot.torso.reset() robot.torso.wait_for_motion_done() goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = dishwasher_id goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0.9 * math.pi)) goal_pose.pose.position.x = 1.25 goal_pose.pose.position.y = base_y_position_rack2 ControlToPose(robot, goal_pose, ControlParameters(0.8, 1.0, 0.15, 0.1, 0.4, 0.1, 0.2)).execute({}) robot.rightArm.reset() robot.rightArm.wait_for_motion_done() return 'done' with self: self.add_auto('PRE_GRAB_HANDLE', CBState(_pre_grab_handle), ['done']) self.add_auto('ALIGN_WITH_DISHWASHER', CBState(_align_with_dishwasher), ['done']) self.add_auto('GRAB_HANDLE', CBState(_grab_handle), ['done']) self.add_auto('DRIVE_TO_OPEN_DISHWASHER', CBState(_drive_to_open_dishwasher), ['done']) self.add_auto('FULLY_OPEN_DISHWASHER_DOOR', CBState(_fully_open_dishwasher_door), ['done']) self.add_auto('ALIGN_WITH_DISHWASHER_TO_OPEN_RACK', CBState(_align_with_dishwasher_to_open_rack), ['done']) self.add_auto('ARM_IN_RACK', CBState(_arm_in_rack), ['done']) self.add_auto('DRIVE_TO_OPEN_RACK', CBState(_drive_to_open_rack), ['done']) self.add('RETRACT_ARM_FROM_RACK', CBState(_retract_arm_from_rack), transitions={'done': 'succeeded'})
def __init__(self): # Create a SMACH state machine self.sm = smach.StateMachine(outcomes=['succeeded', 'aborted']) with self.sm: # Add states to the state machine smach.StateMachine.add('GO_TO_CENTER', CBState(self.go_to_center, cb_args=[self]), transitions={ 'lock': 'GO_TO_CENTER', 'succeeded': 'POSITION_CONTROL', 'aborted': 'aborted' }) smach.StateMachine.add('POSITION_CONTROL', CBState(self.position_control, cb_args=[self]), transitions={ 'stay': 'POSITION_CONTROL', 'leave': 'VIBRATORY_PHASE', 'aborted': 'aborted' }) smach.StateMachine.add('VIBRATORY_PHASE', CBState(self.vibratory_phase, cb_args=[self]), transitions={ 'vibrate': 'VIBRATORY_PHASE', 'succeeded': 'RATE_CONTROL', 'aborted': 'aborted' }) smach.StateMachine.add('RATE_CONTROL', CBState(self.rate_control, cb_args=[self]), transitions={ 'stay': 'RATE_CONTROL', 'leave': 'GO_TO_CENTER', 'collision': 'RATE_COLLISION', 'aborted': 'aborted' }) smach.StateMachine.add('RATE_COLLISION', CBState(self.rate_collision, cb_args=[self]), transitions={ 'succeeded': 'GO_TO_CENTER', 'aborted': 'aborted' }) # Read all the parameters from the parameter server # Topics to interact master_name = self.read_parameter('~master_name', 'phantom') slave_name = self.read_parameter('~slave_name', 'grips') self.master_state_topic = '/%s/state' % master_name self.feedback_topic = '/%s/force_feedback' % master_name self.slave_state_topic = '/%s/state' % slave_name self.ik_mc_topic = '/%s/ik_command' % slave_name self.gripper_topic = '/%s/GRIP/command' % slave_name # Workspace definition self.units = self.read_parameter('~units', 'mm') width = self.read_parameter('~workspace/width', 140.0) height = self.read_parameter('~workspace/height', 100.0) depth = self.read_parameter('~workspace/depth', 55.0) self.center_pos = self.read_parameter('~workspace/center', [0, 0, 0]) self.workspace = np.array([width, depth, height]) self.hysteresis = self.read_parameter('~hysteresis', 3.0) self.pivot_dist = self.read_parameter('~pivot_dist', 5.0) # Force feedback parameters self.k_center = self.read_parameter('~k_center', 0.1) self.b_center = self.read_parameter('~b_center', 0.003) self.k_rate = self.read_parameter('~k_rate', 0.05) self.b_rate = self.read_parameter('~b_rate', 0.003) # Position parameters self.rpy_offset = self.read_parameter('~rpy_offset', [0, 0, 0]) self.q_offset = tr.quaternion_from_euler(*self.rpy_offset) self.publish_frequency = self.read_parameter('~publish_rate', 1000.0) self.position_ratio = self.read_parameter('~position_ratio', 250) # Vibration parameters self.vib_a = self.read_parameter('~vibration/a', 2.0) # Amplitude (mm) self.vib_c = self.read_parameter('~vibration/c', 5.0) # Damping self.vib_freq = self.read_parameter('~vibration/frequency', 30.0) # Frequency (Hz) self.vib_time = self.read_parameter('~vibration/duration', 0.3) # Duration (s) self.vib_start_time = 0.0 # Rate parameters self.rate_pivot = np.zeros(3) self.rate_gain = self.read_parameter('~rate_gain', 1.0) # Initial values self.frame_id = self.read_parameter('~reference_frame', 'world') self.colors = TextColors() self.gripper_cmd = 0.0 self.master_pos = None self.master_rot = np.array([0, 0, 0, 1]) self.master_vel = np.zeros(3) self.master_dir = np.zeros(3) self.slave_pos = None self.slave_rot = np.array([0, 0, 0, 1]) self.timer = None self.force_feedback = np.zeros(3) # Synch self.slave_synch_pos = np.zeros(3) self.slave_synch_rot = np.array([0, 0, 0, 1]) self.master_synch_rot = np.array([0, 0, 0, 1]) # Setup Subscribers/Publishers self.feedback_pub = rospy.Publisher(self.feedback_topic, OmniFeedback) self.ik_mc_pub = rospy.Publisher(self.ik_mc_topic, PoseStamped) self.gripper_pub = rospy.Publisher(self.gripper_topic, Float64) self.vis_pub = rospy.Publisher('visualization_marker', Marker) rospy.Subscriber(self.master_state_topic, OmniState, self.cb_master_state) rospy.Subscriber(self.slave_state_topic, EndpointState, self.cb_slave_state) self.loginfo('Waiting for [%s] and [%s] topics' % (self.master_state_topic, self.slave_state_topic)) while not rospy.is_shutdown(): if (self.slave_pos == None) or (self.master_pos == None): rospy.sleep(0.01) else: self.loginfo('Rate position controller running') # Register rospy shutdown hook rospy.on_shutdown(self.shutdown_hook) break # Make sure the first command sent to the slave is equal to its current position6D self.command_pos = np.array(self.slave_pos) self.command_rot = np.array(self.slave_rot) # Start the timer that will publish the ik commands self.timer = rospy.Timer(rospy.Duration(1.0 / self.publish_frequency), self.publish_command) self.loginfo('State machine state: GO_TO_CENTER')
def construct_sm(): sm = StateMachine(outcomes=["succeeded", "aborted", "preempted"]) sm.userdata.nums = range(25, 88, 3) sm.userdata.even_nums = [] sm.userdata.odd_nums = [] with sm: ## %Tag(ITERATOR)% tutorial_it = Iterator( outcomes=["succeeded", "preempted", "aborted"], input_keys=["nums", "even_nums", "odd_nums"], it=lambda: range(0, len(sm.userdata.nums)), output_keys=["even_nums", "odd_nums"], it_label="index", exhausted_outcome="succeeded", ) ## %EndTag(ITERATOR)% ## %Tag(CONTAINER)% with tutorial_it: container_sm = StateMachine( outcomes=["succeeded", "preempted", "aborted", "continue"], input_keys=["nums", "index", "even_nums", "odd_nums"], output_keys=["even_nums", "odd_nums"], ) with container_sm: # test wether even or odd StateMachine.add( "EVEN_OR_ODD", ConditionState( cond_cb=lambda ud: ud.nums[ud.index] % 2, input_keys=["nums", "index"], ), { "true": "ODD", "false": "EVEN" }, ) # add even state @smach.cb_interface( input_keys=["nums", "index", "even_nums"], output_keys=["odd_nums"], outcomes=["succeeded"], ) def even_cb(ud): ud.even_nums.append(ud.nums[ud.index]) return "succeeded" StateMachine.add("EVEN", CBState(even_cb), {"succeeded": "continue"}) # add odd state @smach.cb_interface( input_keys=["nums", "index", "odd_nums"], output_keys=["odd_nums"], outcomes=["succeeded"], ) def odd_cb(ud): ud.odd_nums.append(ud.nums[ud.index]) return "succeeded" StateMachine.add("ODD", CBState(odd_cb), {"succeeded": "continue"}) ## %EndTag(CONTAINER)% ## %Tag(ADDCONTAINER)% # close container_sm Iterator.set_contained_state("CONTAINER_STATE", container_sm, loop_outcomes=["continue"]) ## %EndTag(ADDCONTAINER)% ## %Tag(ADDITERATOR)% # close the tutorial_it StateMachine.add("TUTORIAL_IT", tutorial_it, { "succeeded": "succeeded", "aborted": "aborted" }) ## %EndTag(ADDITERATOR)% return sm
def __init__(self): rospy.init_node('Shopping') self.smach_bool = False rospy.on_shutdown(self.shutdown) self.sm_EnterRoom = StateMachine( outcomes=['succeeded', 'aborted', 'error']) with self.sm_EnterRoom: StateMachine.add('DOOR_DETECT', DoorDetect().door_detect_, transitions={ 'invalid': 'WAIT', 'valid': 'DOOR_DETECT', 'preempted': 'aborted' }) # waits StateMachine.add('WAIT', Wait(), transitions={ 'succeeded': 'ENTER', 'error': 'error' }, remapping={'rec': 'wait_len'}) # StateMachine.add('ENTER', LinearDisplacement(), transitions={'succeeded': 'succeeded', # 'preempted': 'ENTER', # 'error': 'error'}, # remapping={'displacementVec': 'point'}) self.sm_EnterRoom.userdata.start_waypoint = gpsr_target['speaker'][ 'pos'] StateMachine.add('ENTER', NavStack(), transitions={ 'succeeded': 'succeeded', 'aborted': 'ENTER', 'error': 'error' }, remapping={'pos_xm': 'start_waypoint'}) # how to get stop signal? self.trace = Concurrence(outcomes=['remeber', 'stop', 'aborted'], default_outcome='stop', outcome_map={ 'remeber': { 'STOP': 'remeber' }, 'stop': { 'STOP': 'stop' }, 'aborted': { 'FOLLOW': 'aborted' } }, child_termination_cb=self.trace_child_cb, input_keys=['PT_LIST', 'mission'], output_keys=['PT_LIST', 'mission']) with self.trace: self.meta_follow = StateMachine( ['succeeded', 'aborted', 'preempted']) with self.meta_follow: self.meta_follow.userdata.pos_xm = Pose() StateMachine.add('FIND', FindPeople().find_people_, transitions={ 'invalid': 'META_NAV', 'valid': 'FIND', 'preempted': 'preempted' }, remapping={'pos_xm': 'pos_xm'}) self.meta_nav = Concurrence( outcomes=['time_over', 'get_pos', 'aborted'], default_outcome='aborted', outcome_map={ 'time_over': { 'WAIT': 'succeeded' }, 'get_pos': { 'NAV': 'succeeded' }, 'aborted': { 'NAV': 'aborted' } }, child_termination_cb=self.nav_child_cb, input_keys=['pos_xm']) with self.meta_nav: Concurrence.add('NAV', NavStack(), remapping={'pos_xm': 'pos_xm'}) Concurrence.add('WAIT', Wait_trace()) StateMachine.add('META_NAV', self.meta_nav, transitions={ 'get_pos': 'FIND', 'time_over': 'FIND', 'aborted': 'FIND' }) Concurrence.add('FOLLOW', self.meta_follow) Concurrence.add('STOP', CheckStop2(), remapping={ 'PT_LIST': 'PT_LIST', 'mission': 'mission' }) self.Pick = StateMachine(outcomes=['succeeded', 'aborted', 'error'], input_keys=['target']) with self.Pick: self.Pick.userdata.target_pos = PointStamped() self.Pick.userdata.nav_pos = Pose() self.Pick.userdata.pick_pos = PointStamped() self.Pick.userdata.distance = 1.0 self.Pick.userdata.distance2 = 0.9 self.Pick.userdata.target_mode = 1 self.Pick.userdata.objmode = 1 #self.Pick.userdata.target = 'ice_tea' StateMachine.add('FIND', FindObject(), transitions={ 'succeeded': 'DISTANCE', 'aborted': 'aborted', 'error': 'error' }, remapping={ 'name': 'target', 'object_pos': 'target_pos', 'object_map_point': 'object_map_point' }) # StateMachine.add('FIND', self.FindObj, # transitions={'succeeded': 'DISTANCE', # 'aborted': 'aborted', 'error': 'error'}, # remapping={'target': 'target', # 'indice': 'indice', # 'targets': 'targets', # 'object_map_point':'object_map_point', # 'target_pos': 'target_pos'}) # StateMachine.add('FIND', self.FindObj, # transitions={'succeeded': 'DISTANCE', # 'aborted': 'aborted', 'error': 'error'}, # remapping = {'name':'target' , # 'object_pos':'target_pos', # 'object_map_point':'object_map_point'}) StateMachine.add('DISTANCE', CBState(self.PickDistance, outcomes=['succeeded', 'error'], input_keys=['name'], output_keys=['distance']), transitions={ 'succeeded': 'POS_JUS', 'error': 'POS_JUS' }, remapping={ 'distance': 'distance', 'name': 'target' }) StateMachine.add('POS_JUS', new_PosJustfy(), transitions={ 'succeeded': 'NAV', 'aborted': 'aborted', 'error': 'error' }, remapping={ 'pose': 'nav_pos', 'distance': 'distance', 'object_pos': 'target_pos' }) StateMachine.add('NAV', NavStack(), transitions={ 'succeeded': 'FIND_AGAIN', 'aborted': 'NAV', 'error': 'error', }, remapping={'pos_xm': 'nav_pos'}) StateMachine.add('PICK2', ArmCmdForTf(), transitions={ 'succeeded': 'succeeded', 'error': 'error', 'aborted': 'aborted' }, remapping={ 'arm_ps': 'object_map_point', 'mode': 'objmode' }) StateMachine.add('FIND_AGAIN', FindObject(), transitions={ 'succeeded': 'PICK', 'aborted': 'PICK2', 'error': 'error' }, remapping={ 'object_pos': 'pick_pos', 'name': 'target', 'object_state': 'object_state' }) StateMachine.add('PICK', new_ArmCmd(), transitions={ 'succeeded': 'succeeded', 'error': 'error', 'aborted': 'aborted' }, remapping={ 'arm_ps': 'pick_pos', 'mode': 'objmode', 'object_state': 'object_state' }) self.sm_FaceDetect = StateMachine( outcomes=['succeeded', 'aborted', 'error'], output_keys=['people_position', 'num_list']) with self.sm_FaceDetect: self.sm_FaceDetect.userdata.people_position = list() self.sm_FaceDetect.userdata.sentences = 'please look at me' StateMachine.add('SPEAK', Speak(), remapping={'sentences': "sentences"}, transitions={ 'succeeded': 'GET_POSITION', 'aborted': 'aborted', 'error': 'error' }) # call face_reco service for get all the people position which is a list self.sm_FaceDetect.userdata.name_id = -1 # self.sm_FaceDetect.userdata.num_list = list() self.sm_FaceDetect.userdata.distance = 0.6 StateMachine.add('GET_POSITION', FaceReco(), remapping={ 'name_id': 'name_id', 'position': 'people_position', 'num_list': 'num_list', 'distance': 'distance' }, transitions={ 'succeeded': 'succeeded', 'again': 'GET_POSITION', 'aborted': 'GET_POSITION', 'error': 'error', 'turn_l': 'MOVEAHEAD', 'turn_r': 'MOVEAHEAD', 'train_error': 'aborted' }) self.sm_FaceDetect.userdata.point_1 = Point(0.1, 0.0, 0.0) StateMachine.add( 'MOVEAHEAD', SimpleMove_move(), transitions={ 'succeeded': 'GET_POSITION', 'error': 'error', 'aborted': 'GET_POSITION' }, remapping={'point': 'point_1'}, ) self.GetTask = StateMachine(outcomes=['succeeded', 'aborted', 'error'], output_keys=['task']) with self.GetTask: self.GetTask.userdata.people_position = list() self.GetTask.userdata.num_list = list() self.GetTask.userdata.person_position = Pose() StateMachine.add('GET_POSITION', self.sm_FaceDetect, transitions={ 'succeeded': 'GET_VALUE1', 'aborted': 'aborted', 'error': 'error' }, remapping={ 'people_position': 'people_position', 'num_list': 'num_list' }) StateMachine.add('GET_VALUE1', GetValue(), remapping={ 'element_list': 'people_position', 'element': 'person_position' }, transitions={ 'succeeded': 'NAV1', 'aborted': "succeeded", 'error': 'error' }) StateMachine.add('NAV1', NavStack(), transitions={ 'succeeded': 'ASK_TASK1', 'aborted': 'NAV1', 'error': 'error' }, remapping={'pos_xm': 'person_position'}) self.GetTask.userdata.sentence1 = 'what do you want?' StateMachine.add('ASK_TASK1', Speak(), transitions={ 'succeeded': 'ANS1', 'aborted': 'GET_VALUE1', 'error': 'error' }, remapping={'sentence': 'sentence1'}) StateMachine.add('ANS1', ShoppingGetTask(), transitions={ 'succeeded': 'GET_VALUE1', 'aborted': 'GET_VALUE1' }, remapping={'task': 'task'}) # StateMachine.add('ASK_TASK2' , Speak() , transitions={'succeeded':'ANS2', # 'aborted':'aborted', # 'error':'error'}, # remapping={'sentence':'sentence1'}) # StateMachine.add('ANS2' , ShoppingGetTask() , transitions={'succeeded':'succeeded', # 'aborted':'aborted'}, # remapping={'task':'task'}) self.DealTask = StateMachine( outcomes=['succeeded', 'aborted', 'error'], input_keys=['task', 'mission']) with self.DealTask: self.DealTask.userdata.nav_pos = Pose() self.DealTask.userdata.indice = 0 self.DealTask.userdata.indice2 = 3 self.DealTask.userdata.name = '' StateMachine.add('NXT_TASK', ShoppingNextTask(), transitions={ 'go': 'NAV', 'back': 'NAV_CASH', 'finish': 'succeeded', 'aborted': "aborted" }, remapping={ 'pose': 'nav_pos', 'name': 'name', 'indice': 'indice' }) StateMachine.add('NAV', NavStack(), transitions={ 'succeeded': 'PICK', 'aborted': 'NAV', 'error': 'error' }, remapping={'pos_xm', 'nav_pos'}) StateMachine.add('PICK', self.Pick, transitions={ 'succeeded': 'NXT_TASK', 'aborted': 'NXT_TASK', 'error': 'error' }, remapping={'target': 'name'}) StateMachine.add('NAV_CASH', NavStack(), transitions={ 'succeeded': 'PLACE', 'aborted': 'NAV_CASH', 'error': 'error' }, remapping={'pos_xm', 'nav_pos'}) #place need to be upgraded StateMachine.add('PLACE', Place2(), transitions={ 'succeeded': 'NXT_TASK', 'aborted': 'NXT_TASK', }) self.shopping = StateMachine( outcomes=['succeeded', 'aborted', 'error']) with self.shopping: self.shopping.userdata.PT_LIST = {} self.shopping.userdata.mission = {} self.shopping.userdata.task = list() self.shopping.userdata.rec = 5.0 # StateMachine.add('ENTERROOM', # self.sm_EnterRoom, # transitions={'succeeded':'START','aborted':'aborted'}) StateMachine.add('START', GetSignal(), transitions={ 'succeeded': 'RUNNODE', 'aborted': 'aborted' }) StateMachine.add('RUNNODE', RunNode(), transitions={ 'succeeded': 'WAIT', 'aborted': 'aborted' }) StateMachine.add('WAIT', Wait(), transitions={ 'succeeded': 'TRACE', 'error': 'error' }, remapping={'rec': 'rec'}) StateMachine.add('TRACE', self.trace, transitions={ 'remeber': 'TRACE', 'stop': 'GET_TASK', 'aborted': 'aborted' }, remapping={ 'PT_LIST': 'PT_LIST', 'mission': 'mission' }) StateMachine.add('GET_TASK', self.GetTask, transitions={ 'succeeded': 'DEAL_TASK', 'aborted': 'aborted', 'error': 'error' }, remapping={'task': 'task'}) StateMachine.add('DEAL_TASK', self.DealTask, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }, remapping={'task': 'task'}) intro_server = IntrospectionServer('shopping', self.shopping, 'SM_ROOT') intro_server.start() out = self.shopping.execute() intro_server.stop() if out == 'succeeded': self.smach_bool = True
def __init__(self, robot, furniture_designator): # type: (Hero, VariableDesignator) -> None StateMachine.__init__(self, outcomes=['done'], output_keys=["laser_dot"]) is_writeable(furniture_designator) def _show_view(timeout=5): rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo() robot.hmi.show_image_from_msg(rgb, timeout) return rgb, depth, depth_info @cb_interface(outcomes=['done']) def _prepare_operator(_): global OPERATOR OPERATOR = None robot.ed.reset() robot.head.reset() robot.speech.speak("Let's point, please stand in front of me!", block=False) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) robot.speech.speak( "Please point at the object you want me to hand you", block=False) # hmm, weird sentence rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=1) robot.speech.speak("Three") _show_view(timeout=1) robot.speech.speak("Two") _show_view(timeout=1) robot.speech.speak("One") return 'done' @cb_interface(outcomes=['done']) def _get_operator(_): global OPERATOR def _is_operator(person): if person.position.z > 2.5: robot.speech.speak( "Please stand in my view with your full body") return False if person.position.z < 1.5: robot.speech.speak( "Please stand in my view with your full body") return False if "is_pointing" not in person.tags: robot.speech.speak("Please point with your arm stretched") return False return True while not rospy.is_shutdown() and OPERATOR is None: persons = robot.perception.detect_person_3d(*_show_view()) if persons: persons = sorted(persons, key=lambda x: x.position.z) person = persons[0] if _is_operator(person): OPERATOR = person # robot.speech.speak("I see an operator at %.2f meter in front of me" % OPERATOR.position.z) rospy.loginfo("I see an operator at %.2f meter in front of me" % OPERATOR.position.z) return 'done' @cb_interface(outcomes=['done', 'failed'], output_keys=['laser_dot']) def _get_furniture(user_data): global OPERATOR final_result = None while not rospy.is_shutdown() and final_result is None: result = None while not rospy.is_shutdown() and result is None: try: map_pose = robot.tf_listener.transformPose( "map", PoseStamped(header=Header( frame_id=OPERATOR.header.frame_id, stamp=rospy.Time.now() - rospy.Duration.from_sec(0.5)), pose=OPERATOR.pointing_pose)) result = robot.ed.ray_trace(map_pose) except Exception as e: rospy.logerr( "Could not get ray trace from closest person: {}". format(e)) rospy.sleep(1.) # result.intersection_point type: PointStamped # result.entity_id: string rospy.loginfo( "There is a ray intersection with {i} at ({p.x:.4}, {p.y:.4}, {p.z:.4})" .format(i=result.entity_id, p=result.intersection_point.point)) if result.entity_id in all_possible_furniture: final_result = result else: rospy.loginfo("{} is not furniture".format( result.entity_id)) robot.speech.speak("That's not furniture, you dummy.") rospy.sleep(3) OPERATOR = None robot.get_arm().send_joint_goal("reset") return 'failed' robot.speech.speak("You pointed at %s" % final_result.entity_id) robot.get_arm().send_joint_goal("reset") # Fill the designator and user data the furniture inspection furniture_designator.write( robot.ed.get_entity(final_result.entity_id)) user_data['laser_dot'] = result.intersection_point return 'done' with self: self.add('PREPARE_OPERATOR', CBState(_prepare_operator), transitions={'done': 'GET_OPERATOR'}) self.add('GET_OPERATOR', CBState(_get_operator), transitions={'done': 'GET_FURNITURE'}) self.add('GET_FURNITURE', CBState(_get_furniture), transitions={ 'done': 'done', 'failed': 'GET_OPERATOR' })
def __init__(self): rospy.init_node('whoiswho_nopick') rospy.logwarn('start who is who test') self.smach_bool =False rospy.on_shutdown(self.shutdown) # subprocess.call("xterm -e rosrun xm_speech xm_speech_client.py &", shell = True) # add waypoints into the list self.waypoints=[] location= (Point(1.003, -0.217, 0.000), #找人数的点 Point( 5.284, -1.246, 0.000), #出门抓东西的点 Point( 0.000,0.000,0.000)) #结束出门的点 quaternions=(Quaternion(0.000, 0.000, 0.723, 0.691), Quaternion(0.000, 0.000,0.050, 0.999), Quaternion(0.000,0.000,0.000, 1)) # euler_angles=[0.000,3.1415,0.000] # 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(3): self.waypoints.append(Pose(location[i], quaternions[i])) # the locate can specified by ourselves self.sm_EnterRoom = StateMachine(outcomes = ['succeeded','aborted','error']) with self.sm_EnterRoom: # rospy.logerr('sm_EnterRoom add State') # arm go to the nav_pose in the srdf # self.sm_EnterRoom.userdata.arm_mode =0 # self.sm_EnterRoom.userdata.arm_ps_1 = PointStamped() # StateMachine.add('NAV_POSE', # ArmCmd(), # transitions={'succeeded':'DOOR_DETECT','aborted':'NAV_POSE','error':'error'}, # remapping ={'mode':'arm_mode','arm_ps':'arm_ps_1'}) # wait the door to open # StateMachine.add('DOOR_DETECT', # DoorDetect().door_detect_, # transitions={'invalid':'WAIT','valid':'DOOR_DETECT','preempted':'error'}) # simple delay 5s # self.sm_EnterRoom.userdata.rec = 5.0 # StateMachine.add('WAIT', # Wait(), # transitions={'succeeded':'NAV_1','error':'error'}, # remapping ={'rec':'rec'}) # self.sm_EnterRoom.userdata.point = Point(1.5,0.0,0.0) # StateMachine.add('SIMPLE_MOVE', # SimpleMove_move(), # transitions={'succeeded':'NAV_1','aborted':'NAV_1','error':'error'}, # remapping={'point':'point'}) # navstack to room of crowds # waypoints are the list of Pose fit the data type need self.sm_EnterRoom.userdata.start_waypoint = self.waypoints[0] # self.sm_EnterRoom.userdata.nav_mode =0 StateMachine.add('NAV_1', NavStack(), transitions={'succeeded':'SELF_INTRO','aborted':'NAV_1','error':'error'}, remapping = {'pos_xm':'start_waypoint'}) # self-introduce self.sm_EnterRoom.userdata.sentences_2 = 'I am robot xiaomeng' StateMachine.add('SELF_INTRO', Speak(), remapping ={'sentences':'sentences_2'}, transitions ={'succeeded':'succeeded','aborted':'SELF_INTRO','error':'error'}) # we have already identity the people from 0-4 when the first recongization self.sm_FaceDetect = StateMachine(outcomes = ['succeeded','aborted','error'], output_keys = ['people_position','num_list']) with self.sm_FaceDetect: self.sm_FaceDetect.userdata.people_position =list() self.sm_FaceDetect.userdata.sentences = 'please look at me' StateMachine.add('SPEAK', Speak(), remapping = {'sentences':"sentences"}, transitions = {'succeeded':'GET_POSITION','aborted':'aborted','error':'error'}) # call face_reco service for get all the people position which is a list self.sm_FaceDetect.userdata.name_id =-1 self.sm_FaceDetect.userdata.num_list = list() StateMachine.add('GET_POSITION', FaceReco(), remapping ={'name_id':'name_id','position':'people_position','num_list':'num_list'}, transitions ={'succeeded':'succeeded', 'again':'GET_POSITION', 'aborted':'GET_POSITION', 'error':'error', 'turn_l':'TURN_L', 'turn_r':'TURN_R', 'train_error':'aborted'}) # if the face-recognize failed, we should make some remedy to make the state continue self.sm_FaceDetect.userdata.turn_point_1 = Point(0.0,0.0,pi/8) StateMachine.add('TURN_L', SimpleMove_move(), transitions={'succeeded':'SPEAK_2','error':'error'}, remapping ={'point':'turn_point_1'}) self.sm_FaceDetect.userdata.turn_point_2 = Point(0.0,0.0,-pi/8) StateMachine.add('TURN_R', SimpleMove_move(), remapping ={'point':'turn_point_2'}, transitions ={'succeeded':'SPEAK_2','error':'error'}) StateMachine.add('SPEAK_2', Speak(), remapping ={'sentences':'sentences'}, transitions ={'succeeded':'GET_POSITION','aborted':'aborted','error':'error'}) self.sm_GetTarget = StateMachine(outcomes =['succeeded','aborted','error'], input_keys =['target'])#the target is a string..... with self.sm_GetTarget: # because xm is nav to pose in the nav_pose self.sm_GetTarget.userdata.nav_ps = self.waypoints[1] # this smach code donnot to grasp ,so this part is useless StateMachine.add('NAV_TARGET', NavStack(), remapping ={'pos_xm':'nav_ps'}, transitions ={'succeeded':'FIND_OBJECT','aborted':'NAV_TARGET','error':'error'}) self.sm_GetTarget.userdata.object_pos = PointStamped() StateMachine.add('FIND_OBJECT', new_vision.FindObject(), remapping ={'name':'target','object_pos':'object_pos'}, transitions ={'succeeded':'TALK','aborted':'FIND_OBJECT','error':'error'}) self.sm_GetTarget.userdata.sentences = 'I find the target' StateMachine.add('TALK', Speak(), remapping ={'sentences':'sentences'}, transitions ={'succeeded':'PICK','aborted':'TALK','error':'error'}) self.sm_GetTarget.userdata.arm_mode_1 =1 StateMachine.add('PICK', ArmCmd(), remapping ={'mode':'arm_mode_1','arm_ps':'object_pos'}, transitions ={'succeeded':'succeeded','aborted':'succeeded','error':'error'}) self.sm_GetTarget.userdata.arm_mode_2 = 0 self.sm_GetTarget.userdata.arm_ps_2 = PointStamped() StateMachine.add('PUT', PlaceBag(), transitions ={'succeeded':'succeeded','aborted':'aborted'}) # concurrence for the speech_node # we want to make the speech-meaning a timeout, so we use the concurrence function # but in fact we can also use multithread to slove the problem self.meta_Remember = Concurrence(outcomes=['succeeded','aborted','error'], output_keys=['name','target'], default_outcome ='succeeded', outcome_map={'succeeded':{'NAME_AND_THING':'succeeded'}}, child_termination_cb =self.child_cb) with self.meta_Remember: Concurrence.add('GET_EMPTY_NAME', Hehe() ) Concurrence.add('NAME_AND_THING', NameAndThing(), remapping ={'name':'name','target':'target'} ) # input one position is a list # io_keys can wirte and read # userdata can update real-time # no need to call the cv service self.sm_Remember = StateMachine(outcomes =['succeeded','aborted','error'], input_keys =['person_position'], output_keys =['name','target'] ) with self.sm_Remember: # rospy.logerr("sm_Remember add state") # self.sm_Remember.userdata.nav_mode =1 # rospy.logerr("Is here1?") StateMachine.add('NAV_GO', NavStack(), remapping ={'pos_xm':'person_position'}, transitions ={'succeeded':'TALK','aborted':'NAV_GO','error':'error'} ) self.sm_Remember.userdata.sentences = "what is your name and what do you want" StateMachine.add('TALK', Speak(), remapping ={'sentences':'sentences'}, transitions ={'succeeded':'GET_BOTH','aborted':'TALK','error':'error'}) # StateMachine.add('RUNNODE', # RunNode(), # transitions ={'succeeded':'GET_BOTH','aborted':'aborted'}) StateMachine.add('GET_BOTH', self.meta_Remember, remapping ={'name':'name','target':'target'}, transitions ={'succeeded':'WAIT','aborted':'GET_BOTH','error':'error'}) self.sm_Remember.userdata.rec =2.0 StateMachine.add('WAIT', Wait(), remapping={'rec':'rec'}, transitions ={'succeeded':'succeeded','error':"error"}) self.sm_GiveBack = StateMachine(outcomes =['succeeded','aborted','error'], input_keys =['name_id','goal_name','goal_target'])# the name is a string with self.sm_GiveBack: # rospy.logerr('sm_GiveBack add State') # self.sm_GiveBack.userdata.nav_ps = self.waypoints[0] # self.sm_GiveBack.userdata.nav_mode_1 =0 # StateMachine.add('NAV_ROOM', # NavStack(), # remapping ={'pos_xm':'nav_ps','mode':'nav_mode_1'}, # transitions ={'succeeded':'FACE_RECO','aborted':'NAV_ROOM','error':'error'}) # self.sm_GiveBack.userdata.name_id =0 # self.sm_GiveBack.userdata.name_list =list() self.sm_GiveBack.userdata.sentences = "please look at me" StateMachine.add('SPEAK', Speak(), remapping ={'sentences':'sentences'}, transitions ={'succeeded':'WAIT','aborted':'aborted','error':'error'}) self.sm_GiveBack.userdata.rec =5.0 StateMachine.add('WAIT', Wait(), remapping ={'rec':'rec'}, transitions ={'succeeded':'FACE_RECO','error':'error'}) self.sm_GiveBack.userdata.person_position =PointStamped() StateMachine.add('FACE_RECO', FaceReco(), remapping ={'position':'person_position','name_id':'name_id'}, transitions ={'succeeded':'NAV_GO', 'again':'FACE_RECO', 'aborted':'FACE_RECO', 'error':'error', 'turn_l':'TURN_L', 'turn_r':'TURN_R', 'train_error':'aborted'} ) self.sm_GiveBack.userdata.turn_point_1 = Point(0.0,0.0,pi/8) StateMachine.add('TURN_L', SimpleMove_move(), transitions={'succeeded':'SPEAK_2','error':'error'}, remapping ={'point':'turn_point_1'}) self.sm_GiveBack.userdata.turn_point_2 = Point(0.0,0.0,-pi/8) StateMachine.add('TURN_R', SimpleMove_move(), remapping ={'point':'turn_point_2'}, transitions ={'succeeded':'SPEAK_2','error':'error'}) StateMachine.add('SPEAK_2', Speak(), remapping ={'sentences':'sentences'}, transitions ={'succeeded':'FACE_RECO','aborted':'aborted','error':'error'}) #please pay attention! # self.sm_GiveBack.userdata.nav_mode_2 =1 StateMachine.add('NAV_GO', NavStack(), remapping ={'pos_xm':'person_position'}, transitions ={'succeeded':'GET_NAME','aborted':'NAV_GO','error':'error'}) StateMachine.add("GET_NAME", NameHehe2(), remapping ={'goal_target':'goal_name','goal_name':'goal_name','goal_target':'goal_target', 'sentences':'sentences_1'}, transitions ={'succeeded':'TALK_1','aborted':'aborted','error':'error'}) self.sm_GiveBack.userdata.sentences_1 ='' StateMachine.add('TALK_1', Speak(), remapping ={'sentences':"sentences_1"}, transitions ={'succeeded':'PUT','aborted':"PUT",'error':'error'}) StateMachine.add('PUT' , PlaceBag(), transitions = {'succeeded':'succeeded', 'aborted':'succeeded'}) # this pose should be specified to make the people get it # self.sm_GiveBack.userdata.arm_mode =2 # self.sm_GiveBack.userdata.place_ps = PointStamped() # self.sm_GiveBack.userdata.place_ps.header.frame_id ='base_footprint' # self.sm_GiveBack.userdata.place_ps.point.x =0.7 # self.sm_GiveBack.userdata.place_ps.point.y =0.0 # self.sm_GiveBack.userdata.place_ps.point.z =0.3 # StateMachine.add('PLACE', # ArmCmd(), # remapping ={'mode':'arm_mode','arm_ps':'place_ps'}, # transitions ={'succeeded':'TALK_2','aborted':'PLACE','error':'error'}) # self.sm_GiveBack.userdata.sentences_2 = 'I get the thing you want, am I?' # StateMachine.add('TALK_2', # Speak(), # remapping ={'sentences':'sentences_2'}, # transitions ={'succeeded':'succeeded','aborted':"aborted",'error':'error'}) self.sm_EndTask = StateMachine(outcomes =['succeeded','aborted','error']) with self.sm_EndTask: # rospy.logerr('sm_EndTask add State') self.sm_EndTask.userdata.nav_ps = self.waypoints[2] # self.sm_EndTask.userdata.nav_mode = 0 StateMachine.add('NAV_BYEBYE', NavStack(), remapping ={'pos_xm':'nav_ps'}, transitions ={'succeeded':'succeeded','aborted':'NAV_BYEBYE','error':'error'}) self.sm_EndTask.userdata.point = Point(1.8,0.0,0.0) # StateMachine.add('SIMPLE_MOVE', # SimpleMove_move(), # remapping={'point':'point'}, # transitions={'succeeded':'succeeded','aborted':'aborted','error':'error'}) self.WHOISWHO = StateMachine(outcomes =['succeeded','aborted','error']) with self.WHOISWHO: # we can generate some state which contant the list_value StateMachine.add('ENTERROOM', self.sm_EnterRoom, transitions ={'succeeded':'FACEDETECT','aborted':'aborted','error':'error'}) self.WHOISWHO.userdata.people_position =list() self.WHOISWHO.userdata.num_list = list() StateMachine.add('FACEDETECT', self.sm_FaceDetect, remapping ={'people_position':'people_position','num_list':'num_list'}, transitions = {'succeeded':'GETPERSON','aborted':'aborted','error':'error'} ) self.WHOISWHO.userdata.person_position = PointStamped() # if someone is recongized failed, we should make the total list in the same order # get the last element of the list # FGM: This state is to get different people pos StateMachine.add('GETPERSON', GetValue(), remapping ={'element_list':'people_position','element':'person_position'}, transitions ={'succeeded':'REMEMBER','aborted':"GETID",'error':'error'} ) # the cv will remember the face with the increasing ID self.WHOISWHO.userdata.name ='' self.WHOISWHO.userdata.target= '' self.WHOISWHO.userdata.name_list =list() self.WHOISWHO.userdata.target_list =list() self.WHOISWHO.userdata.goal_name ='' self.WHOISWHO.userdata.goal_target'' # if donnot need to remember, the order of ids should be the same with the positions return # the first of the position should be the 0 # last be the 4 StateMachine.add('REMEMBER', self.sm_Remember, remapping ={'person_position':'person_position','name':'name','target':'target'}, transitions ={'succeeded':'NAMEINLIST','aborted':'aborted','error':'error'} ) #this state use for joining the name and the target into the name_list and target_list # insert in the head of the lists # if the name and target are empty, we should also append them to the lists StateMachine.add('NAMEINLIST', NameInList(), remapping ={'name':'name','target':'target','name_list':'name_list','target_list':'target_list'}, transitions ={'succeeded':'GETPERSON','aborted':'aborted','error':'error'} ) # ############################################################################ # so far here ,the tasks of remember is completed , the rest is the tasks to return the target to the people # we should take out the name and the matched target for xm to grasp and give back # in fact, we will pop the name and target out the list,so the name_id is gradually reduced # ############################################################################ StateMachine.add('GETTARGET', GetValue2(), remapping ={'element_list':'target_list','element':'target'}, transitions ={'succeeded':'CATCHTARGET','aborted':'ENDTASK','error':'error'}) StateMachine.add('CATCHTARGET', self.sm_GetTarget, transitions= {'succeeded':'NAV_ROOM','aborted':'NAV_ROOM','error':'error'}, remapping = {'target':'target'}) # ############################################################################ self.WHOISWHO.userdata.nav_ps = self.waypoints[0] # self.WHOISWHO.userdata.nav_mode_1 =0 StateMachine.add('NAV_ROOM', NavStack(), remapping ={'pos_xm':'nav_ps'}, transitions ={'succeeded':'GIVEBACK','aborted':'NAV_ROOM','error':'error'}) StateMachine.add('CHECKFINISH', CBState(self.checkfinish,outcomes=['finish','continue'],input_keys=['num_list']), transitions={'finish':'ENDTASK', 'continue':'GETTARGET'}, remapping={'num_list':'num_list'}) # StateMachine.add('CLEAR_MAP', # ClearMap(), # transitions ={'succeeded':'GETID','aborted':'GETID'}) # get the last num of the list, ID # if the name and target list is empty, will skipped this item self.WHOISWHO.userdata.sentences_1 = 'please change the order' StateMachine.add('SPEAK_1', Speak(), remapping ={'sentences':'sentences_1'}, transitions ={'succeeded':'WAIT_HEHE','aborted':'WAIT_HEHE','error':'error'}) self.WHOISWHO.userdata.rec_hehe =10.0 StateMachine.add('WAIT_HEHE', Wait(), remapping ={'rec':'rec_hehe'}, transitions ={'succeeded':'SPEAK_2','error':'error'}) self.WHOISWHO.userdata.sentences_2 = 'I will make the recongization task' StateMachine.add('SPEAK_2', Speak(), transitions ={'succeeded':'GETID','aborted':'GETID','error':'error'}, remapping ={'sentences':'sentences_2'}) self.WHOISWHO.userdata.name_id = 0 StateMachine.add('GETID', GetId(), remapping ={'output_id':'name_id','input_list':'name_list','num_list':'num_list'}, transitions ={'succeeded':'GETTARGET','aborted':'ENDTASK','error':'error'} ) StateMachine.add('GIVEBACK', self.sm_GiveBack, remapping ={'name_id':'name_id','name_list':'name_list','target_list':'target_list'}, transitions ={'succeeded':'CHECKFINISH','aborted':'CHECKFINISH','error':'error'} ) StateMachine.add('ENDTASK', self.sm_EndTask, transitions ={'succeeded':'succeeded','aborted':'aborted','error':'error'}) # rospy.logerr('sm_Top execute begin') intro_server = IntrospectionServer('whoiswho',self.WHOISWHO, 'SM_ROOT') intro_server.start() out = self.WHOISWHO.execute() intro_server.stop() if out == 'succeeded': self.smach_bool = True
if __name__ == '__main__': rospy.init_node('drone_state_machine') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['outcome']) sm.userdata.lspeed = 0.5 sm.userdata.rspeed = 1.2 # Create and start the introspection server sis = smach_ros.IntrospectionServer('drone_server', sm, '/SM_DRONE') sis.start() # Open the container with sm: # Add states to the container smach.StateMachine.add('TAKEOFF', CBState(takeoff_cb), {'finished': 'MOVE', 'failed':'outcome'}) smach.StateMachine.add('MOVE', CBState(move_cb), {'finished': 'ROTATE', 'failed':'outcome'}) smach.StateMachine.add('ROTATE', CBState(rotate_cb), {'finished': 'LAND', 'failed':'outcome'}) smach.StateMachine.add('LAND', CBState(land_cb), {'finished': 'outcome', 'failed':'outcome'}) # Execute SMACH plan outcome = sm.execute() sis.stop()
def __init__(self, robot, cupboard_id, required_items): StateMachine.__init__(self, outcomes=['succeeded', 'failed'], output_keys=["item_picked"]) arm = robot.get_arm()._arm picked_items = [] def send_joint_goal(position_array, wait_for_motion_done=True): arm._send_joint_trajectory([position_array], timeout=rospy.Duration(0)) if wait_for_motion_done: arm.wait_for_motion_done() def send_gripper_goal(open_close_string, max_torque=0.1): arm.send_gripper_goal(open_close_string, max_torque=max_torque) rospy.sleep(1.0) # Does not work with motion_done apparently def show_image(package_name, path_to_image_in_package): path = os.path.join(rospkg.RosPack().get_path(package_name), path_to_image_in_package) if not os.path.exists(path): rospy.logerr("Image path {} does not exist".format(path)) else: try: rospy.loginfo("Showing {}".format(path)) robot.hmi.show_image(path, 10) except Exception as e: rospy.logerr("Could not show image {}: {}".format(path, e)) return 'succeeded' @cb_interface(outcomes=['succeeded', 'failed'], output_keys=["item_picked"]) def _ask_user(user_data): leftover_items = [item for item in required_items if item not in picked_items] if not leftover_items: robot.speech.speak("We picked 'm all apparently") return 'failed' item_name = leftover_items[0] if item_name == 'plate': send_joint_goal(plate_handover) else: arm.send_joint_goal("carrying_pose") picked_items.append(item_name) robot.speech.speak("Please put the {} in my gripper, like this".format(item_name), block=False) show_image('challenge_set_the_table', item_img_dict[item_name]) send_gripper_goal("open") rospy.sleep(5.0) robot.speech.speak("Thanks for that!", block=False) if item_name == 'plate': send_gripper_goal("close", max_torque=0.7) else: send_gripper_goal("close") # Set output data user_data['item_picked'] = item_name arm.send_joint_goal("carrying_pose") return 'succeeded' with self: self.add('ASK_USER', CBState(_ask_user), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
#else: #return 'failed' ### -- BUCLE PRINCIPAL -- ### if __name__ == '__main__': rospy.init_node('altern_tripod_state_machine') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['outcome']) # Open the container with sm: # Stand UP state smach.StateMachine.add('ON_FLOOR', CBState(on_floor_cb), { 'finished': 'STOP', 'failed': 'outcome' }) # Stop state smach.StateMachine.add( 'STOP', CBState(stop_cb), { 'finished_1': 'ALTERN_TRIPOD', 'finished_2': 'TETRAPOD', 'finished_3': 'STAIR_CLIMB', 'failed': 'outcome' }) # Altern tripod state smach.StateMachine.add('ALTERN_TRIPOD', CBState(altern_tripod_cb), {