def execute(self, userdata=None): # Try to place the object item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) arm_designator = ds.OccupiedArmDesignator( self._robot, { "required_goals": ["reset", "handover_to_human"], "required_gripper_types": [arms.GripperTypes.GRASPING] }) resolved_arm = arm_designator.resolve() if resolved_arm is None: rospy.logwarn("No arm holding an entity") return "failed" place = states.Place(robot=self._robot, item_to_place=item, place_pose=self.place_designator, arm=arm_designator) result = place.execute() # If failed, do handover to human in order to continue if result != "done": rospy.loginfo("{place} resulted in {out}".format(place=place, out=result)) handover = states.HandoverToHuman(robot=self._robot, arm_designator=arm_designator) handover.execute() return "succeeded" if result == "done" else "failed"
def execute(self, userdata=None): # Try to place the object item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) arm_designator = ds.OccupiedArmDesignator( self._robot, arm_properties={ "required_trajectories": ["prepare_place"], "required_goals": ["reset", "handover_to_human"], "required_gripper_types": [arms.GripperTypes.GRASPING] }) resolved_arm = arm_designator.resolve() if resolved_arm is None: rospy.logwarn("No arm holding an entity") return "failed" sm = states.Place(robot=self._robot, item_to_place=item, place_pose=self._place_designator, arm=arm_designator) result = sm.execute() # If failed, do handover to human in order to continue if result != "done": sm = states.HandoverToHuman(robot=self._robot, arm_designator=arm_designator) sm.execute() return "succeeded" if result == "done" else "failed"
def execute(self, userdata=None): arm = None # See if there's an arm holding something for k, v in self._robot.arms.iteritems(): if v.occupied_by is not None: arm = v break if arm is None: return "failed" # Try to place the object item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) arm_designator = ds.ArmDesignator(all_arms={arm.side: arm}, preferred_arm=arm) sm = states.Place(robot=self._robot, item_to_place=item, place_pose=self._place_designator, arm=arm_designator) result = sm.execute() # If failed, do handover to human in order to continue if result != "done": sm = states.HandoverToHuman(robot=self._robot, arm_designator=arm_designator) sm.execute() return "succeeded" if result == "done" else "failed"
def __init__(self, robot, place_designator=None): """ Constructor :param robot: robot object :param place_designator: Designator that resolves to the pose to place at. E.g. an EmptySpotDesignator """ smach.State.__init__(self, outcomes=["succeeded", "failed"]) self._robot = robot if place_designator is not None: self._place_designator = place_designator else: place_entity_designator = ds.EdEntityDesignator( robot=robot, id=DEFAULT_PLACE_ENTITY) self._place_designator = ds.EmptySpotDesignator( robot=robot, place_location_designator=place_entity_designator, area=DEFAULT_PLACE_AREA)
def setup_statemachine(robot): state_machine = StateMachine(outcomes=['done']) furniture_designator = VariableDesignator(resolve_type=Entity) entity_designator = VariableDesignator(resolve_type=Entity) arm_designator = UnoccupiedArmDesignator(robot, {}) with state_machine: # Intro StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT), transitions={'Done': 'SAY_START', 'Aborted': 'done', 'Failed': 'SAY_START'}) # Say we're gonna start StateMachine.add('SAY_START', Say(robot, "Hand me that it is!", block=False), transitions={'spoken': 'NAVIGATE_TO_START'}) # Drive to the start location StateMachine.add('NAVIGATE_TO_START', NavigateToWaypoint(robot, ds.EdEntityDesignator(robot, id=HOME_LOCATION)), transitions={'arrived': 'GET_FURNITURE_FROM_OPERATOR_POSE', 'unreachable': 'NAVIGATE_TO_START', # ToDo: other fallback 'goal_not_defined': 'done'}) # I'm not even going to fill this in # The pre-work StateMachine.add('GET_FURNITURE_FROM_OPERATOR_POSE', GetFurnitureFromOperatorPose(robot, furniture_designator.writeable), transitions={'done': 'INSPECT_FURNITURE'}) # Go to the furniture object that was pointing to see what's there StateMachine.add('INSPECT_FURNITURE', InspectFurniture(robot, furniture_designator, entity_designator.writeable), transitions={"succeeded": "IDENTIFY_OBJECT", "failed": "NAVIGATE_TO_START"}) # If no entities, try again # Point at the object StateMachine.add('IDENTIFY_OBJECT', IdentifyObject(robot, entity_designator, arm_designator), transitions={'done': 'NAVIGATE_TO_START', # Just keep on going 'failed': 'NAVIGATE_TO_START'}) # Just keep on going return state_machine
def execute(self, userdata=None): # Try to place the object item = ds.EdEntityDesignator(robot=self._robot, id=arm.occupied_by.id) arm_designator = ds.OccupiedArmDesignator(self._robot) resolved_arm = arm_designator.resolve() if resolved_arm is None: rospy.logwarn("No arm holding an entity") return "failed" sm = states.Place(robot=self._robot, item_to_place=item, place_pose=self._place_designator, arm=arm_designator) result = sm.execute() # If failed, do handover to human in order to continue if result != "done": sm = states.HandoverToHuman(robot=self._robot, arm_designator=arm_designator) sm.execute() return "succeeded" if result == "done" else "failed"
def __init__(self, dummy_robot): smach.StateMachine.__init__(self, outcomes=[ "succeeded", "failed", ]) # Create designators dummy_trashbin_designator = ds.EdEntityDesignator( dummy_robot, id=CHALLENGE_KNOWLEDGE.trashbin_id2, name='trashbin_designator') dummy_arm_designator_un = ds.UnoccupiedArmDesignator(dummy_robot, {}) dummy_arm_designator_oc = ds.OccupiedArmDesignator(dummy_robot, {}) with self: smach.StateMachine.add("PICK_UP_TRASH", PickUpTrash(dummy_robot, dummy_trashbin_designator, dummy_arm_designator_un), transitions={ "succeeded": "TURN_BASE", "failed": "TURN_BASE", "aborted": "failed" }) smach.StateMachine.add("TURN_BASE", states.ForceDrive(dummy_robot, 0, 0, 0.5, 3), transitions={"done": "DROP_TRASH"}) smach.StateMachine.add("DROP_TRASH", DropTrash(dummy_robot, dummy_arm_designator_oc), transitions={ "succeeded": "succeeded", "failed": "failed" })
import argparse import robot_smach_states.util.designators as ds from robot_skills import get_robot if __name__ == "__main__": rospy.init_node("testdesignator") parser = argparse.ArgumentParser( description="Test the empty spot designator") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() robot = get_robot(args.robot) furniture_designator = ds.EdEntityDesignator(robot, id="dinner_table") def with_area(): esd = ds.EmptySpotDesignator( robot=robot, place_location_designator=furniture_designator, name="with_area", area="on_top_of") print(esd.resolve()) def without_area(): esd = ds.EmptySpotDesignator( robot=robot, place_location_designator=furniture_designator, name="without_area") print(esd.resolve())
return 'succeeded' if __name__ == "__main__": from robot_skills import get_robot if len(sys.argv) > 1: robot_name = sys.argv[1] point_at = sys.argv[2] look_at = sys.argv[3] rospy.init_node('test_follow_operator') robot = get_robot(robot_name) sm = PointAt( robot, arm_designator=ds.UnoccupiedArmDesignator( robot, {'required_goals': ['point_at']}), point_at_designator=ds.EdEntityDesignator(robot, id=point_at, name='point_at_des'), look_at_designator=ds.EdEntityDesignator(robot, id=look_at, name='look_at_des')) sm.execute() else: print( "Please provide robot name, point_at ID and look_at ID as argument." ) exit(1)
def __init__(self, robot, attempts = 1, sleep_interval = 1): # TODO: add center_point in front of the robot and radius of the search on ds.EdEntityDesignator human_entity = ds.EdEntityDesignator(robot, type="human") WaitForDesignator.__init__(self, robot, human_entity, attempts, sleep_interval)
def __init__(self, robot): # type: (Robot) -> str """ Initialization method :param robot: robot api object """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) # Designators bar_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.bar_id, name='bar_des') room_designator = ds.EdEntityDesignator(robot=robot, id=challenge_knowledge.room_id, name='room_des') objects_list_des = ds.VariableDesignator(resolve_type=[ClassificationResult], name='objects_list_des') unav_drink_des = ds.VariableDesignator(resolve_type=str, name='unav_drink_str_des') hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm') with self: smach.StateMachine.add("INITIALIZE", states.Initialize(robot=robot), transitions={"initialized": "INITIAL_POSE", "abort": "aborted"}) smach.StateMachine.add("INITIAL_POSE", states.SetInitialPose(robot, challenge_knowledge.starting_point), transitions={"done": "INSPECT_BAR", "preempted": "aborted", "error": "INSPECT_BAR"}) # Inspect bar and store the list of available drinks smach.StateMachine.add("INSPECT_BAR", states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of", objectIDsDes=objects_list_des), transitions={"done": "INSPECT_FALLBACK", #TODO: Change to CHECK_INSPECT_RESULT after RWC2019 "failed": "INSPECT_FALLBACK"}) smach.StateMachine.add("CHECK_INSPECT_RESULT", CheckInspect(objects_list_des, [ClassificationResult]), transitions={"true": "IDENTIFY_UNAVAILABLE_DRINK", "false": "INSPECT_FALLBACK"}) smach.StateMachine.add("IDENTIFY_UNAVAILABLE_DRINK", IdentifyUnavailableDrinkFromRecognitions(objects=common_knowledge.objects, classification_list_designator=objects_list_des, unavailable_drink_designator=unav_drink_des.writeable, max_unavailable_drinks=challenge_knowledge.MAX_UNAVAILABLE_DRINKS), transitions={"done": "NAVIGATE_TO_ROOM", "failed": "INSPECT_FALLBACK"}) # Inspect fallback - ask the bartender which drink is unavailable and store the unavailable drink smach.StateMachine.add("INSPECT_FALLBACK", AskAvailability(robot=robot, unavailable_drink_designator=unav_drink_des.writeable, objects=common_knowledge.objects), transitions={"succeeded": "RESET_ROBOT", "failed": "RESET_ROBOT"}) smach.StateMachine.add("RESET_ROBOT", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "NAVIGATE_TO_ROOM", 'failed': "NAVIGATE_TO_ROOM"}) # Navigate to the predefined room smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "SAY_HI", "unreachable": "SAY_HI", "goal_not_defined": "aborted"}) smach.StateMachine.add("SAY_HI", states.Say(robot, "Hi, I am {}. I'll be your waiter today".format(robot.robot_name)), transitions={"spoken": "SERVE_DRINK_1"}) # Explicitly add a new state for each drink, i.e., don't use a range iterator to make sure a new state # is constructed every time for idx in range(1, challenge_knowledge.NR_DRINKS + 1): next_state = "SERVE_DRINK_{}".format(idx + 1) if idx < challenge_knowledge.NR_DRINKS else "SAY_DONE" smach.StateMachine.add("SERVE_DRINK_{}".format(idx), ServeOneDrink(robot=robot, bar_designator=bar_designator, room_id=challenge_knowledge.room_id, room_designator=room_designator, objects_list_des=objects_list_des, unav_drink_des=unav_drink_des, name_options=common_knowledge.names, objects=common_knowledge.objects), transitions={"succeeded": next_state, "failed": next_state, "aborted": next_state}) smach.StateMachine.add("SAY_DONE", states.Say(robot, "My job here is done. Enjoy your day and see you next time"), transitions={"spoken": "succeeded"})
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # Create designators grasp_designator1 = ds.EdEntityDesignator(robot, type="temp") grasp_designator2 = ds.EdEntityDesignator(robot, type="temp") grasp_designator3 = ds.EdEntityDesignator(robot, type="temp") start_pose = robot.base.get_location() start_x = start_pose.frame.p.x() start_y = start_pose.frame.p.y() start_rz = start_pose.frame.M.GetRPY()[2] with self: # Start challenge via StartChallengeRobust smach.StateMachine.add("START_CHALLENGE_ROBUST", states.StartChallengeRobust(robot, STARTING_POINT, use_entry_points=True), 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': 'ANNOUNCEMENT', '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_1), radius=0.7), transitions={'arrived': 'ANNOUNCEMENT', 'unreachable': 'ANNOUNCEMENT', 'goal_not_defined': 'ANNOUNCEMENT'}) # Part I: Set a table smach.StateMachine.add('ANNOUNCEMENT', states.Say(robot, "Let's see if my master has a task for me! ", block=True), transitions={'spoken': 'FETCH_COMMAND_I'}) smach.StateMachine.add('FETCH_COMMAND_I', # Hear "set the table" HearFetchCommand(robot, 15.0, "set"), transitions={'done': 'ASK_FOR_MEAL'}) smach.StateMachine.add('ASK_FOR_MEAL', states.Say(robot, "What should I serve, master?", block=True), transitions={'spoken': 'GET_ORDER'}) smach.StateMachine.add('GET_ORDER', GetBreakfastOrder(robot, knowledge.options, grasp_designator1, grasp_designator2, grasp_designator3, timeout=15.0), transitions={'done': 'SET_THE_TABLE'}) smach.StateMachine.add('SET_THE_TABLE', # Take order and Set the table (bring the objects to the table) ManipulateMachine(robot=robot, grasp_designator1=grasp_designator1, grasp_designator2=grasp_designator2, grasp_designator3=grasp_designator3, grasp_furniture_id1=knowledge.grasp_furniture_id1, grasp_furniture_id2=knowledge.grasp_furniture_id2, place_furniture_id=knowledge.place_furniture_id), transitions={'succeeded': 'ANNOUNCE_TASK_COMPLETION', 'failed': 'RETURN_TO_START_2'}) smach.StateMachine.add('ANNOUNCE_TASK_COMPLETION', states.Say(robot, "The table is set! Moving to the meeting point for the next task.", block=False), transitions={'spoken': 'RETURN_TO_START_2'}) # Part II: Clean the table smach.StateMachine.add('RETURN_TO_START_2', states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3), transitions={'arrived': 'FETCH_COMMAND_II', 'unreachable': 'FETCH_COMMAND_II', 'goal_not_defined': 'FETCH_COMMAND_II'}) smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table" HearFetchCommand(robot, 15.0, "clear"), transitions={'done': 'CLEAR_UP'}) smach.StateMachine.add('CLEAR_UP', # Clear the table ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.place_furniture_id, place_furniture_id1=knowledge.grasp_furniture_id1, place_furniture_id2=knowledge.grasp_furniture_id2), transitions={'succeeded': 'END_CHALLENGE', 'failed': 'END_CHALLENGE'}) # End smach.StateMachine.add('END_CHALLENGE', states.Say(robot, "I am done here"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "set_a_table")
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) # Create designators grasp_designator1 = ds.EdEntityDesignator(robot, type="temp") grasp_designator2 = ds.EdEntityDesignator(robot, type="temp") grasp_designator3 = ds.EdEntityDesignator(robot, type="temp") start_pose = robot.base.get_location() start_x = start_pose.frame.p.x() start_y = start_pose.frame.p.y() start_rz = start_pose.frame.M.GetRPY()[2] with self: # Part I: Set a table smach.StateMachine.add('ENTER_ROOM', # Enter the room states.Initialize(robot), transitions={'initialized': 'ANNOUNCEMENT', 'abort': 'Aborted'}) smach.StateMachine.add('ANNOUNCEMENT', states.Say(robot, "Let's see if my master has a task for me! ", block=True), transitions={'spoken': 'FETCH_COMMAND_I'}) smach.StateMachine.add('FETCH_COMMAND_I', # Hear "set the table" HearFetchCommand(robot, 15.0, "set"), transitions={'done': 'ASK_FOR_MEAL'}) smach.StateMachine.add('ASK_FOR_MEAL', states.Say(robot, "What should I serve, master?", block=True), transitions={'spoken': 'GET_ORDER'}) smach.StateMachine.add('GET_ORDER', GetBreakfastOrder(robot, knowledge.options, grasp_designator1, grasp_designator2, grasp_designator3, timeout=15.0), transitions={'done': 'SET_THE_TABLE'}) smach.StateMachine.add('SET_THE_TABLE', # Take order and Set the table (bring the objects to the table) ManipulateMachine(robot=robot, grasp_designator1=grasp_designator1, grasp_designator2=grasp_designator2, grasp_designator3=grasp_designator3, grasp_furniture_id1=knowledge.cupboard, grasp_furniture_id3=knowledge.cupboard, place_furniture_id=knowledge.table), transitions={'succeeded': 'ANNOUNCE_TASK_COMPLETION', 'failed': 'RETURN_TO_START_2'}) smach.StateMachine.add('ANNOUNCE_TASK_COMPLETION', states.Say(robot, "The table is set! Moving to the meeting point for the next task.", block=False), transitions={'spoken': 'RETURN_TO_START_2'}) # Part II: Clean the table smach.StateMachine.add('RETURN_TO_START_2', states.NavigateToPose(robot=robot, x=start_x, y=start_y, rz=start_rz, radius=0.3), transitions={'arrived': 'FETCH_COMMAND_II', 'unreachable': 'FETCH_COMMAND_II', 'goal_not_defined': 'FETCH_COMMAND_II'}) smach.StateMachine.add('FETCH_COMMAND_II', # Hear "clear up the table" HearFetchCommand(robot, 15.0, "clear"), transitions={'done': 'CLEAR_UP'}) smach.StateMachine.add('CLEAR_UP', # Clear the table ClearManipulateMachine(robot=robot, grasp_furniture_id=knowledge.table, place_furniture_id1=knowledge.cupboard, place_furniture_id3=knowledge.cupboard), transitions={'succeeded': 'END_CHALLENGE', 'failed': 'END_CHALLENGE'}) # End smach.StateMachine.add('END_CHALLENGE', states.Say(robot, "I am done here"), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "set_a_table")
def __init__(self, robot, attempts = 1, sleep_interval = 1): # TODO: add center_point in front of the robot and radius of the search on ds.EdEntityDesignator # human_entity = ds.EdEntityDesignator(robot, center_point=gm.PointStamped(x=1.0, frame_id="base_link"), radius=1, id="human") human_entity = ds.EdEntityDesignator(robot, type="human") ds.WaitForDesignator.__init__(self, robot, human_entity, attempts, sleep_interval)
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, 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="temp") # will be updated later on 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) self.cabinet = ds.EntityByIdDesignator( robot, id="temp") # will be updated later on self.place_entity_designator = ds.EdEntityDesignator(robot=robot, id="temp") self.arm_designator = ds.ArmDesignator(robot, {}) self.place_designator = EmptySpotDesignator( robot=robot, place_location_designator=self.place_entity_designator, arm_designator=self.arm_designator, area="temp") self.placeaction1 = PlaceSingleItem( robot=robot, place_designator=self.place_designator) self.placeaction2 = PlaceSingleItem( robot=robot, place_designator=self.place_designator) 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: "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 # The Inspect-state (INSPECT_TABLE) gathers a list of ClassificationResults for Entities on the table # These are passed to the pdf_writer 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, navigation_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" }) smach.StateMachine.add("MOVE_TO_PLACE", states.NavigateToSymbolic( robot, {self.cabinet: "in_front_of"}, self.cabinet), transitions={ 'arrived': 'PLACE_ITEM_1', 'unreachable': 'PLACE_ITEM_1', 'goal_not_defined': 'PLACE_ITEM_1' }) smach.StateMachine.add("PLACE_ITEM_1", self.placeaction1, transitions={ "succeeded": "PLACE_ITEM_2", "failed": "PLACE_ITEM_2" }) smach.StateMachine.add("PLACE_ITEM_2", self.placeaction2, transitions={ "succeeded": "succeeded", "failed": "failed" })
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 __init__(self, robot, bar_designator, room_id, room_designator, objects_list_des, unav_drink_des, name_options, objects): """ Initialization method :param robot: robot api object :param bar_designator: (EntityDesignator) in which the bar location is stored :param room_id: room ID from challenge knowledge :param room_designator: (EntityDesignator) in which the room location is stored :param objects_list_des: (VariableDesignator) in which the available drinks are stored :param unav_drink_des: (VariableDesignator) in which the unavailable drink is stored :param name_options: Names from common knowledge :param objects: Objects from common knowledge """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) # Designators arm_designator = ds.UnoccupiedArmDesignator(robot=robot, arm_properties={}, name='arm_des').lockable() drink_str_designator = ds.VariableDesignator(resolve_type=str, name='drink_str_des') drink_designator = ds.EdEntityDesignator(robot=robot, type_designator=drink_str_designator, name='drink_des') operator_name = ds.VariableDesignator(resolve_type=str, name='name_des') operator_designator = ds.VariableDesignator(resolve_type=Entity, name='operator_des') learn_check_designator = ds.VariableDesignator(initial_value=True, resolve_type=bool, name='learn_check_des') hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_2') with self: # Lock the arm_designator smach.StateMachine.add("LOCK_ARM", states.LockDesignator(arm_designator), transitions={'locked': "GET_ORDER"}) # Get order smach.StateMachine.add("GET_ORDER", GetOrder(robot=robot, operator_name=operator_name, drink_designator=drink_str_designator, available_drinks_designator=objects_list_des, unavailable_drink_designator=unav_drink_des, name_options=name_options, objects=objects, learn_check_designator=learn_check_designator.writeable, target_room_designator=room_designator), transitions={"succeeded": "INSPECT_BAR", "failed": "failed", "aborted": "aborted"}) # Inspect bar smach.StateMachine.add("INSPECT_BAR", states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of"), transitions={"done": "GRASP_DRINK", "failed": "FALLBACK_BAR"}) # Grasp drink smach.StateMachine.add("GRASP_DRINK", states.Grab(robot=robot, item=drink_designator, arm=arm_designator), transitions={"done": "FIND_OPERATOR", "failed": "FALLBACK_BAR"}) # Inspect or grasp fallback - ask for assistance smach.StateMachine.add("FALLBACK_BAR", states.Say(robot=robot, sentence=DescriptionStrDesignator("fallback_bar", drink_str_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "HANDOVER_FROM_HUMAN"}) # Handover from human fallback smach.StateMachine.add("HANDOVER_FROM_HUMAN", states.HandoverFromHuman(robot=robot, arm_designator=arm_designator, grabbed_entity_designator=drink_designator), transitions={"succeeded": "RESET_ROBOT_2", "failed": "RESET_ROBOT_2", "timeout": "RESET_ROBOT_2"}) smach.StateMachine.add("RESET_ROBOT_2", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "CHECK_LEARN_OPERATOR", 'failed': "CHECK_LEARN_OPERATOR"}) smach.StateMachine.add("CHECK_LEARN_OPERATOR", states.CheckBool(learn_check_designator), transitions={"true": "FIND_OPERATOR", "false": "GO_TO_ROOM"}) smach.StateMachine.add("GO_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "SAY_NOT_FOUND", "unreachable": "failed", "goal_not_defined": "aborted"}) # Find operator smach.StateMachine.add("FIND_OPERATOR", states.FindPersonInRoom(robot=robot, area=room_id, name=operator_name, discard_other_labels=True, found_entity_designator=operator_designator.writeable), transitions={"found": "GOTO_OPERATOR", "not_found": "SAY_NOT_FOUND"}) # Move to this person smach.StateMachine.add("GOTO_OPERATOR", states.NavigateToObserve(robot=robot, entity_designator=operator_designator), transitions={"arrived": "SAY_THE_NAME", "unreachable": "SAY_NOT_FOUND", "goal_not_defined": "SAY_NOT_FOUND"}) # Say not found smach.StateMachine.add("SAY_NOT_FOUND", states.Say(robot=robot, sentence=DescriptionStrDesignator("not_found_operator", drink_str_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "RISE_FOR_HMI_2"}) # Say the name smach.StateMachine.add("SAY_THE_NAME", states.Say(robot=robot, sentence=DescriptionStrDesignator("found_operator", drink_str_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "RISE_FOR_HMI_2"}) smach.StateMachine.add("RISE_FOR_HMI_2", states.RiseForHMI(robot=robot), transitions={"succeeded": "HAND_OVER", "failed": "HAND_OVER"}) # Hand over the drink to the operator smach.StateMachine.add("HAND_OVER", states.HandoverToHuman(robot=robot, arm_designator=arm_designator), transitions={"succeeded": "UNLOCK_ARM", "failed": "UNLOCK_ARM"}) smach.StateMachine.add("UNLOCK_ARM", states.UnlockDesignator(arm_designator), transitions={'unlocked': "RESET_ROBOT_3"}) smach.StateMachine.add("RESET_ROBOT_3", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "RETURN_TO_ROOM", 'failed': "RETURN_TO_ROOM"}) smach.StateMachine.add("RETURN_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "succeeded", "unreachable": "failed", "goal_not_defined": "aborted"})
def __init__(self, robot, operator_name, drink_designator, available_drinks_designator, unavailable_drink_designator, name_options, objects, learn_check_designator, target_room_designator): # type: (Robot, str, VariableDesignator) -> None """ Initialization method :param robot: robot api object :param operator_name: name with which the operator will be stored in image recognition module :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 name_options: Names from common knowledge :param objects: Objects from common knowledge :param learn_check_designator: (VariableDesignator) Bool flag indicating whether the operator was learnt successfully :param target_room_designator: (EdEntityDesignator) Entity specifying the target room where the operator needs to be searched for getting an order """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_3') with self: # Operator id caller_id = "operator" caller_designator = ds.EdEntityDesignator(robot=robot, id=caller_id, name="caller_des", none_resolve=True) smach.StateMachine.add("RESET_ROBOT_GET_ORDER", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "SAY_PEOPLE_WITHOUT_DRINKS", 'failed': "SAY_PEOPLE_WITHOUT_DRINKS"}) # Detect - people holding drinks and people without drinks smach.StateMachine.add("SAY_PEOPLE_WITHOUT_DRINKS", states.Say(robot=robot, sentence="Trying to find people without a drink", look_at_standing_person=True, block=True), transitions={"spoken": "FIND_PERSON_WITHOUT_DRINK"}) # TODO: Change DummyState to actual state smach.StateMachine.add("FIND_PERSON_WITHOUT_DRINK", states.SetPoseFirstFoundPersonToEntity(robot=robot, properties={'tags': ['LNotHolding', 'RNotHolding']}, strict=True, dst_entity_designator=caller_id, query_entity_designator=target_room_designator), transitions={"done": "SAY_I_HAVE_SEEN", "failed": "SAY_PEOPLE_WITHOUT_DRINKS_FAILED"}) # Detect fallback - detect waving people smach.StateMachine.add("SAY_PEOPLE_WITHOUT_DRINKS_FAILED", states.Say(robot=robot, sentence="Could not detect people without drinks", look_at_standing_person=True, block=True), transitions={"spoken": "ASK_FOR_WAVING"}) smach.StateMachine.add("ASK_FOR_WAVING", states.Say(robot=robot, sentence="Please raise your arm completely and wave, if you want me to bring you something", look_at_standing_person=True, block=True), transitions={"spoken": "WAIT_FOR_WAVING"}) # Change to WAIT_FOR_WAVING smach.StateMachine.add("WAIT_FOR_WAVING", states.SetPoseFirstFoundPersonToEntity(robot=robot, properties={'tags': ['LWave', 'RWave']}, strict=False, dst_entity_designator=caller_id, query_entity_designator=target_room_designator), transitions={"done": "SAY_I_HAVE_SEEN", "failed": "SAY_COULD_NOT_FIND_WAVING"}) # Navigate to person who wants to place an order smach.StateMachine.add("SAY_COULD_NOT_FIND_WAVING", states.Say(robot=robot, sentence="I did not find any waving person.", look_at_standing_person=True, block=True), transitions={"spoken": "ASK_STEP_IN_FRONT"}) smach.StateMachine.add("SAY_I_HAVE_SEEN", states.Say(robot=robot, sentence="Found person who might want to place an order. I will be there shortly!", look_at_standing_person=True, block=True), transitions={"spoken": "NAVIGATE_TO_PERSON"}) # Navigate to waving people smach.StateMachine.add("NAVIGATE_TO_PERSON", states.NavigateToObserve(robot=robot, entity_designator=caller_designator, radius=1.1), transitions={"arrived": "LEARN_NAME", "unreachable": "SAY_COULD_NOT_NAVIGATE", "goal_not_defined": "SAY_PEOPLE_WITHOUT_DRINKS"}) # Detect waving people fallback - ask operator in front smach.StateMachine.add("SAY_COULD_NOT_NAVIGATE", states.Say(robot=robot, sentence="Sorry! I could not navigate to you.", look_at_standing_person=True), transitions={"spoken": "ASK_STEP_IN_FRONT"}) smach.StateMachine.add("ASK_STEP_IN_FRONT", states.Say(robot=robot, sentence="Please step in front of me to give your order", look_at_standing_person=True, block=True), transitions={"spoken": "LEARN_NAME"}) # Ask operator for his name smach.StateMachine.add("LEARN_NAME", states.AskPersonName(robot=robot, person_name_des=operator_name.writeable, name_options=name_options, default_name="john", nr_tries=2), transitions={"succeeded": "LEARN_OPERATOR", "failed": "LEARN_NAME_FALLBACK", "timeout": "LEARN_NAME_FALLBACK"}) # Ask operator for his name fallback smach.StateMachine.add("LEARN_NAME_FALLBACK", states.Say(robot=robot, sentence="Sorry, I did not get your name, I'll just call you john", look_at_standing_person=True), transitions={"spoken": "LEARN_OPERATOR"}) # Learn operator smach.StateMachine.add("LEARN_OPERATOR", states.LearnPerson(robot=robot, name_designator=operator_name, nr_tries=5), transitions={"succeeded": "ASK_DRINK", "failed": "LEARN_OPERATOR_FALLBACK"}) # Learn operator fallback smach.StateMachine.add("LEARN_OPERATOR_FALLBACK", states.Say(robot=robot, sentence="I will call you by your name when I'm back", look_at_standing_person=True, block=True), transitions={"spoken": "LEARN_OPERATOR_FLAG_TOGGLE"}) smach.StateMachine.add("LEARN_OPERATOR_FLAG_TOGGLE", states.ToggleBool(learn_check_designator), transitions={"done": "ASK_DRINK"}) # Ask for preferred beverage smach.StateMachine.add("ASK_DRINK", AskDrink(robot=robot, operator_name=operator_name, drink_designator=drink_designator.writeable, available_drinks_designator=available_drinks_designator, unavailable_drink_designator=unavailable_drink_designator, objects=objects), transitions={"succeeded": "succeeded", "failed": "failed", "aborted": "aborted"})
return "succeeded" smach.StateMachine.add("SELECT_ENTITY", smach.CBState(select_entity), transitions={"succeeded": "succeeded", "no_entities": "failed"}) if __name__ == "__main__": rospy.init_node("test_furniture_inspection") # Robot _robot = get_robot_from_argv(index=1) # Test data furniture = ds.EdEntityDesignator(robot=_robot, id="desk") entity_designator = ds.VariableDesignator(resolve_type=Entity) ps = geometry_msgs.msg.PointStamped() ps.header.frame_id = "/map" ps.point.x = 2.0 ps.point.y = -1.0 ps.point.z = 1.0 user_data = smach.UserData() user_data["laser_dot"] = ps sm = InspectFurniture(robot=_robot, furniture_designator=furniture, entity_designator=entity_designator.writeable) sm.execute(user_data)
import robot_smach_states.util.designators as ds from robot_smach_states import Grab if __name__ == "__main__": parser = argparse.ArgumentParser(description="Put an imaginary object in the world model and grasp it using the " "'Grab' smach state") parser.add_argument("x", type=float, help="x-coordinate (in map) of the imaginary object") parser.add_argument("y", type=float, help="y-coordinate (in map) of the imaginary object") parser.add_argument("z", type=float, help="z-coordinate (in map) of the imaginary object") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() rospy.init_node("test_grasping") robot = get_robot(args.robot) entity_id = "test_item" pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)), frame_id="/map") robot.ed.update_entity(id=entity_id, frame_stamped=pose) item = ds.EdEntityDesignator(robot, id=entity_id) arm = ds.UnoccupiedArmDesignator(robot, {}) grab_state = Grab(robot, item, arm) grab_state.execute()
def __init__(self, robot, manipulated_items): """@param manipulated_items is VariableDesignator that will be a list of items manipulated by the robot.""" self.manipulated_items = manipulated_items smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) self.cabinet = ds.EntityByIdDesignator(robot, id=CABINET, name="pick_shelf") # self.place_shelf = ds.EntityByIdDesignator(robot, id=PLACE_SHELF, name="place_shelf") not_manipulated = lambda entity: not entity in self.manipulated_items.resolve( ) def entity_z_pos(entity): """ Checks if the entity is between the minimum and maximum grasp height :param entity: :return: """ if not entity._pose: return False return MIN_GRASP_HEIGHT < entity._pose.p.z() < MAX_GRASP_HEIGHT # select the entity closest in x direction to the robot in base_link frame def weight_function(entity): # TODO: return x coordinate of entity.center_point in base_link frame epose = entity.pose.projectToFrame( robot.robot_name + "/base_link", robot.tf_listener) # Get position in base_link p = epose.frame.p return p.x()**2 self.current_item = ds.LockingDesignator(ds.EdEntityDesignator( robot, criteriafuncs=[ not_ignored, size, not_manipulated, min_entity_height, entity_z_pos, max_width ], weight_function=weight_function, debug=False, name="item"), name="current_item") #This makes that the empty spot is resolved only once, even when the robot moves. This is important because the sort is based on distance between robot and constraint-area # self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot, self.cabinet, name="placement", area=PLACE_SHELF), name="place_position") self.place_position = ds.LockingDesignator(EmptyShelfDesignator( robot, self.cabinet, name="placement", area=PLACE_SHELF), name="place_position") self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, { 'required_trajectories': ['prepare_grasp'], 'required_goals': ['carrying_pose'], 'required_gripper_types': [arms.GripperTypes.GRASPING], 'required_arm_name': PREFERRED_ARM }, name="empty_arm_designator") self.arm_with_item_designator = ds.ArmHoldingEntityDesignator( robot, {'required_objects': [self.current_item]}, { "required_trajectories": ["prepare_place"], "required_goals": ["reset", "handover_to_human"], 'required_gripper_types': [arms.GripperTypes.GRASPING] }, name="arm_with_item_designator") # print "{0} = pick_shelf".format(self.pick_shelf) # print "{0} = current_item".format(self.current_item) # print "{0} = place_position".format(self.place_position) # print "{0} = empty_arm_designator".format(self.empty_arm_designator) # print "{0} = arm_with_item_designator".format(self.arm_with_item_designator) with self: # smach.StateMachine.add( "NAV_TO_OBSERVE_PICK_SHELF", # #states.NavigateToObserve(robot, self.pick_shelf), # states.NavigateToSymbolic(robot, {self.pick_shelf:"in_front_of", EntityByIdDesignator(robot, id=ROOM):"in"}, self.pick_shelf), # transitions={ 'arrived' :'LOOKAT_PICK_SHELF', # 'unreachable' :'LOOKAT_PICK_SHELF', # 'goal_not_defined' :'LOOKAT_PICK_SHELF'}) smach.StateMachine.add("REMOVE_ENTITIES", RemoveSegmentedEntities(robot=robot), transitions={'done': 'LOOKAT_PICK_SHELF'}) smach.StateMachine.add("LOOKAT_PICK_SHELF", states.LookAtArea(robot, self.cabinet, area=PICK_SHELF), transitions={'succeeded': 'SEGMENT_SHELF'}) smach.StateMachine.add("SEGMENT_SHELF", SegmentShelf(robot, entity_id=CABINET, area_id=PICK_SHELF), transitions={'done': 'LOCK_ITEM'}) @smach.cb_interface(outcomes=['locked']) def lock(userdata=None): self.current_item.lock( ) #This determines that self.current_item cannot not resolve to a new value until it is unlocked again. if self.current_item.resolve(): rospy.loginfo("Current_item is now locked to {0}".format( self.current_item.resolve().id)) self.place_position.lock( ) #This determines that self.place_position will lock/cache its result after its resolved the first time. return 'locked' smach.StateMachine.add('LOCK_ITEM', smach.CBState(lock), transitions={'locked': 'ANNOUNCE_ITEM'}) smach.StateMachine.add("ANNOUNCE_ITEM", states.Say(robot, EntityDescriptionDesignator( self.current_item, name="current_item_desc"), block=False), transitions={'spoken': 'GRAB_ITEM'}) smach.StateMachine.add("GRAB_ITEM", Grab(robot, self.current_item, self.empty_arm_designator), transitions={ 'done': 'STORE_ITEM', 'failed': 'SAY_GRAB_FAILED' }) smach.StateMachine.add( "SAY_GRAB_FAILED", states.Say(robot, ["I couldn't grab this thing"], mood="sad"), transitions={'spoken': 'UNLOCK_ITEM_AFTER_FAILED_GRAB'} ) # Not sure whether to fail or keep looping with NAV_TO_OBSERVE_PICK_SHELF @smach.cb_interface(outcomes=['unlocked']) def unlock_and_ignore(userdata=None): global ignore_ids # import ipdb; ipdb.set_trace() if self.current_item.resolve(): ignore_ids += [self.current_item.resolve().id] rospy.loginfo("Current_item WAS now locked to {0}".format( self.current_item.resolve().id)) self.current_item.unlock( ) #This determines that self.current_item can now resolve to a new value on the next call self.place_position.unlock( ) #This determines that self.place_position can now resolve to a new position on the next call return 'unlocked' smach.StateMachine.add('UNLOCK_ITEM_AFTER_FAILED_GRAB', smach.CBState(unlock_and_ignore), transitions={'unlocked': 'failed'}) @smach.cb_interface(outcomes=['stored']) def store_as_manipulated(userdata=None): # manipulated_items.current += [self.current_item.current] item_list = manipulated_items.resolve() item_list += [self.current_item.resolve()] w = ds.VariableWriter(manipulated_items) w.write(item_list) return 'stored' smach.StateMachine.add( 'STORE_ITEM', smach.CBState(store_as_manipulated), transitions={'stored': 'LOOKAT_PLACE_SHELF'}) smach.StateMachine.add("LOOKAT_PLACE_SHELF", states.LookAtArea(robot, self.cabinet, area=PLACE_SHELF), transitions={'succeeded': 'PLACE_ITEM'}) smach.StateMachine.add("PLACE_ITEM", Place(robot, self.current_item, self.place_position, self.arm_with_item_designator), transitions={ 'done': 'RESET_HEAD_PLACE', 'failed': 'RESET_HEAD_HUMAN' }) smach.StateMachine.add( "RESET_HEAD_PLACE", states.CancelHead(robot), transitions={'done': 'UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE'}) smach.StateMachine.add( "RESET_HEAD_HUMAN", states.CancelHead(robot), transitions={'done': 'SAY_HANDOVER_TO_HUMAN'}) smach.StateMachine.add('UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE', smach.CBState(unlock_and_ignore), transitions={'unlocked': 'succeeded'}) smach.StateMachine.add( "SAY_HANDOVER_TO_HUMAN", states.Say(robot, [ "I'm can't get rid of this item myself, can somebody help me maybe?" ]), transitions={'spoken': 'HANDOVER_TO_HUMAN'}) smach.StateMachine.add('HANDOVER_TO_HUMAN', states.HandoverToHuman( robot, self.arm_with_item_designator), transitions={ 'succeeded': 'UNLOCK_AFTER_HANDOVER', 'failed': 'UNLOCK_AFTER_HANDOVER' }) smach.StateMachine.add('UNLOCK_AFTER_HANDOVER', smach.CBState(unlock_and_ignore), transitions={'unlocked': 'failed'})
def __init__(self, robot, selected_entity_designator, room_des): smach.StateMachine.__init__(self, outcomes=['done', 'failed']) store_entity_id_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id") store_entity_des = ds.EdEntityDesignator( robot, id_designator=store_entity_id_des) selected_entity_type_des = ds.AttrDesignator( selected_entity_designator, "type", resolve_type=str) store_area_name_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id") trash_place_pose = DropPoseDesignator(robot, store_entity_des, 0.6, "drop_pose") category_des = ds.VariableDesignator(resolve_type=str, name="category_des") with self: smach.StateMachine.add( "SPEAK", Say(robot, ["I will pick-up the {object}", "Let's move the {object}"], object=selected_entity_type_des, block=True), transitions={"spoken": "GRAB"}) smach.StateMachine.add("GRAB", Grab( robot, selected_entity_designator, ds.UnoccupiedArmDesignator( robot, arm_properties={ "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] }, name="empty_arm_designator")), transitions={ "done": "SAY_GRAB_SUCCESS", "failed": "ARM_RESET" }) smach.StateMachine.add( "ARM_RESET", ArmToJointConfig( robot, ds.UnoccupiedArmDesignator( robot, arm_properties={"required_goals": ["reset"]}, name="empty_arm_designator"), "reset"), transitions={ "succeeded": "SAY_GRAB_FAILED", "failed": "SAY_GRAB_FAILED" }) smach.StateMachine.add( 'SAY_GRAB_SUCCESS', Say(robot, [ "Now I am going to move this item", "Let's clean up this object", "Away with this one", "Everything will be cleaned" ], block=False), transitions={"spoken": "GET_CATEGORY"}) smach.StateMachine.add( 'SAY_GRAB_FAILED', 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": "GET_CATEGORY", "no": "done" }) # # ROBOT # smach.StateMachine.add('GET_CATEGORY', # EntityToCategory(robot, selected_entity_designator, category_des.writeable), # transitions={"done": "DETERMINE_PLACE_LOCATION", # "failed": "NAVIGATE_TO_TRASH"}) # OPERATOR smach.StateMachine.add('GET_CATEGORY', OperatorToCategory(robot, category_des.writeable, room_des), transitions={ "done": "DETERMINE_PLACE_LOCATION", "failed": "NAVIGATE_TO_TRASH" }) smach.StateMachine.add( 'DETERMINE_PLACE_LOCATION', CategoryToLocation(category_des, store_entity_id_des.writeable, store_area_name_des.writeable), transitions={ "trashbin": "INSPECT_TRASH", "other": "PLACE_TO_STORE", "failed": "NAVIGATE_TO_TRASH" }) smach.StateMachine.add('NAVIGATE_TO_TRASH', NavigateToPlace( robot, trash_place_pose, ds.OccupiedArmDesignator( robot, {}, name="occupied_arm_designator")), transitions={ "arrived": "PLACE_IN_TRASH", "unreachable": "SAY_PLACE_FAILED", "goal_not_defined": "SAY_PLACE_FAILED" }) smach.StateMachine.add('INSPECT_TRASH', Inspect(robot, store_entity_des), transitions={ "done": "PLACE_IN_TRASH", "failed": "SAY_PLACE_FAILED" }) arm_properties_place = { "required_trajectories": ["prepare_place"], "required_gripper_types": [arms.GripperTypes.GRASPING] } arm_designator_place = ds.OccupiedArmDesignator( robot, arm_properties_place, name="occupied_arm_designator") smach.StateMachine.add('PLACE_IN_TRASH', Place(robot, selected_entity_designator, trash_place_pose, arm_designator_place), transitions={ "done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED" }) arm_designator_place_store = ds.OccupiedArmDesignator( robot, arm_properties_place, name="occupied_arm_designator") smach.StateMachine.add('PLACE_TO_STORE', Place(robot, selected_entity_designator, store_entity_des, arm_designator_place_store, "on_top_of"), transitions={ "done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED" }) smach.StateMachine.add( 'SAY_PLACE_SUCCESS', Say(robot, [ "Bye bye!", "Yeah!", "Successfully disposed the item", "Another score for {}".format(robot.robot_name) ], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"}) smach.StateMachine.add( 'SAY_PLACE_FAILED', 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 single_item(robot, results_writer, cls, support, waypoint, inspect_from_area=None, non_strict_class=False, search_area='on_top_of'): """ Benchmark grasping for a single item. Outputs a record dictionary :param robot: an instance of Robot :param results_writer: a csv.DictWriter to which the output record is written :param cls: class/type of item to grab :param support: ID of the entity supporting the item-to-grab :param waypoint: From where should the robot start the grasp :param inspect_from_area: Which area of the support-entity should the robot be in to start the inspection :param non_strict_class: If set to True, the robot is not strict about the type of item it grabs, eg. it continues grasping with another type of object :param search_area: which area of the support-entity to search/inspect for an item of the given class :return: a dict with the benchmark result """ grasp_cls = ds.Designator(cls, name='grasp_cls') support_entity = ds.EdEntityDesignator(robot, id=support, name='support_entity') entity_ids = ds.VariableDesignator([], resolve_type=[ClassificationResult], name='entity_ids') waypoint_des = ds.EdEntityDesignator(robot, id=waypoint, name='waypoint') arm = ds.LockingDesignator( ds.UnoccupiedArmDesignator(robot, name='unoccupied-arm', arm_properties={ "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] })) arm.lock() record = { 'robot': robot.robot_name, 'start_waypoint': waypoint, 'expected_class': cls, 'id': None, 'timestamp': datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S') } try: say_announce = Say( robot, sentence="Please put a {cls} {search_area} the {support}".format( cls=cls, support=support, search_area=search_area)) say_announce.execute() nav_to_start = NavigateToWaypoint(robot, waypoint_designator=waypoint_des, look_at_designator=support_entity) assert nav_to_start.execute( ) == 'arrived', "I did not arrive at the start" inspect = Inspect(robot=robot, entityDes=support_entity, objectIDsDes=entity_ids, navigation_area=inspect_from_area, searchArea=search_area) record['inspect_start'] = time.time() assert inspect.execute( ) == 'done', "I could not inspect the support entity" record['inspect_end'] = time.time() record['inspect_duration'] = '{:3.3f}'.format(record['inspect_end'] - record['inspect_start']) inspection_result = entity_ids.resolve( ) # type: List[ClassificationResult] if inspection_result: if grasp_cls.resolve() in ANY_OPTIONS or non_strict_class: rospy.loginfo("Any item will match") matching_results = inspection_result else: matching_results = [ result for result in inspection_result if result.type == grasp_cls.resolve() ] rospy.loginfo("Found {} items of class {}".format( len(matching_results), grasp_cls.resolve())) if matching_results: if len(matching_results) > 1: rospy.logwarn( "There are multiple items OK to grab, will select the last one" ) record['observed_class'] = ' '.join( [result.type for result in matching_results]) else: record['observed_class'] = matching_results[-1].type selected_entity_id = matching_results[-1].id rospy.loginfo("Selected entity {} for grasping".format( selected_entity_id)) grasp_entity = ds.EdEntityDesignator(robot, id=selected_entity_id) record['id'] = selected_entity_id[:6] entity = grasp_entity.resolve() # type: Entity if entity: vector_stamped = entity.pose.extractVectorStamped( ) # type: VectorStamped record['x'] = '{:.3f}'.format(vector_stamped.vector.x()) record['y'] = '{:.3f}'.format(vector_stamped.vector.y()) record['z'] = '{:.3f}'.format(vector_stamped.vector.z()) grab_state = Grab(robot, grasp_entity, arm) record['grab_start'] = time.time() assert grab_state.execute() == 'done', "I couldn't grasp" record['grab_end'] = time.time() record['grab_duration'] = '{:3.3f}'.format( record['grab_end'] - record['grab_start']) assert nav_to_start.execute( ) == 'arrived', "I could not navigate back to the start" rospy.logwarn("Robot will turn around to drop the {}".format( grasp_cls.resolve())) force_drive = ForceDrive(robot, 0, 0, 1, 3.14) # rotate 180 degs in pi seconds force_drive.execute() say_drop = Say( robot, sentence="I'm going to drop the item, please hold it!") say_drop.execute() drop_it = SetGripper(robot, arm_designator=arm, grab_entity_designator=grasp_entity) drop_it.execute() force_drive_back = ForceDrive( robot, 0, 0, -1, 3.14) # rotate -180 degs in pi seconds force_drive_back.execute() nav_to_start.execute() else: raise AssertionError("No {} found".format(grasp_cls.resolve())) else: rospy.logerr("No entities found at all :-(") except AssertionError as assertion_err: say_fail = Say(robot, sentence=assertion_err.message + ", sorry") say_fail.execute() finally: results_writer.writerow(record) return record