def __init__(self, robot, order_type): smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) beverage_dest_desig = EdEntityDesignator(robot) #.id is overwritten by instruct_barman with self: @smach.cb_interface(outcomes=['spoken']) def instruct_barman(userdata=None): try: order = ORDERS[order_type] beverage_dest_desig.id = order['location'] robot.speech.speak("Barman, please put a {name} in my basket for table {location}".format(**order)) except KeyError: rospy.logerr("No beverage in ORDERS") return 'spoken' smach.StateMachine.add( 'INSTRUCT_BARMAN', smach.CBState(instruct_barman), transitions={'spoken' :'AWAIT_PUT_ORDER_CONFIRMATION'}) smach.StateMachine.add( 'AWAIT_PUT_ORDER_CONFIRMATION', states.WaitTime(robot, 8), transitions={ 'waited' :'GOTO_ORDER_DESTINATION_1', 'preempted' :'failed'}) smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_1', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS), transitions={ 'arrived' :'SAY_TAKE_ORDER', 'unreachable' :'GOTO_ORDER_DESTINATION_2', 'goal_not_defined' :'GOTO_ORDER_DESTINATION_2'}) smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_2', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS), transitions={ 'arrived' :'SAY_TAKE_ORDER', 'unreachable' :'failed', 'goal_not_defined' :'failed'}) @smach.cb_interface(outcomes=['spoken']) def instruct_guest(userdata=None): try: order = ORDERS[order_type] robot.speech.speak("Dear guest at table {location}, you can get your {name} from my basket.".format(**order)) except KeyError: rospy.logerr("No beverage in ORDERS") return 'spoken' smach.StateMachine.add( 'SAY_TAKE_ORDER', smach.CBState(instruct_guest), transitions={'spoken' :'AWAIT_TAKE_ORDER_CONFIRMATION'}) smach.StateMachine.add( 'AWAIT_TAKE_ORDER_CONFIRMATION', states.WaitTime(robot, 5), transitions={ 'waited' :'SAY_ENJOY_ORDER', 'preempted' :'failed'}) smach.StateMachine.add( 'SAY_ENJOY_ORDER', states.Say(robot, ["Enjoy your {}".format(order_type)], block=False), transitions={ 'spoken' :'succeeded'})
def __init__(self, robot): smach.StateMachine.__init__( self, outcomes=['container_success', 'container_failed']) with self: smach.StateMachine.add('SAY_STARTING_TEST', states.Say( robot, "Starting people recognition test", block=False), transitions={'spoken': 'NAV_TO_WAYPOINT'}) smach.StateMachine.add( 'NAV_TO_WAYPOINT', states.NavigateToWaypoint( robot, EdEntityDesignator(robot, id=challenge_knowledge.wp_test_nav)), transitions={ 'arrived': 'LOOK_AT_OPERATOR', 'unreachable': 'SAY_FAILED_WAYPOINT', 'goal_not_defined': 'SAY_FAILED_WAYPOINT' }) smach.StateMachine.add('SAY_FAILED_WAYPOINT', states.Say(robot, "Failed reaching the waypoint.", block=True), transitions={'spoken': 'LOOK_AT_OPERATOR'}) smach.StateMachine.add('LOOK_AT_OPERATOR', states_interaction.LookAtPersonInFront( robot, lookDown=False), transitions={ 'succeeded': 'RECOGNIZE_PEOPLE', 'failed': 'RECOGNIZE_PEOPLE' }) smach.StateMachine.add('RECOGNIZE_PEOPLE', test_states.RecognizePeople(robot), transitions={ 'succeeded': 'container_success', 'failed': 'SAY_NO_PEOPLE', 'no_people': 'SAY_NO_PEOPLE' }) smach.StateMachine.add('SAY_NO_PEOPLE', states.Say( robot, "I don see anyone in front of me.", block=False), transitions={'spoken': 'LOOK_AT_OPERATOR'})
def __init__(self, robot, cupboard_id, cupboard_navigation_area): StateMachine.__init__(self, outcomes=["succeeded", "failed"]) cupboard = EdEntityDesignator(robot=robot, id=cupboard_id) with self: StateMachine.add("NAVIGATE_TO_CUPBOARD", NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area}, cupboard), transitions={'arrived': 'CLOSE_CUPBOARD', 'unreachable': 'failed', 'goal_not_defined': 'failed'}) StateMachine.add("CLOSE_CUPBOARD", CloseCupboard(robot, cupboard_id), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
def __init__(self, robot, cupboard_id, cupboard_navigation_area, required_items): StateMachine.__init__(self, outcomes=["succeeded", "failed"], output_keys=["item_picked"]) cupboard = EdEntityDesignator(robot=robot, id=cupboard_id) with self: StateMachine.add("NAVIGATE_TO_CUPBOARD", NavigateToSymbolic(robot, {cupboard: cupboard_navigation_area}, cupboard), transitions={'arrived': 'PICK_ITEM_FROM_CUPBOARD', 'unreachable': 'failed', 'goal_not_defined': 'failed'}) StateMachine.add("PICK_ITEM_FROM_CUPBOARD", PickItemFromCupboardDrawer(robot, cupboard_id, required_items), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
def __init__(self, robot, dishwasher_id, dishwasher_navigate_area): StateMachine.__init__(self, outcomes=["succeeded", "failed"]) dishwasher = EdEntityDesignator(robot=robot, id=dishwasher_id) with self: StateMachine.add("NAVIGATE_TO_DISHWASHER", NavigateToSymbolic(robot, {dishwasher: dishwasher_navigate_area}, dishwasher), transitions={'arrived': 'OPEN_DISHWASHER', 'unreachable': 'failed', 'goal_not_defined': 'failed'}) StateMachine.add("OPEN_DISHWASHER", OpenDishwasher(robot, dishwasher_id), transitions={'succeeded': 'succeeded', 'failed': 'failed'})
def __init__(self, robot, rack_id, rack_navigation_area): StateMachine.__init__(self, outcomes=["succeeded", "failed"]) rack = EdEntityDesignator(robot=robot, id=rack_id) with self: StateMachine.add("NAVIGATE_TO_RACK", NavigateToSymbolic(robot, {rack: rack_navigation_area}, rack), transitions={ 'arrived': 'GRAB_RACK', 'unreachable': 'failed', 'goal_not_defined': 'failed' }) StateMachine.add("GRAB_RACK", GrabRack(robot, rack_id), transitions={'done': 'succeeded'})
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) des = EdEntityDesignator(robot=robot, id='left_planks') arm_des = UnoccupiedArmDesignator(robot.arms, robot.leftArm) with sm: # Start challenge via StartChallengeRobust, skipped atm smach.StateMachine.add("INSPECT_AND_GRAB", InspectAndGrab(robot, supporting_entity_designator=des, arm_designator=arm_des), transitions={ "succeeded": "Done", "inspect_failed": "Aborted", "grasp_failed": "Aborted" }) return sm
def __init__(self, robot, personNameDesLocal): @smach.cb_interface(outcomes=['spoken']) def saySearchingOperatorCB(userdata=None): printOk("saySearchingOperatorCB") robot.speech.speak("I am searching for " + personNameDesLocal.resolve() + "!", block=False) return 'spoken' smach.StateMachine.__init__( self, outcomes=['container_success', 'container_failed']) with self: smach.StateMachine.add( 'NAV_TO_WAYPOINT', states.NavigateToWaypoint( robot, EdEntityDesignator(robot, id=challenge_knowledge.wp_test_nav)), transitions={ 'arrived': 'SAY_SEARCHING_OPERATOR', 'unreachable': 'SAY_FAILED_WAYPOINT', 'goal_not_defined': 'SAY_FAILED_WAYPOINT' }) smach.StateMachine.add( 'SAY_FAILED_WAYPOINT', states.Say(robot, "Failed reaching the waypoint.", block=True), transitions={'spoken': 'SAY_SEARCHING_OPERATOR'}) smach.StateMachine.add('SAY_SEARCHING_OPERATOR', smach.CBState(saySearchingOperatorCB), transitions={'spoken': 'SAY_UNFINSHED'}) smach.StateMachine.add('SAY_UNFINSHED', states.Say(robot, "This part is not finished yet.", block=False), transitions={'spoken': 'container_success'})
with self: if navigation_area: smach.StateMachine.add('NAVIGATE_TO_INSPECT', NavigateToSymbolic(robot, {entityDes: navigation_area}, entityDes), transitions={'unreachable': 'failed', 'goal_not_defined': 'failed', 'arrived': 'SEGMENT'}) else: smach.StateMachine.add('NAVIGATE_TO_INSPECT', NavigateToObserve(robot, entityDes, radius=1.0), transitions={'unreachable': 'failed', 'goal_not_defined': 'failed', 'arrived': 'SEGMENT'}) smach.StateMachine.add('SEGMENT', SegmentObjects(robot, objectIDsDes.writeable, entityDes, searchArea, threshold=threshold), transitions={'done': 'done'}) if __name__ == "__main__": from robot_skills.amigo import Amigo from robot_smach_states.util.designators import EdEntityDesignator rospy.init_node('state_machine') robot = Amigo() sm = Inspect(robot=robot, entityDes=EdEntityDesignator(robot=robot, id="closet")) print sm.execute()
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: # Start challenge via StartChallengeRobust smach.StateMachine.add("START_CHALLENGE_ROBUST", states.StartChallengeRobust( robot, challenge_knowledge.starting_point, use_entry_points=True), transitions={ "Done": "SAY_GOTO_TARGET2", "Aborted": "SAY_GOTO_TARGET2", "Failed": "SAY_GOTO_TARGET2" }) smach.StateMachine.add( 'SAY_GOTO_TARGET1', states.Say(robot, [ "I will go to target 1 now", "I will now go to target 1", "Lets go to target 1", "Going to target 1" ], block=False), transitions={'spoken': 'GOTO_TARGET1'}) ###################################################################################################################################################### # # TARGET 1 # ###################################################################################################################################################### smach.StateMachine.add('GOTO_TARGET1', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target1), challenge_knowledge.target1_radius1), transitions={ 'arrived': 'SAY_TARGET1_REACHED', 'unreachable': 'RESET_ED_TARGET1', 'goal_not_defined': 'RESET_ED_TARGET1' }) smach.StateMachine.add( 'SAY_TARGET1_REACHED', states.Say(robot, [ "Reached target 1", "I have arrived at target 1", "I am now at target 1" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET3'}) smach.StateMachine.add('RESET_ED_TARGET1', states.ResetED(robot), transitions={'done': 'GOTO_TARGET1_BACKUP'}) smach.StateMachine.add('GOTO_TARGET1_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target1), challenge_knowledge.target1_radius2), transitions={ 'arrived': 'SAY_TARGET1_REACHED', 'unreachable': 'TIMEOUT1', 'goal_not_defined': 'TIMEOUT1' }) smach.StateMachine.add('TIMEOUT1', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'GOTO_TARGET1', 'time_out': 'SAY_TARGET1_FAILED' }) # Should we mention that we failed??? smach.StateMachine.add( 'SAY_TARGET1_FAILED', states.Say(robot, [ "I am not able to reach target 1", "I cannot reach target 1", "Target 1 is unreachable" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET3'}) ###################################################################################################################################################### # # TARGET 2 # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOTO_TARGET2', states.Say(robot, [ "I will go to target 2 now", "I will now go to target 2", "Lets go to target 2", "Going to target 2" ], block=False), transitions={'spoken': 'GOTO_TARGET2_PRE'}) smach.StateMachine.add( 'GOTO_TARGET2_PRE', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=challenge_knowledge.target2_pre), challenge_knowledge.target2_pre_radius1, EntityByIdDesignator(robot, id=challenge_knowledge.target2)), transitions={ 'arrived': 'GOTO_TARGET2', 'unreachable': 'TIMEOUT2', 'goal_not_defined': 'TIMEOUT2' }) smach.StateMachine.add('GOTO_TARGET2', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target2), challenge_knowledge.target2_radius1), transitions={ 'arrived': 'SAY_TARGET2_REACHED', 'unreachable': 'DETERMINE_OBJECT', 'goal_not_defined': 'DETERMINE_OBJECT' }) smach.StateMachine.add( 'DETERMINE_OBJECT', DetermineObject(robot, challenge_knowledge.target2, challenge_knowledge.target2_obstacle_radius), transitions={ 'done': 'GOTO_TARGET2_AGAIN', 'timeout': 'GOTO_TARGET2_AGAIN' }) smach.StateMachine.add('GOTO_TARGET2_AGAIN', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target2), challenge_knowledge.target2_radius1), transitions={ 'arrived': 'SAY_TARGET2_REACHED', 'unreachable': 'RESET_ED_TARGET2', 'goal_not_defined': 'RESET_ED_TARGET2' }) smach.StateMachine.add( 'SAY_TARGET2_REACHED', states.Say(robot, [ "Reached target 2", "I have arrived at target 2", "I am now at target 2" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET1'}) smach.StateMachine.add('RESET_ED_TARGET2', states.ResetED(robot), transitions={'done': 'GOTO_TARGET2_BACKUP'}) smach.StateMachine.add('GOTO_TARGET2_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target2), challenge_knowledge.target2_radius2), transitions={ 'arrived': 'SAY_TARGET2_REACHED', 'unreachable': 'TIMEOUT2', 'goal_not_defined': 'TIMEOUT2' }) smach.StateMachine.add('TIMEOUT2', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'GOTO_TARGET2_PRE', 'time_out': 'SAY_TARGET2_FAILED' }) smach.StateMachine.add( 'SAY_TARGET2_FAILED', states.Say(robot, [ "I am unable to reach target 2", "I cannot reach target 2", "Target 2 is unreachable" ], block=True), transitions={'spoken': 'SAY_GOTO_TARGET1'}) ###################################################################################################################################################### # # TARGET 3 # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOTO_TARGET3', states.Say(robot, [ "I will go to target 3 now", "I will now go to target 3", "Lets go to target 3", "Going to target 3" ], block=False), transitions={'spoken': 'GOTO_TARGET3'}) smach.StateMachine.add('GOTO_TARGET3', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target3), challenge_knowledge.target3_radius1), transitions={ 'arrived': 'SAY_TARGET3_REACHED', 'unreachable': 'RESET_ED_TARGET3', 'goal_not_defined': 'RESET_ED_TARGET3' }) smach.StateMachine.add( 'SAY_TARGET3_REACHED', states.Say(robot, [ "Reached target 3", "I have arrived at target 3", "I am now at target 3" ], block=True), transitions={'spoken': 'TURN'}) smach.StateMachine.add('RESET_ED_TARGET3', states.ResetED(robot), transitions={'done': 'GOTO_TARGET3_BACKUP'}) smach.StateMachine.add('GOTO_TARGET3_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target3), challenge_knowledge.target3_radius2), transitions={ 'arrived': 'SAY_TARGET3_REACHED', 'unreachable': 'TIMEOUT3', 'goal_not_defined': 'TIMEOUT3' }) smach.StateMachine.add('TIMEOUT3', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'GOTO_TARGET3', 'time_out': 'SAY_TARGET3_FAILED' }) # Should we mention that we failed??? smach.StateMachine.add( 'SAY_TARGET3_FAILED', states.Say(robot, [ "I am unable to reach target 3", "I cannot reach target 3", "Target 3 is unreachable" ], block=True), transitions={'spoken': 'TURN'}) ###################################################################################################################################################### # # Follow waiter # ###################################################################################################################################################### smach.StateMachine.add('TURN', Turn(robot, challenge_knowledge.rotation), transitions={'turned': 'SAY_STAND_IN_FRONT'}) smach.StateMachine.add( 'SAY_STAND_IN_FRONT', states.Say(robot, "Please stand in front of me!", block=True, look_at_standing_person=True), transitions={'spoken': 'FOLLOW_WITH_DOOR_CHECK'}) # TODO: Fix concurrence door_id_designator = VariableDesignator( challenge_knowledge.target_door_1) open_door_wp1_des = VariableDesignator(resolve_type=str) open_door_wp2_des = VariableDesignator(resolve_type=str) cc = smach.Concurrence( ['stopped', 'no_operator', 'lost_operator'], default_outcome='no_operator', child_termination_cb=lambda so: True, outcome_map={ 'stopped': { 'FOLLOW_OPERATOR': 'stopped' }, # 'stopped': {'FOLLOW_OPERATOR': 'stopped', 'DETERMINE_DOOR': 'door_found'}, 'no_operator': { 'FOLLOW_OPERATOR': 'no_operator' }, # 'no_operator': {'FOLLOW_OPERATOR': 'no_operator', 'DETERMINE_DOOR': 'door_found'}, # 'lost_operator': {'FOLLOW_OPERATOR': 'lost_operator', 'DETERMINE_DOOR': 'preempted'}, 'lost_operator': { 'FOLLOW_OPERATOR': 'lost_operator' } }) with cc: smach.Concurrence.add('FOLLOW_OPERATOR', states.FollowOperator(robot, replan=True)) smach.Concurrence.add('DETERMINE_DOOR', DetermineDoor(robot, door_id_designator)) smach.StateMachine.add('FOLLOW_WITH_DOOR_CHECK', cc, transitions={ 'no_operator': 'FOLLOW_WITH_DOOR_CHECK', 'stopped': 'SAY_SHOULD_I_RETURN', 'lost_operator': 'SAY_SHOULD_I_RETURN' }) # smach.StateMachine.add( 'FOLLOW_OPERATOR', states.FollowOperator(robot, replan=True), transitions={ 'no_operator':'SAY_SHOULD_I_RETURN', 'stopped' : 'SAY_SHOULD_I_RETURN', 'lost_operator' : 'SAY_SHOULD_I_RETURN'}) smach.StateMachine.add('SAY_SHOULD_I_RETURN', states.Say(robot, "Should I return to target 3?", look_at_standing_person=True), transitions={'spoken': 'HEAR_SHOULD_I_RETURN'}) smach.StateMachine.add('HEAR_SHOULD_I_RETURN', states.HearOptions(robot, ["yes", "no"]), transitions={ 'no_result': 'SAY_STAND_IN_FRONT', "yes": "SELECT_WAYPOINTS", "no": "SAY_STAND_IN_FRONT" }) smach.StateMachine.add('SELECT_WAYPOINTS', SelectWaypoints(door_id_designator, open_door_wp1_des, open_door_wp2_des), transitions={'done': 'SAY_GOBACK_ARENA'}) ###################################################################################################################################################### # # RETURN TO ARENA DOOR # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOBACK_ARENA', states.Say(robot, [ "I will go back to the arena", "I will return to the arena", "Lets return to the arena", "Going back to the arena", "Returning to the arena" ], block=False), transitions={'spoken': 'GOTO_ARENA_DOOR'}) smach.StateMachine.add( 'GOTO_ARENA_DOOR', states.NavigateToWaypoint( robot, EdEntityDesignator(robot, id_designator=door_id_designator), challenge_knowledge.target_door_radius), transitions={ 'arrived': 'ARENA_DOOR_REACHED', 'unreachable': 'RESET_ED_ARENA_DOOR', 'goal_not_defined': 'RESET_ED_ARENA_DOOR' }) smach.StateMachine.add('ARENA_DOOR_REACHED', states.Say(robot, [ "I am at the door of the arena", "I have arrived at the door of the arena", "I am now at the door of the arena" ], block=True), transitions={'spoken': 'SAY_OPEN_DOOR'}) smach.StateMachine.add('RESET_ED_ARENA_DOOR', states.ResetED(robot), transitions={'done': 'GOTO_ARENA_DOOR_BACKUP'}) smach.StateMachine.add( 'GOTO_ARENA_DOOR_BACKUP', states.NavigateToWaypoint( robot, EdEntityDesignator(robot, id_designator=door_id_designator), challenge_knowledge.target_door_radius), transitions={ 'arrived': 'ARENA_DOOR_REACHED', 'unreachable': 'TIMEOUT_ARENA_DOOR', 'goal_not_defined': 'TIMEOUT_ARENA_DOOR' }) smach.StateMachine.add('TIMEOUT_ARENA_DOOR', checkTimeOut( robot, challenge_knowledge.time_out_seconds_door), transitions={ 'not_yet': 'GOTO_ARENA_DOOR', 'time_out': 'SAY_GOTO_ARENA_DOOR_FAILED' }) smach.StateMachine.add('SAY_GOTO_ARENA_DOOR_FAILED', states.Say(robot, [ "I am unable to reach the arena door", "I cannot reach the arena door", "The arena door is unreachable" ], block=True), transitions={'spoken': 'Done'}) ###################################################################################################################################################### # # Opening Door # ###################################################################################################################################################### smach.StateMachine.add( 'OPEN_DOOR', states.OpenDoorByPushing( robot, EdEntityDesignator(robot, id_designator=open_door_wp1_des), EdEntityDesignator(robot, id_designator=open_door_wp2_des)), transitions={ 'succeeded': 'SAY_RETURN_TARGET3', 'failed': 'TIMEOUT_ARENA_DOOR_OPENING' }) smach.StateMachine.add( 'SAY_OPEN_DOOR', states.Say(robot, [ "I am going to open the door", "Going to open the door of the arena", "Door, open sesame" ], block=True), transitions={'spoken': 'OPEN_DOOR'}) smach.StateMachine.add( 'SAY_OPEN_DOOR_AGAIN', states.Say(robot, [ "I failed to open the door. I will try it again", "Let me try again to open the door" ], block=True), transitions={'spoken': 'OPEN_DOOR'}) smach.StateMachine.add('TIMEOUT_ARENA_DOOR_OPENING', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'SAY_OPEN_DOOR_AGAIN', 'time_out': 'SAY_OPEN_DOOR_FAILED' }) smach.StateMachine.add( 'SAY_OPEN_DOOR_FAILED', states.Say(robot, [ "I was not able to open the door. I am done with this challange", "I was not able to open the door. I am done with this challange" ], block=True), transitions={'spoken': 'Done'}) ###################################################################################################################################################### # # RETURN TO TARGET 3 # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_RETURN_TARGET3', states.Say(robot, [ "I will go back to target 3 now", "I will return to target 3", "Lets go to target 3 again", "Going to target 3, again" ], block=False), transitions={'spoken': 'RETURN_TARGET3'}) smach.StateMachine.add( 'RETURN_TARGET3', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=challenge_knowledge.target4), challenge_knowledge.target4_radius1), transitions={ 'arrived': 'SAY_TARGET3_RETURN_REACHED', 'unreachable': 'RESET_ED_RETURN_TARGET3', 'goal_not_defined': 'RESET_ED_RETURN_TARGET3' }) smach.StateMachine.add( 'SAY_TARGET3_RETURN_REACHED', states.Say(robot, [ "Reached target 3 again", "I have arrived at target 3 again", "I am now at target 3 again" ], block=True), transitions={'spoken': 'SAY_GOTO_EXIT'}) smach.StateMachine.add( 'RESET_ED_RETURN_TARGET3', states.ResetED(robot), transitions={'done': 'GOTO_RETURN_TARGET3_BACKUP'}) smach.StateMachine.add('GOTO_RETURN_TARGET3_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.target4), challenge_knowledge.target4_radius2), transitions={ 'arrived': 'SAY_TARGET3_RETURN_REACHED', 'unreachable': 'TIMEOUT3_RETURN', 'goal_not_defined': 'TIMEOUT3_RETURN' }) smach.StateMachine.add('TIMEOUT3_RETURN', checkTimeOut( robot, challenge_knowledge.time_out_seconds), transitions={ 'not_yet': 'RETURN_TARGET3', 'time_out': 'SAY_RETURN_TARGET3_FAILED' }) # Should we mention that we failed??? smach.StateMachine.add( 'SAY_RETURN_TARGET3_FAILED', states.Say(robot, [ "I am unable to reach target 3 again", "I cannot reach target 3 again", "Target 3 is unreachable" ], block=True), transitions={'spoken': 'SAY_GOTO_EXIT'}) ###################################################################################################################################################### # # TARGET EXIT # ###################################################################################################################################################### smach.StateMachine.add( 'SAY_GOTO_EXIT', states.Say(robot, [ "I will move to the exit now. See you guys later!", "I am done with this challenge. Going to the exit" ], block=False), transitions={'spoken': 'GO_TO_EXIT'}) # Amigo goes to the exit (waypoint stated in knowledge base) smach.StateMachine.add('GO_TO_EXIT', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit1), radius=0.6), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_EXIT', 'goal_not_defined': 'RESET_ED_EXIT' }) smach.StateMachine.add('RESET_ED_EXIT', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT_BACKUP'}) smach.StateMachine.add('GO_TO_EXIT_BACKUP', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit2), radius=0.6), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_EXIT2', 'goal_not_defined': 'RESET_ED_EXIT2' }) smach.StateMachine.add('RESET_ED_EXIT2', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT_BACKUP2'}) smach.StateMachine.add('GO_TO_EXIT_BACKUP2', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit3), radius=0.6), transitions={ 'arrived': 'GO_TO_EXIT_BACKUP3', 'unreachable': 'RESET_ED_EXIT3', 'goal_not_defined': 'RESET_ED_EXIT3' }) smach.StateMachine.add('RESET_ED_EXIT3', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT_BACKUP3'}) smach.StateMachine.add('GO_TO_EXIT_BACKUP3', states.NavigateToWaypoint( robot, EntityByIdDesignator( robot, id=challenge_knowledge.exit4), radius=0.6), transitions={ 'arrived': 'AT_END', 'unreachable': 'AT_END', 'goal_not_defined': 'AT_END' }) smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) analyse_designators(sm, "navigation") return sm
else: smach.StateMachine.add('NAVIGATE_TO_INSPECT', NavigateToObserve(robot, entityDes, radius=1.0), transitions={ 'unreachable': 'failed', 'goal_not_defined': 'failed', 'arrived': 'SEGMENT' }) smach.StateMachine.add('SEGMENT', SegmentObjects(robot, objectIDsDes.writeable, entityDes, searchArea), transitions={'done': 'done'}) if __name__ == "__main__": from robot_skills.amigo import Amigo from robot_smach_states.util.designators import EdEntityDesignator rospy.init_node('state_machine') robot = Amigo() sm = Inspect(robot=robot, entityDes=EdEntityDesignator(robot=robot, id="closet")) print sm.execute()
smach.StateMachine.add('CHECK', CheckEmpty(robot, seen_entities_des, entity_des, volume, volume_threshold), transitions={ 'empty': 'empty', 'partially_occupied': 'partially_occupied', 'occupied': 'occupied' }) if __name__ == "__main__": from robot_skills import get_robot_from_argv from robot_smach_states.util.designators import EdEntityDesignator from robocup_knowledge import knowledge_loader common = knowledge_loader.load_knowledge("common") rospy.init_node('inspect_test') robot = get_robot_from_argv(index=1) sm = Inspect(robot=robot, entityDes=EdEntityDesignator(robot=robot, id="display_cabinet"), navigation_area="in_front_of") print(sm.execute())
def __init__(self, robot, item_to_place, place_pose, arm, place_volume=None, update_supporting_entity=False): """ Drive the robot to be close to the designated place_pose and move the designated arm to place the designated item there :param robot: Robot to execute state with :param item_to_place: Designator that resolves to the entity to place. e.g EntityByIdDesignator :param place_pose: The place pose can be one of three things: 1: Designator that resolves to the pose to place at. E.g. an EmptySpotDesignator 2: EdEntityDesignator resolving to an object on which the robot should place something 3: A string identifying an object on which the robot should place something :param arm: Designator -> arm to place with, so Arm that holds entity_to_place, e.g. via ArmHoldingEntityDesignator :param place_volume (optional) string identifying the volume where to place the object, e.g., 'on_top_of', 'shelf3' :param update_supporting_entity (optional) bool to indicate whether the supporting entity should be updated. This can only be used if the supporting entity is supplied, case 2 or 3 mentioned under item_to_place """ smach.StateMachine.__init__(self, outcomes=['done', 'failed']) # Check types or designator resolve types assert (item_to_place.resolve_type == Entity or type(item_to_place) == Entity) assert (arm.resolve_type == PublicArm or type(arm) == PublicArm) #assert(place_volume.resolve_type == str or (type(place_volume) == str)) # parse place volume if place_volume is not None: if isinstance(place_volume, str): place_area = place_volume elif place_volume.resolve_type == str: place_area = place_volume.resolve() else: raise AssertionError("Cannot place in {}".format(place_volume)) # Case 3 if isinstance(place_pose, str): furniture_designator = EdEntityDesignator(robot=robot, id=place_pose) place_designator = EmptySpotDesignator( robot=robot, place_location_designator=furniture_designator, area=place_area) # Case 1 elif place_pose.resolve_type == FrameStamped or type( place_pose) == FrameStamped: furniture_designator = None place_designator = place_pose # Case 2 elif place_pose.resolve_type == Entity: furniture_designator = place_pose place_designator = EmptySpotDesignator( robot=robot, place_location_designator=furniture_designator, area=place_area) else: raise AssertionError("Cannot place on {}".format(place_pose)) locking_place_designator = LockingDesignator(place_designator) with self: if furniture_designator is not None: smach.StateMachine.add('INSPECT', Inspect(robot, furniture_designator, navigation_area="in_front_of"), transitions={ 'done': 'PREPARE_PLACE', 'failed': 'failed' }) smach.StateMachine.add('PREPARE_PLACE', PreparePlace(robot, arm), transitions={ 'succeeded': 'LOCK_DESIGNATOR', 'failed': 'failed' }) smach.StateMachine.add('LOCK_DESIGNATOR', LockDesignator(locking_place_designator), transitions={'locked': 'NAVIGATE_TO_PLACE'}) smach.StateMachine.add('NAVIGATE_TO_PLACE', NavigateToPlace(robot, locking_place_designator, arm), transitions={ 'unreachable': 'failed', 'goal_not_defined': 'failed', 'arrived': 'PUT' }) smach.StateMachine.add('PUT', Put(robot, item_to_place, locking_place_designator, arm), transitions={ 'succeeded': 'done', 'failed': 'failed' })
}) smach.StateMachine.add('PUT', Put(robot, item_to_place, locking_place_designator, arm), transitions={ 'succeeded': 'done', 'failed': 'failed' }) if __name__ == "__main__": from robot_skills import get_robot_from_argv from robot_smach_states.util.designators import EdEntityDesignator, ArmDesignator rospy.init_node('state_machine') robot = get_robot_from_argv(index=1) robot.ed.update_entity(id="bla") place_entity = EdEntityDesignator(robot, id="bla") arm = ArmDesignator(robot, {}) sm = Place(robot=robot, item_to_place=place_entity, place_pose='dinner_table', arm=arm, place_volume='on_top_of') print(sm.execute())
def __init__(self, robot, objectsIDsDesLocal): smach.StateMachine.__init__( self, outcomes=['container_success', 'container_failed']) with self: smach.StateMachine.add( 'SAY_STARTING_TEST', states.Say(robot, "Starting object pickup test test", block=False), transitions={'spoken': 'SAY_SEARCHING_OBJECTS'}) smach.StateMachine.add( 'SAY_SEARCHING_OBJECTS', states.Say( robot, "I'm going to the dinning table to search for objects", block=False), transitions={'spoken': 'NAV_TO_TABLE'}) smach.StateMachine.add( 'NAV_TO_TABLE', states.NavigateToSymbolic( robot, { EdEntityDesignator(robot, id=challenge_knowledge.INSPECT_ROOM_ID): "in" }, EdEntityDesignator( robot, id=challenge_knowledge.INSPECT_ENTITY_ID)), transitions={ 'arrived': 'SEGMENT_OBJECTS', 'unreachable': 'SAY_FAILED_NAV_TO_TABLE', 'goal_not_defined': 'SAY_FAILED_NAV_TO_TABLE' }) smach.StateMachine.add( 'SAY_FAILED_NAV_TO_TABLE', states.Say( robot, "I could not reach the table, but i will try to continue.", block=True), transitions={'spoken': 'SEGMENT_OBJECTS'}) smach.StateMachine.add( "SEGMENT_OBJECTS", states.SegmentObjects( robot, objectsIDsDesLocal.writeable, EdEntityDesignator( robot, id=challenge_knowledge.INSPECT_ENTITY_ID), "on_top_of"), transitions={'done': 'PICKUP_OBJECT'}) smach.StateMachine.add('PICKUP_OBJECT', test_states.PickUpRandomObj( robot, objectsIDsDesLocal), transitions={ 'succeeded': 'SAY_I_HAVE_OBJ', 'failed': 'SAY_PICKUP_FAILED', 'no_objects': 'SAY_NO_OBJECTS' }) smach.StateMachine.add('SAY_I_HAVE_OBJ', states.Say(robot, "I have the object!", block=False), transitions={'spoken': 'container_success'}) smach.StateMachine.add( 'SAY_PICKUP_FAILED', states.Say( robot, "Something went wrong and i could not pick up the object!", block=False), transitions={'spoken': 'container_failed'}) smach.StateMachine.add('SAY_NO_OBJECTS', states.Say( robot, "I don't see any objects to pick up!", block=False), transitions={'spoken': 'container_failed'})
def __init__(self, robot, source_location, source_navArea, target_location, target_navArea, target_placeArea="on_top_of", source_searchArea="on_top_of"): """ Let the given robot move to a location and remove all entities from that table one at a time :param robot: Robot to use :param source_location: Location which will be cleared :param target_location: Location where the objects will be placed :return: """ smach.StateMachine.__init__(self, outcomes=['done', 'failed']) # Check types or designator resolve types #check_type(source_location, Entity) #check_type(target_location, Entity) segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult]) selected_entity_designator = EntityByIdDesignator(robot, "TBD", name='selected_entity_designator', ) arm_des = UnoccupiedArmDesignator(robot, {}).lockable() arm_des.lock() place_position = states.util.designators.EmptySpotDesignator(robot, EdEntityDesignator( robot, id=target_location.id), area="on_top_of" ) with self: smach.StateMachine.add('INSPECT_SOURCE_ENTITY', states.Inspect(robot, source_location, objectIDsDes=segmented_entities_designator, searchArea=source_searchArea, navigation_area=source_navArea), transitions={'done': 'DETERMINE_IF_CLEAR', 'failed': 'failed'} ) #smach.StateMachine.add('DETERMINE_IF_CLEAR', # isitclear(robot=robot, # objectIDsDes=segmented_entities_designator), # transitions={'clear': 'done', # 'not_clear': 'failed'}) smach.StateMachine.add('DETERMINE_IF_CLEAR', SelectEntity(robot, segmented_entities_designator, selected_entity_designator), transitions={'no_entities_left': 'done', 'entity_selected': 'GRAB'} ) smach.StateMachine.add('GRAB', states.Grab(robot, selected_entity_designator, arm_des), transitions={'done': 'INSPECT_TARGET', 'failed': 'failed'} ) smach.StateMachine.add('INSPECT_TARGET', states.Inspect(robot, target_location, searchArea=target_placeArea, navigation_area=target_navArea), transitions={'done': 'PLACE', 'failed': 'failed'} ) smach.StateMachine.add('PLACE', states.Place(robot, selected_entity_designator, place_position, arm_des), transitions={'done': 'INSPECT_SOURCE_ENTITY', 'failed': 'failed'} )
def __init__(self, robot, location_id, room_id, navigate_area, segment_areas, known_types): smach.StateMachine.__init__(self, outcomes=['done']) # Set up the designators for this machine e_classifications_des = VariableDesignator( [], resolve_type=[ClassificationResult], name='e_classifications_des') e_des = EdEntityDesignator(robot, id=location_id, name='e_des') room_des = EntityByIdDesignator(robot, id=room_id, name='room_des') with self: # Loop over the areas that we have to segment and handle segmented objects for the specified navigation area for i, segment_area in enumerate(segment_areas): smach.StateMachine.add("RESET_ED_%d" % i, robot_smach_states.ResetED(robot), transitions={'done': 'NAVIGATE_%d' % i}) smach.StateMachine.add("NAVIGATE_%d" % i, robot_smach_states.NavigateToSymbolic( robot, { e_des: navigate_area, room_des: "in" }, e_des), transitions={ 'arrived': 'SEGMENT_SAY_%d' % i, 'unreachable': "SAY_UNREACHABLE_%d" % i, 'goal_not_defined': "SAY_UNREACHABLE_%d" % i }) smach.StateMachine.add( "SEGMENT_SAY_%d" % i, robot_smach_states.Say( robot, ["Looking %s the %s" % (segment_area, location_id)], block=False), transitions={"spoken": "SEGMENT_%d" % i}) smach.StateMachine.add( 'SEGMENT_%d' % i, robot_smach_states.SegmentObjects( robot, e_classifications_des.writeable, e_des, segment_area), transitions={'done': "HANDLE_DETECTED_ENTITIES_%d" % i}) # Determine the next state, either it is the next iter or done next_state = "RESET_ED_%d" % ( i + 1) if i + 1 < len(segment_areas) else "done" smach.StateMachine.add( "SAY_UNREACHABLE_%d" % i, robot_smach_states.Say( robot, ["I failed to inspect the %s" % location_id], block=True), transitions={"spoken": next_state}) smach.StateMachine.add("HANDLE_DETECTED_ENTITIES_%d" % i, HandleDetectedEntities( robot, e_classifications_des, known_types, location_id, segment_area), transitions={"done": next_state})
def __init__(self, robot, location_des): """ Visit all selected locations from the list, and handle the found objects :param location_des: is a designator resolving to a dictionary with fields ... TODO """ smach.StateMachine.__init__(self, outcomes=['done']) segment_areas = ValueByKeyDesignator(location_des, "segment_areas", [str], name='segment_areas') segment_area = VariableDesignator(resolve_type=str, name='segment_area') navigation_area_des = ValueByKeyDesignator(location_des, key='navigation_area', resolve_type=str, name='navigation_area') location_id_des = ValueByKeyDesignator(location_des, key='name', resolve_type=str, name='location_id') room_des = EdEntityDesignator(robot, id_designator=ValueByKeyDesignator( location_des, key="room", resolve_type=str)) # Set up the designators for this machine e_classifications_des = VariableDesignator( [], resolve_type=[ClassificationResult], name='e_classifications_des') e_des = EdEntityDesignator(robot, id_designator=location_id_des, name='e_des') with self: smach.StateMachine.add('ITERATE_NEXT_AREA', IterateDesignator(segment_areas, segment_area.writeable), transitions={ "next": 'INSPECT', "stop_iteration": "done" }) # Segment the area and handle segmented objects for the specified navigation area # for i, segment_area in enumerate(segment_areas): smach.StateMachine.add( 'INSPECT', NavigateToSymbolic(robot, {e_des: navigation_area_des}, e_des), transitions={ 'arrived': 'SEGMENT_SAY', 'unreachable': "SAY_UNREACHABLE", 'goal_not_defined': "SAY_UNREACHABLE" }) smach.StateMachine.add("SEGMENT_SAY", Say(robot, "Looking {area} the {entity}", area=segment_area, entity=location_id_des, block=False), transitions={"spoken": "RISE"}) smach.StateMachine.add('RISE', RiseForInspect(robot, e_des, segment_area), transitions={ 'succeeded': 'SEGMENT', 'failed': 'SEGMENT' }) smach.StateMachine.add( 'SEGMENT', SegmentObjects(robot, e_classifications_des.writeable, e_des, segment_area), transitions={'done': "HANDLE_DETECTED_ENTITIES"}) smach.StateMachine.add("SAY_UNREACHABLE", Say(robot, "I failed to inspect the {furn}", furn=location_id_des, block=True), transitions={"spoken": "done"}) smach.StateMachine.add("HANDLE_DETECTED_ENTITIES", HandleDetectedEntities( robot, e_classifications_des, location_id_des, segment_area, room_des), transitions={"done": "ITERATE_NEXT_AREA"})