def __init__(self, robot, furniture_designator): """ Constructor :param robot: robot object """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) with self: smach.StateMachine.add("INSPECT_FURNITURE", states.Inspect(robot, furniture_designator), transitions={ "done": "PICKUP_FROM_FURNITURE", "failed": "PICKUP_FROM_FURNITURE" }) # ToDo: move GrabSingleItem to robot smach states smach.StateMachine.add( "PICKUP_FROM_FURNITURE", # Pickup the thing standing on the furniture GrabSingleItem( robot, DefaultGrabDesignator(robot, furniture_designator, "on_top_of")), transitions={ "succeeded": "succeeded", "failed": "failed" })
def __init__(self, robot, supporting_entity_designator, arm_designator): smach.StateMachine.__init__( self, outcomes=['succeeded', 'inspect_failed', 'grasp_failed']) self._robot = robot self._supporting_entity_designator = supporting_entity_designator self._arm_designator = arm_designator self._classification_result_designator = VariableDesignator( [], resolve_type=[ClassificationResult]) self._grasp_entity_designator = VariableDesignator(resolve_type=Entity) with self: smach.StateMachine.add( 'INSPECT', states.Inspect( robot=self._robot, entityDes=self._supporting_entity_designator, objectIDsDes=self._classification_result_designator, searchArea="on_top_of", navigation_area="in_front_of"), transitions={ 'done': 'SELECT_EASIEST_GRASP_ENTITY', 'failed': 'inspect_failed' }) smach.StateMachine.add( 'SELECT_EASIEST_GRASP_ENTITY', SelectEasiestGraspEntity(robot=self._robot, classification_result_designator=self. _classification_result_designator, grasp_entity_designator=self. _grasp_entity_designator.writeable), transitions={ 'selected': 'GRAB', 'failed': 'inspect_failed' }) smach.StateMachine.add('GRAB', states.Grab( robot=self._robot, item=self._grasp_entity_designator, arm=self._arm_designator), transitions={ 'done': 'succeeded', 'failed': 'grasp_failed' })
def __init__(self, robot, where_to_count_designator, type_to_count_designator, count_designator): """ :param robot: robot object :param where_to_count_designator: Where to look for objects? :param type_to_count_designator: a VariableDesignator (resolve_type=string) that stores the type of the objects that should be counted :param count_designator: VariableDesignator(resolve_type=int).writeable() that will store the number of objects """ smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) entities = ds.VariableDesignator([], resolve_type=[ClassificationResult]) with self: smach.StateMachine.add("INSPECT_TABLE", states.Inspect( robot=robot, entityDes=where_to_count_designator, objectIDsDes=entities, searchArea="on_top_of", navigation_area="in_front_of"), transitions={ "done": "COUNT", "failed": "Aborted" }) smach.StateMachine.add( "COUNT", CountObjectsOnLocation( robot, location=where_to_count_designator, segmented_objects_designator=entities, object_type=type_to_count_designator, num_objects_designator=count_designator.writeable), transitions={ 'done': 'Done', 'failed': 'Aborted' })
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): # 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, 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, furniture_designator, entity_designator): # type: (Robot, object) -> None """ Drives to the designated furniture object, inspects this and selects the entity that will be pointed to :param robot: (Robot) robot API object :param furniture_designator: (EdEntityDesignator) designates the furniture object that was pointed to. :param entity_designator: (EdEntityDesignator) writeable EdEntityDesignator """ # ToDo: we need to add userdata smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"], input_keys=["laser_dot"]) assert ds.is_writeable(entity_designator), "Entity designator must be writeable for this purpose" object_ids_des = ds.VariableDesignator([], resolve_type=[states.ClassificationResult]) with self: smach.StateMachine.add("SAY_GO", states.Say(robot, "Let's go to the {furniture_object}", furniture_object=ds.AttrDesignator(furniture_designator, "id", resolve_type=str)), transitions={"spoken": "INSPECT_FURNITURE"}) smach.StateMachine.add("INSPECT_FURNITURE", states.Inspect(robot=robot, entityDes=furniture_designator, objectIDsDes=object_ids_des, navigation_area="in_front_of", ), transitions={"done": "SELECT_ENTITY", "failed": "SAY_INSPECTION_FAILED"}) # ToDo: fallback? smach.StateMachine.add("SAY_INSPECTION_FAILED", states.Say(robot, "I am sorry but I was not able to reach the {furniture_object}", furniture_object=ds.AttrDesignator(furniture_designator, "id", resolve_type=str)), transitions={"spoken": "failed"}) @smach.cb_interface(outcomes=["succeeded", "no_entities"], input_keys=["laser_dot"]) def select_entity(userdata): """ Selects the entity that the robot believes the operator has pointed to and that the robot will identify later on. Userdata contains key 'laser_dot' with value geometry_msgs.msg.PointStamped where the operator pointed at. :param userdata: (dict) :return: (str) outcome """ assert userdata.laser_dot.header.frame_id.endswith("map"), "Provide your laser dot in map frame" # Extract classification results entity_ids = [cr.id for cr in object_ids_des.resolve()] rospy.loginfo("Segmented entities: {}".format(entity_ids)) # Obtain all corresponding entities all_entities = robot.ed.get_entities() segmented_entities = [e for e in all_entities if e.id in entity_ids] # Filter out 'unprobable' entities candidates = [] for entity in segmented_entities: # type: Entity # The following filtering has been 'copied' from the cleanup challenge # It can be considered a first step but does not take the orientation of the convex hull into # account shape = entity.shape size_x = max(shape.x_max - shape.x_min, 0.001) size_y = max(shape.y_max - shape.y_min, 0.001) if size_x > SIZE_LIMIT or size_y > SIZE_LIMIT: continue if not 1 / min(RATIO_LIMIT, 1000) <= size_x / size_y <= min(RATIO_LIMIT, 1000): continue candidates.append(entity) # If no entities left: don't bother continuing if not candidates: rospy.logwarn("No 'probable' entities left") return "no_entities" # Select entity closest to the point where the operator pointed at (i.e., closest in 2D) closest_tuple = (None, None) x_ref = userdata.laser_dot.point.x y_ref = userdata.laser_dot.point.y # ToDo: use sorting for this... for e in candidates: # type: Entity x_e = e.pose.frame.p.x() y_e = e.pose.frame.p.y() distance_2d = math.hypot(x_ref - x_e, y_ref - y_e) rospy.loginfo("Entity {} at {}, {}: distance = {}".format(e.id, x_e, y_e, distance_2d)) if closest_tuple[0] is None or distance_2d < closest_tuple[1]: closest_tuple = (e, distance_2d) rospy.loginfo("Best entity: {} at {}".format(closest_tuple[0].id, closest_tuple[1])) entity_designator.write(closest_tuple[0]) return "succeeded" smach.StateMachine.add("SELECT_ENTITY", smach.CBState(select_entity), transitions={"succeeded": "succeeded", "no_entities": "failed"})
def __init__(self, robot, source_location, source_navArea, target_location, target_navArea, target_placeArea="on_top_of", source_searchArea="on_top_of"): """ Let the given robot move to a location and remove all entities from that table one at a time :param robot: Robot to use :param source_location: Location which will be cleared :param target_location: Location where the objects will be placed :return: """ smach.StateMachine.__init__(self, outcomes=['done', 'failed']) # Check types or designator resolve types #check_type(source_location, Entity) #check_type(target_location, Entity) segmented_entities_designator = VariableDesignator([], resolve_type=[ClassificationResult]) selected_entity_designator = EntityByIdDesignator(robot, "TBD", name='selected_entity_designator', ) arm_des = UnoccupiedArmDesignator(robot, {}).lockable() arm_des.lock() place_position = states.util.designators.EmptySpotDesignator(robot, EdEntityDesignator( robot, id=target_location.id), area="on_top_of" ) with self: smach.StateMachine.add('INSPECT_SOURCE_ENTITY', states.Inspect(robot, source_location, objectIDsDes=segmented_entities_designator, searchArea=source_searchArea, navigation_area=source_navArea), transitions={'done': 'DETERMINE_IF_CLEAR', 'failed': 'failed'} ) #smach.StateMachine.add('DETERMINE_IF_CLEAR', # isitclear(robot=robot, # objectIDsDes=segmented_entities_designator), # transitions={'clear': 'done', # 'not_clear': 'failed'}) smach.StateMachine.add('DETERMINE_IF_CLEAR', SelectEntity(robot, segmented_entities_designator, selected_entity_designator), transitions={'no_entities_left': 'done', 'entity_selected': 'GRAB'} ) smach.StateMachine.add('GRAB', states.Grab(robot, selected_entity_designator, arm_des), transitions={'done': 'INSPECT_TARGET', 'failed': 'failed'} ) smach.StateMachine.add('INSPECT_TARGET', states.Inspect(robot, target_location, searchArea=target_placeArea, navigation_area=target_navArea), transitions={'done': 'PLACE', 'failed': 'failed'} ) smach.StateMachine.add('PLACE', states.Place(robot, selected_entity_designator, place_position, arm_des), transitions={'done': 'INSPECT_SOURCE_ENTITY', 'failed': 'failed'} )
def __init__(self, robot, 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", robot_smach_states.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", robot_smach_states.Grab(robot, selected_entity_designator, ds.UnoccupiedArmDesignator(robot, {}, name="empty_arm_designator")), transitions={"done": "SAY_GRAB_SUCCESS", "failed": "ARM_RESET"}) smach.StateMachine.add("ARM_RESET", robot_smach_states.ArmToJointConfig(robot, ds.UnoccupiedArmDesignator(robot, {}, name="empty_arm_designator"), "reset"), transitions={"succeeded": "SAY_GRAB_FAILED", "failed": "SAY_GRAB_FAILED"}) smach.StateMachine.add('SAY_GRAB_SUCCESS', robot_smach_states.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', robot_smach_states.Say(robot, ["I could not grab the item.", "I failed to grasp the item", "I cannot reach the item", "Item grab failed"], block=False), transitions={"spoken": "failed"}) smach.StateMachine.add('CHECK_ARM_FREE', ArmFree(robot), transitions={"yes": "done", "no": "CHECK_ARM_OCCUPIED"}) smach.StateMachine.add('CHECK_ARM_OCCUPIED', ArmOccupied(robot), transitions={"yes": "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', robot_smach_states.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', robot_smach_states.Inspect(robot, store_entity_des), transitions={"done": "PLACE_IN_TRASH", "failed": "SAY_PLACE_FAILED"}) smach.StateMachine.add('PLACE_IN_TRASH', robot_smach_states.Place(robot, selected_entity_designator, trash_place_pose, ds.OccupiedArmDesignator(robot, {}, name="occupied_arm_designator")), transitions={"done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED"}) smach.StateMachine.add('PLACE_TO_STORE', robot_smach_states.Place(robot, selected_entity_designator, store_entity_des, ds.OccupiedArmDesignator(robot, {}, name= "occupied_arm_designator"), "on_top_of"), transitions={"done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED"}) smach.StateMachine.add('SAY_PLACE_SUCCESS', robot_smach_states.Say(robot, ["Bye bye!", "Yeah!", "Successfully disposed the item", "Another score for HERO"], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"}) smach.StateMachine.add('SAY_PLACE_FAILED', robot_smach_states.Say(robot, ["I could not cleanup the item.", "I cannot put the item in the trashbin", "Item cleanup failed"], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"})