def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) with self: # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # TEST CONTAINER # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # container for this stage testContainer = smach.StateMachine( outcomes=['container_success', 'container_failed']) with testContainer: smach.StateMachine.add( 'TEST_SPEACH', states.Say(robot, "Testing speach"), transitions={'spoken': 'container_success'}) # add container to state machine smach.StateMachine.add('TEST_CONTAINER', testContainer, transitions={ 'container_success': 'END_CHALLENGE', 'container_failed': 'END_CHALLENGE' }) smach.StateMachine.add('END_CHALLENGE', states.Say( robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'})
def __init__(self, robot, selected_entity_designator, location_id, segment_area): smach.StateMachine.__init__(self, outcomes=['done']) sentences = ["%s, please clean this object %s the %s" % (other_robot_name, segment_area, location_id), "%s, can you clean the trash %s the %s?" % (other_robot_name, segment_area, location_id), "Can another robot clean the garbage %s the %s?" % (segment_area, location_id)] with self: smach.StateMachine.add('SAY_OTHER_ROBOT_CLEANUP', robot_smach_states.Say(robot, sentences, block=False), transitions={"spoken": "NAVIGATE_TO_INITIAL"}) smach.StateMachine.add( "NAVIGATE_TO_INITIAL", robot_smach_states.NavigateToWaypoint(robot, EntityByIdDesignator(robot, id=challenge_knowledge.starting_point,), radius=0.05), transitions={ 'arrived' : 'CONTACT_OTHER_ROBOT', 'unreachable' : 'CONTACT_OTHER_ROBOT', 'goal_not_defined' : 'CONTACT_OTHER_ROBOT'}) smach.StateMachine.add('CONTACT_OTHER_ROBOT', ContactOtherRobot(robot, selected_entity_designator), transitions={"done": "WAIT_FOR_TRIGGER", "failed": "SAY_FAILED"}) smach.StateMachine.add('WAIT_FOR_TRIGGER', robot_smach_states.WaitForTrigger(robot, ["continue", "gpsr"], "/amigo/trigger"), transitions={"continue": "SAY_DONE", "gpsr": "SAY_FAILED", "preempted" : "SAY_FAILED"}) smach.StateMachine.add('SAY_DONE', robot_smach_states.Say(robot, ["Thanks for cleaning", "Thank you", "You are the best"], block=True), transitions={"spoken": "done"}) smach.StateMachine.add('SAY_FAILED', robot_smach_states.Say(robot, ["Too bad then, we will just leave that trash there"], block=True), transitions={"spoken": "done"})
def __init__(self, robot, selected_entity_designator, location_id, segment_area): smach.StateMachine.__init__(self, outcomes=['cleanup', 'no_cleanup']) sentences = ["Calling my operator. Can you help me with the object %s the %s" % (segment_area, location_id), "Contacting my Operator. Can you tell me more about the object %s the %s?" % (segment_area, location_id), "I am asking my operator now about the object %s the %s?" % (segment_area, location_id)] with self: smach.StateMachine.add('SAY_CONTACTING_OPERATOR', robot_smach_states.Say(robot, sentences, block=True), transitions={"spoken": "OPERATOR_FEEDBACK"}) smach.StateMachine.add('OPERATOR_FEEDBACK', OperatorFeedback(robot, selected_entity_designator), transitions={"cleanup": "SAY_CLEANUP", "no_cleanup": "SAY_NO_CLEANUP"}) smach.StateMachine.add('SAY_CLEANUP', robot_smach_states.Say(robot, ["Ok, I will cleanup the object", "That's ok!", "As you wish"], block=True), transitions={"spoken": "cleanup"}) smach.StateMachine.add('SAY_NO_CLEANUP', robot_smach_states.Say(robot, ["Ok, I will leave the object here", "That's ok! I will continue", "Now I know that this object is not trash"], block=True), transitions={"spoken": "no_cleanup"})
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 navigation test", block=False), transitions={'spoken': 'SAY_TEST_SYMBOLIC'}) smach.StateMachine.add('SAY_TEST_SYMBOLIC', states.Say(robot, "Testing Navigate To Symbolic", block=False), transitions={'spoken': 'NAV_TO_SYMB'}) smach.StateMachine.add( 'NAV_TO_SYMB', states.NavigateToSymbolic( robot, {EntityByIdDesignator(robot, id="living_room"): "in"}, EntityByIdDesignator(robot, id="dinnertable")), transitions={ 'arrived': 'SAY_TEST_WAYPOINT', 'unreachable': 'SAY_FAILED_SYMBOLIC', 'goal_not_defined': 'SAY_FAILED_SYMBOLIC' }) smach.StateMachine.add('SAY_FAILED_SYMBOLIC', states.Say(robot, "Failed Navigate To Symbolic.", block=True), transitions={'spoken': 'SAY_TEST_WAYPOINT'}) smach.StateMachine.add('SAY_TEST_WAYPOINT', states.Say(robot, "Testing Navigate To Waypoint.", block=False), transitions={'spoken': 'NAV_TO_WAYPOINT'}) smach.StateMachine.add( 'NAV_TO_WAYPOINT', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=challenge_knowledge.wp_test_nav)), transitions={ 'arrived': 'container_success', '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': 'container_success'})
def __init__(self, robot, selected_entity_designator, location_id, segment_area): smach.StateMachine.__init__(self, outcomes=['done','failed']) place_pose = dropPoseDesignator(robot, 0.6, "drop_pose") with self: smach.StateMachine.add("SPEAK", Speak(robot, selected_entity_designator, location_id, segment_area), transitions={"done": "GRAB"}) smach.StateMachine.add("GRAB", robot_smach_states.Grab(robot, selected_entity_designator, UnoccupiedArmDesignator(robot.arms, robot.rightArm, name="empty_arm_designator")), transitions={"done": "SAY_GRAB_SUCCESS", "failed": "SAY_GRAB_FAILED"}) smach.StateMachine.add('SAY_GRAB_SUCCESS', robot_smach_states.Say(robot, ["Now I am going to toss the item in the trashbin", "Let's clean up this object", "Away with this garbage", "Everything will be cleaned"], block=False), transitions={"spoken": "CHECK_ARM_FREE"}) smach.StateMachine.add('SAY_GRAB_FAILED', robot_smach_states.Say(robot, ["I could not grab the item.", "I failed to grasp the item", "I cannot reach the item", "Item grab failed"], block=False), transitions={"spoken": "failed"}) smach.StateMachine.add('CHECK_ARM_FREE', ArmFree(robot), transitions={"yes": "done", "no": "CHECK_ARM_OCCUPIED"}) smach.StateMachine.add('CHECK_ARM_OCCUPIED', ArmOccupied(robot), transitions={"yes": "PLACE", "no": "done"}) smach.StateMachine.add('PLACE', robot_smach_states.Place(robot, selected_entity_designator, place_pose, OccupiedArmDesignator(robot.arms, robot.rightArm, name="occupied_arm_designator")), transitions={"done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED"}) smach.StateMachine.add('SAY_PLACE_SUCCESS', robot_smach_states.Say(robot, ["Bye bye!", "Yeah!", "Successfully disposed the item", "Another score for AMIGO"], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"}) smach.StateMachine.add('SAY_PLACE_FAILED', robot_smach_states.Say(robot, ["I could not cleanup the item.", "I cannot put the item in the trashbin", "Item cleanup failed"], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"})
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): smach.StateMachine.__init__( self, outcomes=['container_success', 'container_failed']) with self: smach.StateMachine.add( "SAY_WAITING_OPERATOR", states.Say(robot, [ "I'm waiting for the operator to stand in front of me.", "Would the operator please come forward.", "I need an operator, please stand in front of me." ], block=False), transitions={'spoken': 'LOOK_AT_OPERATOR'}) smach.StateMachine.add('LOOK_AT_OPERATOR', states_interaction.LookAtPersonInFront( robot, lookDown=False), transitions={ 'succeeded': 'WAIT_FOR_OPERATOR', 'failed': 'WAIT_FOR_OPERATOR' }) smach.StateMachine.add("WAIT_FOR_OPERATOR", states_interaction.WaitForPersonDetection( robot, attempts=5, sleep_interval=1), transitions={ 'succeeded': 'container_success', 'failed': 'SAY_FAILED_WAITING' }) smach.StateMachine.add( 'SAY_FAILED_WAITING', states.Say(robot, "I don't see anyone.", block=True), transitions={'spoken': 'WAIT_FOR_OPERATOR_2'}) smach.StateMachine.add("WAIT_FOR_OPERATOR_2", states_interaction.WaitForPersonDetection( robot, attempts=5, sleep_interval=1), transitions={ 'succeeded': 'container_success', 'failed': 'container_success' }) smach.StateMachine.add( 'SAY_FAILED_WAITING_AGAIN', states.Say( robot, "I still couldn't find anyone in front of me. I will continue my task.", block=True), transitions={'spoken': 'WAIT_FOR_OPERATOR_2'})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) self.robot = robot @smach.cb_interface(outcomes=['done']) def look_at_standing_person(userdata=None): robot.head.look_at_standing_person() return 'done' with self: smach.StateMachine.add('LOOK_AT_OPERATOR', smach.CBState(look_at_standing_person), transitions={'done': 'SAY_LOOK_AT_ME'}) smach.StateMachine.add( 'SAY_LOOK_AT_ME', states.Say( robot, "Please stand one meter in front of me and look at me while I \ learn to recognize your face.", block=True), transitions={'spoken': 'LEARN_PERSON'}) smach.StateMachine.add( 'LEARN_PERSON', states.LearnPerson( robot, name_designator=ds.VariableDesignator("operator")), transitions={ 'succeeded_learning': 'SAY_OPERATOR_LEARNED', 'failed_learning': 'SAY_FAILED_LEARNING', 'timeout_learning': 'SAY_FAILED_LEARNING' }) smach.StateMachine.add( 'SAY_FAILED_LEARNING', states.Say( robot, "I could not learn my operator's face. Let me try again.", block=True), transitions={'spoken': 'failed'}) smach.StateMachine.add( 'SAY_OPERATOR_LEARNED', states.Say( robot, "Now i know what you look like. Please go mix with the crowd." ), transitions={'spoken': 'succeeded'})
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['done']) with sm: smach.StateMachine.add('WAYPOINT_SAY', states.Say(robot, "Say waypoint"), transitions={ 'spoken' :'WAYPOINT_ASK'}) smach.StateMachine.add('WAYPOINT_ASK', StoreWaypoint(robot), transitions={ 'done' :'TABLE_SAY', 'continue' : 'TABLE_SAY'}) smach.StateMachine.add('TABLE_SAY', states.Say(robot, "Say table"), transitions={ 'spoken' :'TABLE_ASK'}) smach.StateMachine.add('TABLE_ASK', HearWhichTable(robot), transitions={ 'one' :'ORDER_SAY', 'two' :'ORDER_SAY', 'three' :'ORDER_SAY', 'no_result' :'ORDER_SAY',}) smach.StateMachine.add('ORDER_SAY', states.Say(robot, "Say order"), transitions={ 'spoken' :'ORDER_ASK'}) smach.StateMachine.add('ORDER_ASK', AskOrder(robot, "one"), transitions={ 'next_order' :'WAYPOINT_SAY', 'orders_done' : 'WAYPOINT_SAY'}) return sm
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted', 'Failed']) with self: smach.StateMachine.add("SAY_HI", states.Say(robot, "Hi "), transitions={"spoken": "Done"})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted']) runs = ds.Designator([0, 1]) run = ds.VariableDesignator(resolve_type=int) with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SET_INITIAL_POSE', 'abort': 'aborted' }) smach.StateMachine.add('SET_INITIAL_POSE', states.SetInitialPose( robot, challenge_knowledge.starting_point), transitions={ 'done': 'HANDLE_GUEST_1', "preempted": 'aborted', 'error': 'HANDLE_GUEST_1' }) smach.StateMachine.add('HANDLE_GUEST_1', HandleSingleGuest(robot, assume_john=True), transitions={ 'succeeded': 'HANDLE_GUEST_2', 'aborted': 'HANDLE_GUEST_2' }) smach.StateMachine.add('HANDLE_GUEST_2', HandleSingleGuest(robot, assume_john=False), transitions={ 'succeeded': 'SAY_DONE', 'aborted': 'SAY_DONE' }) smach.StateMachine.add( 'SAY_DONE', states.Say(robot, ["That's all folks, my job is done, bye bye!"], block=False), transitions={'spoken': 'GO_BACK'}) smach.StateMachine.add( 'GO_BACK', states.NavigateToWaypoint( robot, ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_door['id']), challenge_knowledge.waypoint_door['radius']), transitions={ 'arrived': 'succeeded', 'unreachable': 'succeeded', 'goal_not_defined': 'succeeded' })
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, grab_designator=None): """ Constructor :param robot: robot object :param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is constructed (grabs the closest object in the volume of the surface) """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms, robot.leftArm, name="empty_arm_designator") self.grab_designator = ds.LockToId(robot=robot, to_be_locked=grab_designator) with self: @smach.cb_interface(outcomes=["locked"]) def lock(userdata=None): """ 'Locks' a locking designator """ # This determines that self.current_item cannot not resolve to a new value until it is unlocked again. self.grab_designator.lock() if self.grab_designator.resolve(): rospy.loginfo("Current_item is now locked to {0}".format(self.grab_designator.resolve().id)) return "locked" smach.StateMachine.add("LOCK_ITEM", smach.CBState(lock), transitions={'locked': 'ANNOUNCE_ITEM'}) smach.StateMachine.add("ANNOUNCE_ITEM", states.Say(robot, EntityDescriptionDesignator(self.grab_designator, name="current_item_desc"), block=False), transitions={'spoken': 'GRAB_ITEM'}) smach.StateMachine.add("GRAB_ITEM", states.Grab(robot, self.grab_designator, self.empty_arm_designator), transitions={'done': 'UNLOCK_ITEM_SUCCEED', 'failed': 'UNLOCK_ITEM_FAIL'}) @smach.cb_interface(outcomes=["unlocked"]) def lock(userdata=None): """ 'Locks' a locking designator """ # This determines that self.current_item cannot not resolve to a new value until it is unlocked again. self.grab_designator.unlock() return "unlocked" smach.StateMachine.add("UNLOCK_ITEM_SUCCEED", smach.CBState(lock), transitions={'unlocked': 'succeeded'}) smach.StateMachine.add("UNLOCK_ITEM_FAIL", smach.CBState(lock), transitions={'unlocked': 'failed'})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=["Done"]) self.robot = robot with self: # smach.StateMachine.add("LOOK_AT_PERSON", # LookAtPerson(robot), # transitions={ 'Done' :'SAY_HI', # 'Aborted' :'SAY_HI', # 'Failed' :'SAY_HI'}) smach.StateMachine.add("SAY_HI", states.Say(robot, [ "Howdy, I got a joke for you", "Hi there, I have a joke for you" ]), transitions={"spoken": "MAKE_JOKES"}) smach.StateMachine.add("MAKE_JOKES", states.Say(robot, jokes), transitions={"spoken": "Done"})
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'})
def __init__(self, robot, personNameDesLocal): smach.StateMachine.__init__( self, outcomes=['container_success', 'container_failed']) with self: smach.StateMachine.add( 'SAY_LOOK_AT_ME', states.Say( robot, "Please stand in front of me and look at my camera while I learn your face.", block=False), transitions={'spoken': 'LOOK_AT_OPERATOR'}) smach.StateMachine.add( 'LOOK_AT_OPERATOR', states_interaction.LookAtPersonInFront(robot, lookDown=True), transitions={ 'succeeded': 'LEARN_PERSON', # 'failed':'SAY_LEARN_FACE_FAILED'}) 'failed': 'LEARN_PERSON' }) smach.StateMachine.add( 'LEARN_PERSON', states.LearnPerson(robot, name_designator=personNameDesLocal), transitions={ 'succeeded_learning': 'container_success', 'failed_learning': 'SAY_LEARN_FACE_FAILED', 'timeout_learning': 'SAY_LEARN_FACE_FAILED' }) smach.StateMachine.add('SAY_LEARN_FACE_FAILED', states.Say(robot, "I could not learn your face.", block=False), transitions={'spoken': 'container_failed'})
def __init__(self, robot, arm_designator, grabbed_entity_label="", grabbed_entity_designator=None, timeout=15, arm_configuration="handover_to_human"): """ Hold up hand to accept an object and close hand once something is inserted :param robot: Robot with which to execute this behavior :param arm_designator: ArmDesignator resolving to arm accept item into :param grabbed_entity_label: What ID to give a dummy item in case no grabbed_entity_designator is supplied :param grabbed_entity_designator: EntityDesignator resolving to the accepted item. Can be a dummy :param timeout: How long to hold hand over before closing without anything :param arm_configuration: Which pose to put arm in when holding hand up for the item. """ smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed', 'timeout']) ds.check_type(arm_designator, PublicArm) if not grabbed_entity_designator and grabbed_entity_label == "": rospy.logerr("No grabbed entity label or grabbed entity designator given") with self: smach.StateMachine.add("POSE", manipulation.ArmToJointConfig(robot, arm_designator, arm_configuration), transitions={'succeeded': 'OPEN_BEFORE_INSERT', 'failed': 'OPEN_BEFORE_INSERT'}) smach.StateMachine.add('OPEN_BEFORE_INSERT', manipulation.SetGripper(robot=robot, arm_designator=arm_designator, gripperstate=manipulation.GripperState. OPEN), transitions={'succeeded': 'SAY1', 'failed': 'SAY1'}) smach.StateMachine.add("SAY1", states.Say(robot, 'Please hand over the trash by putting the top of the bag' ' between my grippers and push firmly into my camera as' ' will be shown on my screen.'), transitions={'spoken': 'SHOW_IMAGE'}) smach.StateMachine.add("SHOW_IMAGE", states.ShowImageState( robot=robot, image_filename="~/ros/kinetic/system/src/challenge_take_out_the_garbage/src" "/challenge_take_out_the_garbage/beun_picture.png", seconds=5), transitions={'succeeded': 'CLOSE_AFTER_INSERT'}) smach.StateMachine.add('CLOSE_AFTER_INSERT', manipulation.CloseGripperOnHandoverToRobot( robot, arm_designator, grabbed_entity_label=grabbed_entity_label, grabbed_entity_designator=grabbed_entity_designator, timeout=timeout), transitions={'succeeded': 'succeeded', 'timeout': 'failed', 'failed': 'failed'})
def __init__(self, robot, category_grammar, categoryw): smach.StateMachine.__init__(self, outcomes=["done"]) hmi_result_des = ds.VariableDesignator(resolve_type=hmi.HMIResult, name="hmi_result_des") category_des = ds.FuncDesignator(ds.AttrDesignator(hmi_result_des, "semantics", resolve_type=unicode), str, resolve_type=str) @smach.cb_interface(outcomes=['done']) def write_category(ud, des_read, des_write): # type: (object, ds.Designator, ds.Designator) -> str assert(ds.is_writeable(des_write)) assert(des_write.resolve_type == des_read.resolve_type) des_write.write(des_read.resolve()) return 'done' with self: smach.StateMachine.add("ASK_WHERE_TO_DROP", robot_smach_states.Say(robot, "Please look at the object in my gripper and tell me which category it is. If it should be thrown away," "call it trash", block=True), transitions={"spoken": "HEAR_LOCATION"}) smach.StateMachine.add("HEAR_LOCATION", robot_smach_states.HearOptionsExtra(robot, category_grammar, ds.writeable(hmi_result_des)), transitions={"heard": "SAY_HEARD_CORRECT", "no_result": "ASK_WHERE_TO_DROP"}) smach.StateMachine.add("SAY_HEARD_CORRECT", robot_smach_states.Say( robot, "I understood that the object is of category {category}, is this correct?", category=category_des, block=True), transitions={"spoken": "HEAR_CORRECT"}) smach.StateMachine.add("HEAR_CORRECT", robot_smach_states.AskYesNo(robot), transitions={"yes": "WRITE_CATEGORY", "no": "ASK_WHERE_TO_DROP", "no_result": "ASK_WHERE_TO_DROP"}) smach.StateMachine.add('WRITE_CATEGORY', smach.CBState(write_category, cb_args=[category_des, categoryw]), transitions={'done': 'done'})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=["Done", "Failed", "Aborted"]) self.robot = robot self.query_detect_face = Conjunction( Compound("property_expected", "ObjectID", "class_label", "face"), Compound("property_expected", "ObjectID", "position", Compound("in_front_of", "amigo")), Compound("property_expected", "ObjectID", "position", Sequence("X", "Y", "Z"))) with self: #TODO: Make sure the arms are straight down and the head is staring away into the distance smach.StateMachine.add("RESET_RIGHT", states.ArmToJointPos( robot, robot.rightArm, [0, 0, 0, 0, 0, 0, 0]), transitions={ "done": "RESET_LEFT", "failed": "RESET_LEFT" }) smach.StateMachine.add("RESET_LEFT", states.ArmToJointPos( robot, robot.leftArm, [0, 0, 0, 0, 0, 0, 0]), transitions={ "done": "WAIT_FOR_PERSON", "failed": "WAIT_FOR_PERSON" }) smach.StateMachine.add("WAIT_FOR_PERSON", states.Wait_queried_perception( self.robot, ["face_recognition"], self.query_detect_face, timeout=60), transitions={ "query_true": "TALK", "timed_out": "Failed", "preempted": "Aborted" }) smach.StateMachine.add("TALK", states.Say(robot, ["Boooo!"]), transitions={'spoken': 'Done'})
def setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) with sm: # Start challenge via StartChallengeRobust smach.StateMachine.add("START_CHALLENGE_ROBUST", states.Initialize(robot), transitions={ 'initialized': "SAY_1", "abort": "Aborted" }) smach.StateMachine.add('SAY_1', states.Say(robot, "Please ask me question one"), transitions={'spoken': 'QUESTION_1'}) smach.StateMachine.add('QUESTION_1', HearQuestion(robot), transitions={'answered': 'SAY_2'}) smach.StateMachine.add('SAY_2', states.Say(robot, "Please ask me question two"), transitions={'spoken': 'QUESTION_2'}) smach.StateMachine.add('QUESTION_2', HearQuestion(robot), transitions={'answered': 'SAY_3'}) smach.StateMachine.add('SAY_3', states.Say(robot, "Please ask me question three"), transitions={'spoken': 'QUESTION_3'}) smach.StateMachine.add('QUESTION_3', HearQuestion(robot), transitions={'answered': 'SAY_4'}) smach.StateMachine.add('SAY_4', states.Say(robot, "Please ask me question four"), transitions={'spoken': 'QUESTION_4'}) smach.StateMachine.add('QUESTION_4', HearQuestion(robot), transitions={'answered': 'SAY_5'}) smach.StateMachine.add('SAY_5', states.Say(robot, "Please ask me question five"), transitions={'spoken': 'QUESTION_5'}) smach.StateMachine.add('QUESTION_5', HearQuestion(robot), transitions={'answered': 'AT_END'}) smach.StateMachine.add('AT_END', states.Say(robot, "That was all folks!"), transitions={'spoken': 'Done'}) return sm
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) self.turned = False with self: smach.StateMachine.add('SAY_SEARCHING_CROWD', states.Say(robot, "I'm looking for the crowd.", block=False), transitions={'spoken': 'RECOGNIZE_PERSONS'}) smach.StateMachine.add('RECOGNIZE_PERSONS', RecognizePersons(robot), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' })
def __init__(self, robot, drop_zone_id): """ :param robot: robot object :param drop_designator: EdEntityDesignator designating the collection zone """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) arm_designator = ds.OccupiedArmDesignator(robot=robot, arm_properties={}) with self: smach.StateMachine.add("GO_TO_COLLECTION_ZONE", states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id), radius=0.5), transitions={"arrived": "DROP_TRASH", "goal_not_defined": "aborted", "unreachable": "OPEN_DOOR_PLEASE"}) smach.StateMachine.add("OPEN_DOOR_PLEASE", states.Say(robot, "Can you please open the door for me? It seems blocked!"), transitions={"spoken": "WAIT_FOR_DOOR_OPEN"}) smach.StateMachine.add("WAIT_FOR_DOOR_OPEN", states.WaitTime(robot=robot, waittime=5), transitions={"waited": "GO_TO_COLLECTION_ZONE2", "preempted": "GO_TO_COLLECTION_ZONE2"}) smach.StateMachine.add("GO_TO_COLLECTION_ZONE2", states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=drop_zone_id), radius=0.5), transitions={"arrived": "DROP_TRASH", "goal_not_defined": "aborted", "unreachable": "failed"}) smach.StateMachine.add("DROP_TRASH", DropTrash(robot=robot, arm_designator=arm_designator), transitions={"succeeded": "succeeded", "failed": "HANDOVER"}) smach.StateMachine.add("HANDOVER", states.HandoverToHuman(robot=robot, arm_designator=arm_designator), transitions={"succeeded": "succeeded", "failed": "failed"})
def __init__(self, robot, object_category_des, room_des): smach.StateMachine.__init__(self, outcomes=["done", "failed"]) room_id_des = ds.AttrDesignator(room_des, "id", resolve_type=str) with self: smach.StateMachine.add("LOOK_INTO_ROOM", robot_smach_states.NavigateToRoom(robot, room_des, room_des), transitions={"arrived": "SAY_COME_TO_ME", "unreachable": "SAY_COME_TO_ME", "goal_not_defined": "SAY_COME_TO_ME"}) smach.StateMachine.add("SAY_COME_TO_ME", robot_smach_states.Say(robot, "Operator, please come to me in the {room}", room=room_id_des, block=True), transitions={"spoken": "WAIT_FOR_OPERATOR"}) smach.StateMachine.add("WAIT_FOR_OPERATOR", robot_smach_states.WaitTime(4), transitions={"waited": "ASK_WHICH_CATERGORY", "preempted": "ASK_WHICH_CATERGORY"}) smach.StateMachine.add("ASK_WHICH_CATERGORY", AskWhichCategory(robot, ds.Designator(challenge_knowledge.category_grammar), object_category_des), transitions={"done": "done"})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) self.target_destination = ds.EntityByIdDesignator( robot, id=challenge_knowledge.default_place) self.car_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_car['id']) self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator( robot, self.target_destination, name="placement", area=challenge_knowledge.default_area), name="place_position") self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, {}, name="empty_arm_designator") # With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked. # For this challenge, unlocking is not needed. self.bag_arm_designator = self.empty_arm_designator.lockable() self.bag_arm_designator.lock() # We don't actually grab something, so there is no need for an actual thing to grab self.current_item = ds.VariableDesignator(Entity( "dummy", "dummy", "/{}/base_link".format(robot.robot_name), kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [], datetime.datetime.now()), name="current_item") with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SET_INITIAL_POSE', 'abort': 'Aborted' }) smach.StateMachine.add('SET_INITIAL_POSE', states.SetInitialPose( robot, challenge_knowledge.starting_point), transitions={ 'done': 'FOLLOW_OPERATOR', 'preempted': 'Aborted', 'error': 'FOLLOW_OPERATOR' }) # Follow the operator until (s)he states that you have arrived at the "car". # smach.StateMachine.add('FOLLOW_OPERATOR', # states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True), # transitions={'stopped': 'ASK_FOR_TASK', # 'lost_operator': 'ASK_FOR_TASK', # 'no_operator': 'FOLLOW_OPERATOR'}) # Use NEW: smach.StateMachine.add('FOLLOW_OPERATOR', states.FollowOperator2(robot), transitions={ 'Done': 'ASK_FOR_TASK', 'Failed': 'ASK_FOR_TASK', 'Aborted': 'FOLLOW_OPERATOR' }) smach.StateMachine.add('ASK_FOR_TASK', states.Say(robot, ["Are we at the car already?"], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_TASK'}) smach.StateMachine.add('WAIT_FOR_TASK', states.HearOptions(robot, ['yes', 'no']), transitions={ 'no': 'FOLLOW_OPERATOR', 'yes': 'CONFIRM_CAR_LOCATION', 'no_result': 'ASK_FOR_TASK' }) smach.StateMachine.add( 'CONFIRM_CAR_LOCATION', states.Say( robot, ["OK, I will remember this location as the car location."], block=True, look_at_standing_person=True), transitions={'spoken': 'REMEMBER_CAR_LOCATION'}) smach.StateMachine.add('REMEMBER_CAR_LOCATION', hmc_states.StoreCarWaypoint(robot), transitions={ 'success': 'ASK_FOR_DESTINATION', 'abort': 'Aborted' }) smach.StateMachine.add( 'ASK_FOR_DESTINATION', states.Say(robot, ["Where should I bring the groceries?"], block=True, look_at_standing_person=True), transitions={'spoken': 'RECEIVE_DESTINATION'}) smach.StateMachine.add( 'RECEIVE_DESTINATION', hmc_states.WaitForOperatorCommand( robot, possible_commands=challenge_knowledge.destinations, commands_as_userdata=True, target=self.target_destination), transitions={ 'success': 'GRAB_ITEM', 'abort': 'Aborted' }) # # smach.StateMachine.add('CONFIRM_DESTINATION', # states.Say(robot, [ # "I will deliver the groceries to the %s" % ds.EntityByIdDesignator(self.target_destination)], # block=True, # look_at_standing_person=True), # transitions={'spoken': 'GRAB_ITEM'}) # Grab the item (bag) the operator hands to the robot, when they are at the "car". smach.StateMachine.add( 'GRAB_ITEM', # states.HandoverFromHuman(robot, self.bag_arm_designator, "current_item", # self.current_item, # arm_configuration=challenge_knowledge.carrying_bag_pose), # transitions={'succeeded': 'ARM_DRIVING_POSE', # 'timeout': 'BACKUP_CLOSE_GRIPPER', # # For now in simulation timeout is considered a success. # 'failed': 'BACKUP_CLOSE_GRIPPER'}) states.Say(robot, [ "I can't pick up the groceries since I don't have arms. Please place them in my basket." ], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_GRAB_ITEM'}) smach.StateMachine.add('WAIT_FOR_GRAB_ITEM', states.WaitTime(robot), transitions={ 'waited': 'SAY_GOING_TO_ROOM', 'preempted': 'Aborted' }) # smach.StateMachine.add('BACKUP_CLOSE_GRIPPER', # states.SetGripper(robot, self.bag_arm_designator, gripperstate=GripperState.CLOSE), # transitions={'succeeded': 'ARM_DRIVING_POSE', # 'failed': 'ARM_DRIVING_POSE'}) # # smach.StateMachine.add('ARM_DRIVING_POSE', # states.ArmToJointConfig(robot, self.bag_arm_designator, # challenge_knowledge.driving_bag_pose), # transitions={'succeeded': 'SAY_GOING_TO_ROOM', # 'failed': 'SAY_GOING_TO_ROOM'}) smach.StateMachine.add( 'SAY_GOING_TO_ROOM', states.Say(robot, [ "Let me bring in your groceries", "Helping you carry stuff", "I'm going back inside" ], block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_DESTINATION'}) smach.StateMachine.add( 'GOTO_DESTINATION', states.NavigateToSymbolic( robot, {self.target_destination: "in_front_of"}, self.target_destination), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'TURN_180_TO_REPLAN', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'TURN_180_TO_REPLAN', hmc_states.TurnToReplan(robot), transitions={ 'success': 'GOTO_DESTINATION_BACKUP', 'abort': 'GOTO_DESTINATION_BACKUP', # implement avoid obstacle behaviour later #'goal_not_defined': 'Aborted'}) }) smach.StateMachine.add( 'GOTO_DESTINATION_BACKUP', states.NavigateToSymbolic( robot, {self.target_destination: "in_front_of"}, self.target_destination), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) # Put the item (bag) down when the robot has arrived at the "drop-off" location (house). smach.StateMachine.add( 'PUTDOWN_ITEM', # hmc_states.DropBagOnGround(robot, self.bag_arm_designator, # challenge_knowledge.drop_bag_pose), states.Say(robot, [ "I can't put the groceries down since I have no arms. Please take them from my basket and put it down." ], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_PUTDOWN_ITEM'}) smach.StateMachine.add('WAIT_FOR_PUTDOWN_ITEM', states.WaitTime(robot), transitions={ 'waited': 'ASKING_FOR_HELP', 'preempted': 'Aborted' }) smach.StateMachine.add( 'ASKING_FOR_HELP', # TODO: look and then face new operator states.Say( robot, "Please follow me and help me carry groceries into the house", block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_CAR'}) #'LEARN_OPERATOR'}) # smach.StateMachine.add('LEARN_OPERATOR', # hmc_states.LearnOperator(robot), # transitions={'learned': 'GOTO_CAR', # 'failed': 'GOTO_CAR'}) smach.StateMachine.add( 'GOTO_CAR', states.NavigateToWaypoint( robot, self.car_waypoint, challenge_knowledge.waypoint_car['radius']), # TODO: detect closed door transitions={ 'unreachable': 'OPEN_DOOR', 'arrived': 'AT_END', 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'OPEN_DOOR', # TODO: implement functionality states.Say(robot, "Please open the door for me"), transitions={'spoken': 'GOTO_CAR'}) smach.StateMachine.add( 'AT_END', states.Say(robot, [ "We arrived at the car, goodbye", "You have reached your destination, goodbye", "The car is right here, see you later!" ], block=True, look_at_standing_person=True), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "help_me_carry")
def __init__(self, robot, operator_name, drink_designator, available_drinks_designator, unavailable_drink_designator, objects): # type (Robot, VariableDesignator) -> None """ Initialization method :param robot: robot api object :param operator_name: (EntityDesignator) in which the operator's name is stored :param drink_designator: (VariableDesignator) in which the drink to fetch is stored :param available_drinks_designator: (VariableDesignator) in which the available drinks are stored :param unavailable_drink_designator: (VariableDesignator) in which the unavailable drink is stored :param objects: Objects from common knowledge """ smach.StateMachine.__init__( self, outcomes=["succeeded", "failed", "aborted"]) with self: # Ask for order smach.StateMachine.add("RISE_FOR_HMI", states.RiseForHMI(robot=robot), transitions={ "succeeded": "ASK_FOR_ORDER", "failed": "ASK_FOR_ORDER" }) smach.StateMachine.add( "ASK_FOR_ORDER", states.Say(robot=robot, sentence=random.choice([ "What would you like to drink?", "Which drink would you like?", "What would you like to quench your thirst?", "What can I get you?", "Please tell me which drink you want" ]), look_at_standing_person=True), transitions={"spoken": "CHECK_AVAILABILITY"}) # Check availability smach.StateMachine.add( "CHECK_AVAILABILITY", CheckAvailability( robot=robot, drink_designator=drink_designator, available_drinks_designator=available_drinks_designator, unavailable_drink_designator=unavailable_drink_designator, objects=objects), transitions={ "available": "ASK_FOR_CONFIRMATION", "unavailable": "STATE_UNAVAILABLE", "aborted": "aborted", "bad_operator": "SAY_BAD_OPERATOR" }) # Ask for confirmation smach.StateMachine.add( "ASK_FOR_CONFIRMATION", states.Say(robot=robot, sentence=DescriptionStrDesignator( "confirmation_available", drink_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "HEAR_CONFIRMATION"}) # Hear the confirmation smach.StateMachine.add("HEAR_CONFIRMATION", states.HearOptions(robot=robot, options=["yes", "no"]), transitions={ "yes": "succeeded", "no": "ASK_FOR_ORDER", "no_result": "ASK_FOR_ORDER" }) # ToDo: fallback? # Announce that the drink is unavailable smach.StateMachine.add( "STATE_UNAVAILABLE", states.Say(robot=robot, sentence=DescriptionStrDesignator( "state_unavailable", unavailable_drink_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "ASK_FOR_ORDER"}) # Tell the operator to stop f*****g around! smach.StateMachine.add( "SAY_BAD_OPERATOR", states.Say( robot=robot, sentence= "I'm not going to ask you again as I have already informed you multiple times that your request is unavailable", look_at_standing_person=True), transitions={"spoken": "aborted"})
def __init__(self, robot): """ Initialization method :param robot: robot api object """ smach.StateMachine.__init__( self, outcomes=["succeeded", "failed", "aborted"]) # Create designators trashbin_designator = ds.EdEntityDesignator( robot=robot, id=CHALLENGE_KNOWLEDGE.trashbin_id, name='trashbin_designator') # Look if there is a second trash bin present # trashbin_designator2 = None if hasattr(CHALLENGE_KNOWLEDGE, "trashbin_id2"): trashbin_designator2 = ds.EdEntityDesignator( robot=robot, id=CHALLENGE_KNOWLEDGE.trashbin_id2, name='trashbin_designator2') next_state = "HELPER_WAYPOINT" rospy.loginfo("There is a second trash bin") else: rospy.loginfo("There is no second trash bin") next_state = "ANNOUNCE_END" # drop_zone_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.drop_zone_id) helper_waypoint_designator = ds.EdEntityDesignator( robot=robot, id=CHALLENGE_KNOWLEDGE.helper_waypoint) end_waypoint_designator = ds.EdEntityDesignator( robot=robot, id=CHALLENGE_KNOWLEDGE.end_waypoint) arm_designator = self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, {}, name="empty_arm_designator") with self: smach.StateMachine.add("START_CHALLENGE_ROBUST", states.StartChallengeRobust( robot, CHALLENGE_KNOWLEDGE.starting_point), transitions={ "Done": "SAY_START_CHALLENGE", "Aborted": "SAY_START_CHALLENGE", "Failed": "SAY_START_CHALLENGE" }) smach.StateMachine.add("SAY_START_CHALLENGE", states.Say( robot, "I will start cleaning up the trash", block=True), transitions={'spoken': "PICK_UP_TRASH"}) smach.StateMachine.add("PICK_UP_TRASH", PickUpTrash( robot=robot, trashbin_designator=trashbin_designator, arm_designator=arm_designator), transitions={ "succeeded": "DROP_DOWN_TRASH", "failed": "HELPER_WAYPOINT", "aborted": "ANNOUNCE_END" }) smach.StateMachine.add( "DROP_DOWN_TRASH", DropDownTrash(robot=robot, drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id), transitions={ "succeeded": "ANNOUNCE_TASK", "failed": "failed", "aborted": "aborted" }) smach.StateMachine.add( "ANNOUNCE_TASK", states.Say(robot, "First bag has been dropped at the collection zone", block=False), transitions={'spoken': next_state}) if next_state == "HELPER_WAYPOINT": smach.StateMachine.add( "HELPER_WAYPOINT", states.NavigateToWaypoint( robot=robot, waypoint_designator=helper_waypoint_designator), transitions={ "arrived": "PICK_UP_TRASH2", "goal_not_defined": "PICK_UP_TRASH2", "unreachable": "PICK_UP_TRASH2" }) smach.StateMachine.add( "PICK_UP_TRASH2", PickUpTrash(robot=robot, trashbin_designator=trashbin_designator2, arm_designator=arm_designator), transitions={ "succeeded": "DROP_DOWN_TRASH2", "failed": "ANNOUNCE_END", "aborted": "ANNOUNCE_END" }) smach.StateMachine.add( "DROP_DOWN_TRASH2", DropDownTrash( robot=robot, drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id), transitions={ "succeeded": "ANNOUNCE_TASK2", "failed": "failed", "aborted": "aborted" }) smach.StateMachine.add( "ANNOUNCE_TASK2", states.Say( robot, "Second bag has been dropped at the collection zone." "All the thrash has been taken care of", block=False), transitions={'spoken': 'ANNOUNCE_END'}) smach.StateMachine.add("ANNOUNCE_END", states.Say( robot, "I have finished taking out the trash.", block=False), transitions={'spoken': 'NAVIGATE_OUT'}) smach.StateMachine.add( "NAVIGATE_OUT", states.NavigateToWaypoint( robot=robot, waypoint_designator=end_waypoint_designator), transitions={ "arrived": "succeeded", "goal_not_defined": "succeeded", "unreachable": "succeeded" })
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, STARTING_POINT), transitions={ "Done": "GO_TO_INTERMEDIATE_WAYPOINT", "Aborted": "GO_TO_INTERMEDIATE_WAYPOINT", "Failed": "GO_TO_INTERMEDIATE_WAYPOINT" }) # There is no transition to Failed in StartChallengeRobust (28 May) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_1), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1', 'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1' }) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP1', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_2), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2', 'goal_not_defined': 'GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2' }) smach.StateMachine.add('GO_TO_INTERMEDIATE_WAYPOINT_BACKUP2', states.NavigateToWaypoint( robot, EntityByIdDesignator(robot, id=INTERMEDIATE_3), radius=0.5), transitions={ 'arrived': 'ASK_CONTINUE', 'unreachable': 'ASK_CONTINUE', 'goal_not_defined': 'ASK_CONTINUE' }) smach.StateMachine.add("ASK_CONTINUE", states.AskContinue(robot, 30), transitions={ 'continue': 'SAY_CONTINUEING', 'no_response': 'SAY_CONTINUEING' }) smach.StateMachine.add( 'SAY_CONTINUEING', states.Say(robot, [ "I heard continue, so I will move to the exit now. See you guys later!" ], 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=EXIT_1), radius=0.7), transitions={ 'arrived': 'AT_END', 'unreachable': 'GO_TO_EXIT_2', 'goal_not_defined': 'GO_TO_EXIT_2' }) smach.StateMachine.add('GO_TO_EXIT_2', states.NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_2), radius=0.5), transitions={ 'arrived': 'AT_END', 'unreachable': 'GO_TO_EXIT_3', 'goal_not_defined': 'GO_TO_EXIT_3' }) smach.StateMachine.add('GO_TO_EXIT_3', states.NavigateToWaypoint(robot, EntityByIdDesignator( robot, id=EXIT_3), radius=0.5), transitions={ 'arrived': 'AT_END', 'unreachable': 'RESET_ED_TARGET', 'goal_not_defined': 'AT_END' }) smach.StateMachine.add('RESET_ED_TARGET', states.ResetED(robot), transitions={'done': 'GO_TO_EXIT'}) # Finally amigo will stop and says 'goodbye' to show that he's done. smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) analyse_designators(sm, "rips") return sm
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") pdf_writer = WritePdf(robot=robot) with self: single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SAY_UNABLE_TO_OPEN_DOOR', 'abort': 'Aborted' }) smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR', states.Say( robot, "I am unable to open the shelf door, " "can you please open it for me?"), transitions={'spoken': 'AWAIT_START'}) smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={ 'continue': "MOVE_TABLE", 'no_response': 'AWAIT_START' }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) room = ds.EntityByIdDesignator(robot, id=ROOM) @smach.cb_interface(outcomes=["done"]) def move_table(userdata=None, manipulate_machine=None): """ Moves the entities for this challenge to the correct poses""" # Determine where to perform the challenge robot_pose = robot.base.get_location() ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm()) cabinet_id = ENTITY_POSES[0][2] table_id = ENTITY_POSES[0][3] # Update the world model robot.ed.update_entity(id="balcony_shelf", frame_stamped=FrameStamped( kdl.Frame(kdl.Rotation(), kdl.Vector(0.0, 3.0, 0.0)), frame_id="map")) robot.ed.update_entity(id=cabinet_id, frame_stamped=ENTITY_POSES[0][0]) robot.ed.update_entity(id=table_id, frame_stamped=ENTITY_POSES[0][1]) # Update designators cabinet.id_ = ENTITY_POSES[0][2] room.id_ = ENTITY_POSES[0][4] # Update manipulate machine manipulate_machine.place_entity_designator.id_ = cabinet_id manipulate_machine.place_designator._area = ENTITY_POSES[0][5] manipulate_machine.place_designator.place_location_designator.id = cabinet_id manipulate_machine.table_designator.id_ = table_id manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2] return "done" smach.StateMachine.add("MOVE_TABLE", smach.CBState(move_table, cb_args=[single_item]), transitions={'done': 'NAV_TO_START'}) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'INSPECT_SHELVES', 'unreachable': 'INSPECT_SHELVES', 'goal_not_defined': 'INSPECT_SHELVES' }) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot, cabinet), transitions={ 'succeeded': 'WRITE_PDF_SHELVES', 'nothing_found': 'WRITE_PDF_SHELVES', 'failed': 'WRITE_PDF_SHELVES' }) smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "RANGE_ITERATOR"}) # Begin setup iterator # The exhausted argument should be set to the prefered state machine outcome range_iterator = smach.Iterator( outcomes=['succeeded', 'failed'], # Outcomes of the iterator state input_keys=[], output_keys=[], it=lambda: range(5), it_label='index', exhausted_outcome='succeeded') with range_iterator: smach.Iterator.set_contained_state( 'SINGLE_ITEM', single_item, loop_outcomes=['succeeded', 'failed']) smach.StateMachine.add('RANGE_ITERATOR', range_iterator, { 'succeeded': 'AT_END', 'failed': 'Aborted' }) # End setup iterator smach.StateMachine.add('AT_END', states.Say(robot, "Goodbye"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "manipulation")
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # - - - - - - - - - - - - - - - - - - - Callback States - - - - - - - - - - - - - - - - - - - @smach.cb_interface(outcomes=['spoken']) def sayHelloCB(userdata=None): printOk("sayHelloCB") robot.speech.speak("Hello " + personNameDes.resolve() + "!", block=False) return 'spoken' # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'INIT_WM', 'abort': 'Aborted' }) smach.StateMachine.add( "INIT_WM", states.InitializeWorldModel(robot), transitions={'done': 'ENTER_ROOM_CONTAINER'}) # smach.StateMachine.add( 'SELECT_NEXT_CONTAINER', # test_states.SelectNextContainer(robot, containerResultDes), # transitions={ 'go_to_enter_room':'ENTER_ROOM_CONTAINER', # 'go_to_wait_person':'WAIT_PERSON_CONTAINER', # 'go_to_pick_up':'PICK_UP_CONTAINER', # 'go_to_recognize_people':'RECOGNIZE_PEOPLE_CONTAINER', # 'go_to_search_people':'SEARCH_PEOPLE_CONTAINER', # 'go_to_end_challenge':'END_CHALLENGE'}) # Navigation test smach.StateMachine.add('ENTER_ROOM_CONTAINER', EnterRoomContainer(robot), transitions={ 'container_success': 'WAIT_PERSON_CONTAINER', 'container_failed': 'WAIT_PERSON_CONTAINER' }) # Human Interaction Test smach.StateMachine.add('WAIT_PERSON_CONTAINER', WaitPersonContainer(robot), transitions={ 'container_success': 'LEARN_NAME_CONTAINER', 'container_failed': 'WAIT_PERSON_CONTAINER' }) # Speech Test smach.StateMachine.add('LEARN_NAME_CONTAINER', LearnNameContainer(robot, personNameDes), transitions={ 'container_failed': 'SAY_HELLO', 'container_success': 'SAY_HELLO' }) smach.StateMachine.add( 'SAY_HELLO', smach.CBState(sayHelloCB), transitions={'spoken': 'LEARN_FACE_CONTAINER'}) # Face Learning Test smach.StateMachine.add('LEARN_FACE_CONTAINER', LearnFaceContainer(robot, personNameDes), transitions={ 'container_success': 'RECOGNIZE_PEOPLE_CONTAINER', 'container_failed': 'RECOGNIZE_PEOPLE_CONTAINER' }) # Face Recognition Test smach.StateMachine.add('RECOGNIZE_PEOPLE_CONTAINER', RecognizePeopleContainer(robot), transitions={ 'container_success': 'PICK_UP_CONTAINER', 'container_failed': 'PICK_UP_CONTAINER' }) # Manipulation Test smach.StateMachine.add('PICK_UP_CONTAINER', PickUpContainer(robot, objectsIDsDes), transitions={ 'container_success': 'SEARCH_PEOPLE_CONTAINER', 'container_failed': 'SEARCH_PEOPLE_CONTAINER' }) # Face Recogniton Test smach.StateMachine.add('SEARCH_PEOPLE_CONTAINER', SearchPeopleContainer(robot, personNameDes), transitions={ 'container_success': 'END_CHALLENGE', 'container_failed': 'END_CHALLENGE' }) smach.StateMachine.add('END_CHALLENGE', states.Say( robot, "My work here is done, goodbye!"), transitions={'spoken': 'Done'})
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'})