def __init__(self, robot, cabinet_id, cabinet_navigate_area, cabinet_inspect_area): smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) self.cabinet = ds.EntityByIdDesignator(robot=robot, id=cabinet_id) with self: smach.StateMachine.add("NAVIGATE_TO_CABINET", states.NavigateToSymbolic( robot, {self.cabinet: cabinet_navigate_area}, self.cabinet), transitions={ 'arrived': 'UPDATE_CABINET_POSE', 'unreachable': 'failed', 'goal_not_defined': 'failed' }) smach.StateMachine.add("UPDATE_CABINET_POSE", UpdateCabinetPose(robot, self.cabinet, cabinet_inspect_area), transitions={ 'succeeded': 'OPEN_DOOR', 'failed': 'failed' }) smach.StateMachine.add("OPEN_DOOR", OpenDoor(robot, self.cabinet), transitions={ 'succeeded': 'succeeded', 'failed': 'failed' })
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, grab_designator_1=None, grab_designator_2=None, place_designator=None, pdf_writer=None): """ Constructor :param robot: robot object :param grab_designator_1: EdEntityDesignator designating the item to grab :param grab_designator_2: EdEntityDesignator designating the item to grab :param pdf_writer: WritePDF object to save images of recognized objects to pdf files """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators self._table_designator = ds.EntityByIdDesignator(robot, id=TABLE) if grab_designator_1 is None: grab_designator_1 = DefaultGrabDesignator( robot=robot, surface_designator=self._table_designator, area_description=GRAB_SURFACE) if grab_designator_2 is None: grab_designator_2 = DefaultGrabDesignator( robot=robot, surface_designator=self._table_designator, area_description=GRAB_SURFACE) with self: smach.StateMachine.add("MOVE_TO_TABLE1", states.NavigateToSymbolic( robot, {self._table_designator: "in_front_of"}, self._table_designator), transitions={ 'arrived': 'INSPECT_TABLE', 'unreachable': 'MOVE_TO_TABLE2', 'goal_not_defined': 'INSPECT_TABLE' }) smach.StateMachine.add( "MOVE_TO_TABLE2", states.NavigateToSymbolic( robot, {self._table_designator: "large_in_front_of"}, self._table_designator), transitions={ 'arrived': 'INSPECT_TABLE', 'unreachable': 'INSPECT_TABLE', 'goal_not_defined': 'INSPECT_TABLE' }) if pdf_writer: # Designator to store the classificationresults class_designator = ds.VariableDesignator( [], resolve_type=[ robot_skills.classification_result.ClassificationResult ]) # Add the designator to the pdf writer state pdf_writer.set_designator(class_designator) smach.StateMachine.add("INSPECT_TABLE", states.Inspect( robot=robot, entityDes=self._table_designator, objectIDsDes=class_designator, searchArea=GRAB_SURFACE, navigation_area="in_front_of"), transitions={ "done": "WRITE_PDF", "failed": "failed" }) smach.StateMachine.add("WRITE_PDF", pdf_writer, transitions={"done": "GRAB_ITEM_1"}) else: smach.StateMachine.add("INSPECT_TABLE", states.Inspect( robot=robot, entityDes=self._table_designator, objectIDsDes=None, searchArea=GRAB_SURFACE, inspection_area="in_front_of"), transitions={ "done": "GRAB_ITEM_1", "failed": "failed" }) smach.StateMachine.add("GRAB_ITEM_1", GrabSingleItem( robot=robot, grab_designator=grab_designator_1), transitions={ "succeeded": "GRAB_ITEM_2", "failed": "GRAB_ITEM_2" }) smach.StateMachine.add("GRAB_ITEM_2", GrabSingleItem( robot=robot, grab_designator=grab_designator_2), transitions={ "succeeded": "MOVE_TO_PLACE", "failed": "MOVE_TO_PLACE" }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) smach.StateMachine.add("MOVE_TO_PLACE", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'PLACE_ITEM_1', 'unreachable': 'PLACE_ITEM_1', 'goal_not_defined': 'PLACE_ITEM_1' }) smach.StateMachine.add("PLACE_ITEM_1", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "PLACE_ITEM_2", "failed": "PLACE_ITEM_2" }) smach.StateMachine.add("PLACE_ITEM_2", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "succeeded", "failed": "failed" })
def __init__(self, robot, grasp_designator1, grasp_designator2, grasp_designator3, grasp_furniture_id1, grasp_furniture_id3, place_furniture_id): """ Constructor :param robot: robot object :param grasp_designator1: EdEntityDesignator designating the first item to grab. :param grasp_designator2: EdEntityDesignator designating the second item to grab. :param grasp_designator3: EdEntityDesignator designating the third item to grab. :param grasp_furniture_id1: string identifying the location where to grasp objects 1 and 2 :param grasp_furniture_id3: string identifying the location where to grasp object 3 :param place_furniture_id: string identifying the location where to place the objects """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators grasp_furniture_designator1 = ds.EntityByIdDesignator( robot, id=grasp_furniture_id1) grasp_furniture_designator3 = ds.EntityByIdDesignator( robot, id=grasp_furniture_id3) place_furniture_designator = ds.EntityByIdDesignator( robot, id=place_furniture_id) place_designator = ds.EmptySpotDesignator( robot=robot, place_location_designator=place_furniture_designator, area="on_top_of") with self: # Move to the inspect location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE1", states.NavigateToSymbolic( robot, {grasp_furniture_designator1: "in_front_of"}, grasp_furniture_designator1), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE', 'unreachable': 'MOVE_TO_GRASP_SURFACE2', 'goal_not_defined': 'INSPECT_GRASP_SURFACE' }) # Backup for moving to inspect location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE2", states.NavigateToSymbolic( robot, {grasp_furniture_designator1: "large_in_front_of"}, grasp_furniture_designator1), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE', 'unreachable': 'INSPECT_GRASP_SURFACE', 'goal_not_defined': 'INSPECT_GRASP_SURFACE' }) # Inspect grasp furniture smach.StateMachine.add("INSPECT_GRASP_SURFACE", states.Inspect( robot=robot, entityDes=grasp_furniture_designator1, objectIDsDes=None, searchArea="on_top_of", navigation_area="in_front_of"), transitions={ "done": "GRAB_ITEM_1", "failed": "failed" }) # Grasp the first item smach.StateMachine.add("GRAB_ITEM_1", GrabSingleItem( robot=robot, grab_designator=grasp_designator1), transitions={ "succeeded": "GRAB_ITEM_2", "failed": "GRAB_ITEM_2" }) # Grasp the second item smach.StateMachine.add("GRAB_ITEM_2", GrabSingleItem( robot=robot, grab_designator=grasp_designator2), transitions={ "succeeded": "MOVE_TO_PLACE", "failed": "MOVE_TO_PLACE" }) # Move to the place location smach.StateMachine.add( "MOVE_TO_PLACE", states.NavigateToSymbolic( robot, {place_furniture_designator: "in_front_of"}, place_furniture_designator), transitions={ 'arrived': 'PLACE_ITEM_1', 'unreachable': 'PLACE_ITEM_1', 'goal_not_defined': 'PLACE_ITEM_1' }) # Place the first item smach.StateMachine.add("PLACE_ITEM_1", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "PLACE_ITEM_2", "failed": "PLACE_ITEM_2" }) # Place the second item smach.StateMachine.add("PLACE_ITEM_2", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "MOVE_TO_GRASP_SURFACE3", "failed": "MOVE_TO_GRASP_SURFACE3" }) # Move back to the grasp surface to grasp the third item smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE3", states.NavigateToSymbolic( robot, {grasp_furniture_designator3: "in_front_of"}, grasp_furniture_designator3), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE2', 'unreachable': 'MOVE_TO_GRASP_SURFACE4', 'goal_not_defined': 'INSPECT_GRASP_SURFACE2' }) # Backup for moving back to the grasp location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE4", states.NavigateToSymbolic( robot, {grasp_furniture_designator3: "large_in_front_of"}, grasp_furniture_designator3), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE2', 'unreachable': 'INSPECT_GRASP_SURFACE2', 'goal_not_defined': 'INSPECT_GRASP_SURFACE2' }) # Inspect grasp furniture smach.StateMachine.add("INSPECT_GRASP_SURFACE2", states.Inspect( robot=robot, entityDes=grasp_furniture_designator3, objectIDsDes=None, searchArea="shelf2", navigation_area="in_front_of"), transitions={ "done": "GRAB_ITEM_3", "failed": "failed" }) # Grasp the third item smach.StateMachine.add("GRAB_ITEM_3", GrabSingleItem( robot=robot, grab_designator=grasp_designator3), transitions={ "succeeded": "MOVE_TO_PLACE_3", "failed": "MOVE_TO_PLACE_3" }) # Move to the place location smach.StateMachine.add( "MOVE_TO_PLACE_3", states.NavigateToSymbolic( robot, {place_furniture_designator: "in_front_of"}, place_furniture_designator), transitions={ 'arrived': 'PLACE_ITEM_3', 'unreachable': 'PLACE_ITEM_3', 'goal_not_defined': 'PLACE_ITEM_3' }) # Place the first item smach.StateMachine.add("PLACE_ITEM_3", PlaceSingleItem( robot=robot, place_designator=place_designator), transitions={ "succeeded": "succeeded", "failed": "succeeded" })
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): 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, 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, 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', states.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', states.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", states.Say(robot, "Looking {area} the {entity}", area=segment_area, entity=location_id_des, block=False), transitions={"spoken": "RISE"}) smach.StateMachine.add('RISE', states.RiseForInspect( robot, e_des, segment_area), transitions={ 'succeeded': 'SEGMENT', 'failed': 'SEGMENT' }) smach.StateMachine.add( 'SEGMENT', states.SegmentObjects(robot, e_classifications_des.writeable, e_des, segment_area), transitions={'done': "HANDLE_DETECTED_ENTITIES"}) smach.StateMachine.add("SAY_UNREACHABLE", states.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"})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"]) hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult) information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator( hmi_result_des, "semantics", resolve_type=unicode), str, resolve_type=str) information_point_designator = ds.EdEntityDesignator( robot, id_designator=information_point_id_designator) with self: single_item = InformMachine(robot) if START_ROBUST: smach.StateMachine.add("START_CHALLENGE", states.StartChallengeRobust( robot, INITIAL_POSE_ID), transitions={ "Done": "ASK_WHERE_TO_GO", "Aborted": "Aborted", "Failed": "Aborted" }) smach.StateMachine.add( "ASK_WHERE_TO_GO", states.Say( robot, "Near which furniture object should I go to start guiding people?" ), transitions={"spoken": "WAIT_WHERE_TO_GO"}) smach.StateMachine.add( "WAIT_WHERE_TO_GO", states.HearOptionsExtra( robot=robot, spec_designator=ds.Designator( initial_value=START_GRAMMAR), speech_result_designator=hmi_result_des.writeable), transitions={ "heard": "ASK_CONFIRMATION", "no_result": "ASK_WHERE_TO_GO" }) # ToDo: add fallbacks #option: STORE_STARTING_POSE smach.StateMachine.add( "ASK_CONFIRMATION", states.Say(robot, [ "I hear that you would like me to start the tours at" " the {place}, is this correct?" ], place=information_point_id_designator, block=True), transitions={"spoken": "CONFIRM_LOCATION"}) smach.StateMachine.add("CONFIRM_LOCATION", states.HearOptions( robot=robot, options=["yes", "no"]), transitions={ "yes": "MOVE_OUT_OF_MY_WAY", "no": "ASK_WHERE_TO_GO", "no_result": "ASK_WHERE_TO_GO" }) smach.StateMachine.add( "MOVE_OUT_OF_MY_WAY", states.Say(robot, "Please move your ass so I can get going!"), transitions={"spoken": "TC_MOVE_TIME"}) smach.StateMachine.add("TC_MOVE_TIME", states.WaitTime(robot=robot, waittime=3), transitions={ "waited": "NAV_TO_START", "preempted": "Aborted" }) smach.StateMachine.add( "NAV_TO_START", states.NavigateToSymbolic( robot=robot, entity_designator_area_name_map={ information_point_designator: "in_front_of" }, entity_lookat_designator=information_point_designator), transitions={ "arrived": "TURN_AROUND", "unreachable": "WAIT_NAV_BACKUP", "goal_not_defined": "Aborted" }) # If this happens: never mind smach.StateMachine.add("WAIT_NAV_BACKUP", states.WaitTime(robot, 3.0), transitions={ "waited": "NAV_TO_START_BACKUP", "preempted": "Aborted" }) smach.StateMachine.add( "NAV_TO_START_BACKUP", states.NavigateToSymbolic( robot=robot, entity_designator_area_name_map={ information_point_designator: "near" }, entity_lookat_designator=information_point_designator), transitions={ "arrived": "TURN_AROUND", "unreachable": "SAY_CANNOT_REACH_WAYPOINT", # Current pose backup "goal_not_defined": "Aborted" }) # If this happens: never mind @smach.cb_interface(outcomes=["done"]) def _turn_around(userdata=None): """ Turns the robot approximately 180 degrees around """ v_th = 0.5 robot.base.force_drive(vx=0.0, vy=0.0, vth=v_th, timeout=math.pi / v_th) return "done" smach.StateMachine.add( "TURN_AROUND", smach.CBState(_turn_around), transitions={"done": "STORE_STARTING_POSE"}) smach.StateMachine.add( "SAY_CANNOT_REACH_WAYPOINT", states.Say( robot, "I am not able to reach the starting point." "I'll use this as starting point"), transitions={"spoken": "STORE_STARTING_POSE"}) else: smach.StateMachine.add("INITIALIZE", states.Initialize(robot), transitions={ "initialized": "STORE_STARTING_POSE", "abort": "Aborted" }) ## This is purely for a back up scenario until the range iterator @smach.cb_interface(outcomes=["succeeded"]) def store_pose(userdata=None): base_loc = robot.base.get_location() base_pose = base_loc.frame location_id = INFORMATION_POINT_ID robot.ed.update_entity(id=location_id, frame_stamped=FrameStamped( base_pose, "/map"), type="waypoint") return "succeeded" smach.StateMachine.add("STORE_STARTING_POSE", smach.CBState(store_pose), transitions={"succeeded": "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(1000), 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"})
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 setup_statemachine(robot): sm = smach.StateMachine(outcomes=['Done', 'Aborted']) start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint") placed_items = [] with sm: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'INIT_WM', 'abort': 'Aborted' }) smach.StateMachine.add("INIT_WM", InitializeWorldModel(robot), transitions={'done': 'AWAIT_START'}) # smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR", # states.Say(robot, ["Hi there, I will now wait until you remove the cup", # "I'm waiting for you to remove the cup"], block=False), # transitions={"spoken": "WAIT_FOR_DOOR"}) # # smach.StateMachine.add("WAIT_FOR_DOOR", # states.WaitForDoorOpen(robot, timeout=10), # transitions={"closed": "DOOR_CLOSED", # "open": "AWAIT_START"}) # # smach.StateMachine.add("DOOR_CLOSED", # states.Say(robot, ["I am waiting for you to remove the cup", # "I'd start, if you remove the cup from my laser"]), # transitions={"spoken": "WAIT_FOR_DOOR"}) if USE_SLAM: drive_state = "RESET_ED_SLAM" else: drive_state = "NAV_TO_START" smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={ 'continue': drive_state, 'no_response': 'AWAIT_START' }) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) room = ds.EntityByIdDesignator(robot, id=ROOM) if USE_SLAM: # vth = 1.0 # smach.StateMachine.add("NAV_TO_FIT_POSE", # ForceDrive(robot, 0, 0, vth, 3.14/vth), # transitions={'done': 'FIT_ENTITY'}) smach.StateMachine.add("RESET_ED_SLAM", states.ResetED(robot), transitions={'done': 'FIT_ENTITY'}) smach.StateMachine.add("FIT_ENTITY", FitEntity(robot, CABINET), transitions={ 'succeeded': 'NAV_TO_START', 'failed': 'SAY_FITTING_FAILED' }) smach.StateMachine.add( "SAY_FITTING_FAILED", states.Say(robot, [ "Fitting the {0} failed, I will stop now.".format(CABINET) ], mood="sad"), transitions={'spoken': 'Aborted'}) smach.StateMachine.add("NAV_TO_START", states.NavigateToSymbolic( robot, {cabinet: "in_front_of"}, cabinet), transitions={ 'arrived': 'INSPECT_SHELVES', 'unreachable': 'FORCE_ROTATE', 'goal_not_defined': 'INSPECT_SHELVES' }) smach.StateMachine.add("FORCE_ROTATE", ForceRotate(robot, 0.5, 2.0, 30.0), transitions={ 'done': "NAV_TO_START", 'timedout': "INSPECT_SHELVES" }) # smach.StateMachine.add("RESET_ED", # states.ResetED(robot), # transitions={'done' :'INSPECT_SHELVES'}) smach.StateMachine.add("INSPECT_SHELVES", InspectShelves(robot, OBJECT_SHELVES), transitions={ 'succeeded': 'EXPORT_PDF', 'nothing_found': 'EXPORT_PDF', 'failed': 'EXPORT_PDF' }) @smach.cb_interface(outcomes=["exported"]) def export_to_pdf(userdata=None): global DETECTED_OBJECTS_WITH_PROBS entities = [e[0] for e in DETECTED_OBJECTS_WITH_PROBS] # Export images (Only best MAX_NUM_ENTITIES_IN_PDF) # pdf.entities_to_pdf(robot.ed, entities[:MAX_NUM_ENTITIES_IN_PDF], "tech_united_manipulation_challenge") return "exported" smach.StateMachine.add('EXPORT_PDF', smach.CBState(export_to_pdf), transitions={'exported': 'RANGE_ITERATOR'}) # Begin setup iterator 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' ) #The exhausted argument should be set to the preffered state machine outcome with range_iterator: single_item = ManipRecogSingleItem( robot, ds.VariableDesignator(placed_items, [Entity], name="placed_items")) 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(sm, "manipulation") 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) @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 # Apparently, the indices here come from: # (cabinet_pose, table_pose, cabinet_amcl, grasp_surface, room, default_place_area) robot_pose = robot.base.get_location() WORKSPACES.sort( key=lambda ws: (ws.place_entity_conf.pose_estimate.frame.p - robot_pose.frame.p).Norm()) closest_workspace = WORKSPACES[0] rospy.loginfo( "Closest workspace: grasp from '{grasp}' and place on '{place}'" .format( grasp=closest_workspace.grasp_entity_conf.entity_id, place=closest_workspace.place_entity_conf.entity_id)) cabinet_id = closest_workspace.place_entity_conf.entity_id table_id = closest_workspace.grasp_entity_conf.entity_id # Update the world model by fitting the entities to the frame_stamped's given below. robot.ed.update_entity(id=cabinet_id, frame_stamped=closest_workspace. place_entity_conf.pose_estimate) robot.ed.update_entity(id=table_id, frame_stamped=closest_workspace. grasp_entity_conf.pose_estimate) # Update designators cabinet.id_ = closest_workspace.place_entity_conf.entity_id # Update manipulate machine manipulate_machine.table_designator.id_ = closest_workspace.grasp_entity_conf.entity_id manipulate_machine.place_entity_designator.id_ = closest_workspace.place_entity_conf.entity_id manipulate_machine.place_designator._area = closest_workspace.place_entity_conf.manipulation_volumes[ 0] manipulate_machine.place_designator.place_location_designator.id = closest_workspace.place_entity_conf.entity_id manipulate_machine.cabinet.id_ = closest_workspace.place_entity_conf.entity_id 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, entity_des, operator_distance=1.5, operator_radius=0.5): """ Constructor :param robot: robot object :param entity_des: designator resolving to a room or a piece of furniture :param operator_distance: (float) check for the operator to be within this range of the robot :param operator_radius: (float) from the point behind the robot defined by `distance`, the person must be within this radius """ smach.StateMachine.__init__(self, outcomes=[ "arrived", "unreachable", "goal_not_defined", "lost_operator", "preempted" ]) self.operator_distance = operator_distance self.operator_radius = operator_radius with self: @smach.cb_interface(outcomes=["room", "object"]) def determine_type(userdata=None): entity = entity_des.resolve() entity_type = entity.type if entity_type == "room": return "room" else: return "object" smach.StateMachine.add("DETERMINE_TYPE", smach.CBState(determine_type), transitions={ "room": "GUIDE_TO_ROOM", "object": "GUIDE_TO_FURNITURE" }) smach.StateMachine.add( "GUIDE_TO_ROOM", guidance.GuideToSymbolic( robot, {entity_des: "in"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "WAIT_ROOM_BACKUP", "goal_not_defined": "goal_not_defined", "lost_operator": "ROOM_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add("WAIT_ROOM_BACKUP", states.WaitTime(robot, 3.0), transitions={ "waited": "GUIDE_TO_ROOM_BACKUP", "preempted": "preempted" }) smach.StateMachine.add( "GUIDE_TO_ROOM_BACKUP", guidance.GuideToSymbolic( robot, {entity_des: "in"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "unreachable", "goal_not_defined": "goal_not_defined", "lost_operator": "ROOM_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add( "GUIDE_TO_FURNITURE", guidance.GuideToSymbolic( robot, {entity_des: "in_front_of"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "WAIT_FURNITURE_BACKUP", # Something is blocking "goal_not_defined": "GUIDE_NEAR_FURNITURE", # in_front_of not defined "lost_operator": "FURNITURE_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add( "GUIDE_NEAR_FURNITURE", guidance.GuideToSymbolic( robot, {entity_des: "near"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "WAIT_FURNITURE_BACKUP", "goal_not_defined": "goal_not_defined", "lost_operator": "FURNITURE_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add("WAIT_FURNITURE_BACKUP", states.WaitTime(robot, 3.0), transitions={ "waited": "GUIDE_NEAR_FURNITURE_BACKUP", "preempted": "preempted" }) smach.StateMachine.add( "GUIDE_NEAR_FURNITURE_BACKUP", guidance.GuideToSymbolic( robot, {entity_des: "near"}, entity_des, operator_distance=self.operator_distance, operator_radius=self.operator_radius), transitions={ "arrived": "arrived", "unreachable": "unreachable", "goal_not_defined": "goal_not_defined", "lost_operator": "FURNITURE_NAV_BACKUP", "preempted": "preempted" }) smach.StateMachine.add("ROOM_NAV_BACKUP", states.NavigateToSymbolic( robot, {entity_des: "in"}, entity_des), transitions={ "arrived": "SAY_ARRIVED", "unreachable": "unreachable", "goal_not_defined": "goal_not_defined", }) smach.StateMachine.add("FURNITURE_NAV_BACKUP", states.NavigateToSymbolic( robot, {entity_des: "near"}, entity_des), transitions={ "arrived": "SAY_ARRIVED", "unreachable": "unreachable", "goal_not_defined": "goal_not_defined", }) smach.StateMachine.add( "SAY_ARRIVED", states.Say( robot, "We have arrived. I'll go back to the meeting point"), transitions={"spoken": "arrived"})
def __init__(self, robot): """ Constructor :param robot: robot object """ smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"]) # Designators furniture_move_designator = PointingDesignator(robot=robot) furniture_pick_designator = PointingDesignator(robot=robot) furniture_place_designator = PointingDesignator(robot=robot) with self: # Start challenge smach.StateMachine.add("START_CHALLENGE", states.StartChallengeRobust( robot=robot, initial_pose=knowledge.initial_pose), transitions={ "Done": "NAVIGATE_TO_MEETING_POINT0", "Aborted": "Aborted", "Failed": "Aborted" }) # Move to meeting point # ToDo: add challenge knowledge smach.StateMachine.add("NAVIGATE_TO_MEETING_POINT0", states.NavigateToWaypoint( robot=robot, waypoint_designator=EdEntityDesignator( robot=robot, id=knowledge.meeting_point), radius=0.15), transitions={ "arrived": "DETECT_POINTING0", "unreachable": "DETECT_POINTING0", "goal_not_defined": "DETECT_POINTING0" }) # Wait for the operator to appear and detect what he's pointing at smach.StateMachine.add("DETECT_POINTING0", PointingDetector( robot=robot, designator=furniture_move_designator, default_entity_id="couch_table", super_type="furniture"), transitions={ "succeeded": "MOVE_TO_ITEM", "failed": "DETECT_POINTING0" }) # Move to jury table smach.StateMachine.add("MOVE_TO_ITEM", states.NavigateToSymbolic( robot, {furniture_move_designator: "near"}, furniture_move_designator), transitions={ "arrived": "SAY_HI_TO_JURY", "unreachable": "SAY_HI_TO_JURY", "goal_not_defined": "SAY_HI_TO_JURY" }) smach.StateMachine.add( "SAY_HI_TO_JURY", states.Say(robot, "Hi guys, I'm glad you're here", block=True), transitions={"spoken": "NAVIGATE_TO_MEETING_POINT1"}) smach.StateMachine.add("NAVIGATE_TO_MEETING_POINT1", states.NavigateToWaypoint( robot=robot, waypoint_designator=EdEntityDesignator( robot=robot, id=knowledge.meeting_point), radius=0.15), transitions={ "arrived": "DETECT_POINTING1", "unreachable": "DETECT_POINTING1", "goal_not_defined": "DETECT_POINTING1" }) # Wait for the operator to appear and detect what he's pointing at smach.StateMachine.add("DETECT_POINTING1", PointingDetector( robot=robot, designator=furniture_pick_designator, default_entity_id="desk", super_type="furniture"), transitions={ "succeeded": "PICKUP_ITEM", "failed": "DETECT_POINTING1" }) # Inspect and pickup smach.StateMachine.add( "PICKUP_ITEM", PickupItem(robot=robot, furniture_designator=furniture_pick_designator), transitions={ "succeeded": "NAVIGATE_TO_MEETING_POINT2", "failed": "NAVIGATE_TO_MEETING_POINT2" }) # Move back to meeting point # ToDo: add challenge knowledge smach.StateMachine.add("NAVIGATE_TO_MEETING_POINT2", states.NavigateToWaypoint( robot=robot, waypoint_designator=EdEntityDesignator( robot=robot, id=knowledge.meeting_point), radius=0.15), transitions={ "arrived": "DETECT_POINTING2", "unreachable": "DETECT_POINTING2", "goal_not_defined": "DETECT_POINTING2" }) # Wait for the operator to appear and detect what he's pointing at smach.StateMachine.add("DETECT_POINTING2", PointingDetector( robot=robot, designator=furniture_place_designator, default_entity_id="side_table", super_type="furniture"), transitions={ "succeeded": "PLACE_ITEM", "failed": "DETECT_POINTING2" }) # Place the object smach.StateMachine.add( "PLACE_ITEM", PlaceSingleItem(robot=robot, place_designator=furniture_place_designator), transitions={ "succeeded": "SAY_DONE", "failed": "SAY_DONE" }) # Say that we're done smach.StateMachine.add( "SAY_DONE", states.Say( robot, "That's it for today, you can have a closer look at my" "skills on the screen", block=False), transitions={"spoken": "RESET_HEAD"}) @smach.cb_interface(outcomes=["done"]) def reset_head(userdata=None): """ Resets the head """ print "Resetting head" robot.head.reset() rospy.sleep(rospy.Duration(1.0)) return "done" smach.StateMachine.add("RESET_HEAD", smach.CBState(reset_head), transitions={"done": "Done"})
robot = Robot() if len(sys.argv) > 2: ids = sys.argv[2:] else: robot.speech.speak("No ids specified, I will do them all", block=False) ids = [e.id for e in robot.ed.get_entities() if e.is_a("furniture")] random.shuffle(ids) print "IDS:", ids for id in ids: robot.speech.speak("I am going to navigate to the %s" % id, block=False) machine = robot_smach_states.NavigateToSymbolic(robot, {robot_smach_states.util.designators.EntityByIdDesignator(robot, id=id): "in_front_of"}, robot_smach_states.util.designators.EntityByIdDesignator(robot, id=id)) machine.execute() robot.head.look_down() robot.head.wait_for_motion_done() import time time.sleep(1) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - # Segment robot.speech.speak("Segmenting on top of the %s" % id, block=False) segmented_entities = robot.ed.update_kinect("on_top_of %s" % id)
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: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={'initialized': 'AWAIT_START', 'abort': 'Aborted'}) smach.StateMachine.add("AWAIT_START", states.AskContinue(robot), transitions={'continue': "MOVE_TABLE", 'no_response': 'AWAIT_START'}) @smach.cb_interface(outcomes=["done"]) def move_table(userdata=None): """ 'Locks' a locking designator """ # For now, don't do anything return "done" # Move away the cabinet robot.ed.update_entity(id="cabinet", frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(), kdl.Vector(12.0, 0, 0)), frame_id="map")) # 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()) # Update the world model robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0]) robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1]) return "done" smach.StateMachine.add("MOVE_TABLE", smach.CBState(move_table), transitions={'done': 'NAV_TO_START'}) cabinet = ds.EntityByIdDesignator(robot, id=CABINET) room = ds.EntityByIdDesignator(robot, id=ROOM) 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), 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: single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) # ToDo: add more pdf stuff 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")