def __init__(self, robot, segmented_entity_ids_designator, entity_to_inspect_designator, segmentation_area="on_top_of", threshold=0.0): """ Constructor :param robot: robot object :param segmented_entity_ids_designator: designator that is used to store the segmented objects :param entity_to_inspect_designator: EdEntityDesignator indicating the (furniture) object to inspect :param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of :param threshold: float for classification score. Entities whose classification score is lower are ignored (i.e. are not added to the segmented_entity_ids_designator) """ smach.State.__init__(self, outcomes=["done"]) self.robot = robot self.threshold = threshold ds.check_resolve_type(entity_to_inspect_designator, Entity) self.entity_to_inspect_designator = entity_to_inspect_designator if isinstance(segmentation_area, str): self.segmentation_area_des = ds.VariableDesignator(segmentation_area) else: # ds.check_resolve_type(segmentation_area, "str") self.segmentation_area_des = segmentation_area ds.check_resolve_type(segmented_entity_ids_designator, [ClassificationResult]) ds.is_writeable(segmented_entity_ids_designator) self.segmented_entity_ids_designator = segmented_entity_ids_designator
def __init__(self, counter, limit): smach.State.__init__(self, outcomes=['counted', 'limit_reached']) self.limit = limit ds.check_resolve_type(counter, int) ds.is_writeable(counter) self.counter = counter
def __init__(self, robot, segmented_entity_ids_designator, entity_to_inspect_designator, segmentation_area="on_top_of"): """ Constructor :param robot: robot object :param segmented_entity_ids_designator: designator that is used to store the segmented objects :param entity_to_inspect_designator: EdEntityDesignator indicating the (furniture) object to inspect :param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of """ smach.State.__init__(self, outcomes=["done"]) self.robot = robot ds.check_resolve_type(entity_to_inspect_designator, Entity) self.entity_to_inspect_designator = entity_to_inspect_designator if isinstance(segmentation_area, str): self.segmentation_area_des = ds.VariableDesignator( segmentation_area) else: # ds.check_resolve_type(segmentation_area, "str") self.segmentation_area_des = segmentation_area ds.check_resolve_type(segmented_entity_ids_designator, [ClassificationResult]) ds.is_writeable(segmented_entity_ids_designator) self.segmented_entity_ids_designator = segmented_entity_ids_designator
def __init__(self, check_designator): """ :param check_designator: boolean designator to be toggled """ super(ToggleBool, self).__init__(outcomes=["done"]) ds.is_writeable(check_designator) ds.check_type(check_designator, bool) self._check_designator = check_designator
def __init__(self, unavailable_drink_list_designator, drink_to_add_designator): super(UpdateUnavailableDrinkList, self).__init__(outcomes=["done", "failed"]) ds.is_writeable(unavailable_drink_list_designator) ds.check_type(unavailable_drink_list_designator, [str]) ds.check_type(drink_to_add_designator, str) self.unavailable_drink_list_designator = unavailable_drink_list_designator self.drink_to_add_designator = drink_to_add_designator
def __init__(self, results_designator, item_designator): smach.State.__init__(self, outcomes=['done']) ds.is_writeable(results_designator) ds.check_resolve_type(results_designator, dict) self.results = results_designator ds.check_resolve_type(item_designator, str) self.item = item_designator
def __init__(self, objects, classification_list_designator, unavailable_drink_designator, max_unavailable_drinks): # TODO: Change unavailable_drink_designator resolve type to list to make the implementation more generalized as # this state becomes a bug when max_unavailable_drinks > 1 super(IdentifyUnavailableDrinkFromRecognitions, self).__init__(outcomes=["done", "failed"]) ds.is_writeable(unavailable_drink_designator) ds.check_type(unavailable_drink_designator, str) ds.check_type(classification_list_designator, [ClassificationResult]) self._drinks_list = [obj["name"] for obj in objects if obj["category"] == "drink"] self._classification_list_designator = classification_list_designator self._unavailable_drink_designator = unavailable_drink_designator self._max_unavailable_drinks = max_unavailable_drinks
def __init__(self, robot, segmented_entity_ids_designator, entity_to_inspect_designator, segmentation_area="on_top_of"): smach.State.__init__(self, outcomes=["done"]) self.robot = robot ds.check_resolve_type(entity_to_inspect_designator, EntityInfo) self.entity_to_inspect_designator = entity_to_inspect_designator self.segmentation_area = segmentation_area ds.check_resolve_type(segmented_entity_ids_designator, [ClassificationResult]) ds.is_writeable(segmented_entity_ids_designator) self.segmented_entity_ids_designator = segmented_entity_ids_designator
def __init__(self, collection_des, element_des): """Iterate over a designator that resolves to a collection. The collection is resolved on the fist time the state is called AND after is was exhausted. Once the collection is exhausted, the outcome will be 'stop_iteration'. The next time the state is executed, the collection will be resolved again :param collection_des: Designator with a iterable resolve_type :param element_des: Writeable designator with a resolve_type that should match the element type of the collection >>> collection_des = ds.Designator(['a', 'b', 'c']) >>> element_des = ds.VariableDesignator(resolve_type=str) >>> >>> iterator = IterateDesignator(collection_des, element_des.writeable) >>> >>> assert iterator.execute() == 'next' >>> assert element_des.resolve() == 'a' >>> >>> assert iterator.execute() == 'next' >>> assert element_des.resolve() == 'b' >>> >>> assert iterator.execute() == 'next' >>> assert element_des.resolve() == 'c' >>> >>> assert iterator.execute() == 'stop_iteration' >>> assert element_des.resolve() == 'c' >>> >>> assert iterator.execute() == 'next' >>> assert element_des.resolve() == 'a' >>> >>> assert iterator.execute() == 'next' >>> assert element_des.resolve() == 'b' >>> >>> assert iterator.execute() == 'next' >>> assert element_des.resolve() == 'c' >>> >>> assert iterator.execute() == 'stop_iteration' """ smach.State.__init__(self, outcomes=['next', 'stop_iteration']) is_writeable(element_des) assert hasattr(collection_des, 'resolve'), "collection_des should have attribute 'resolve'" assert hasattr(collection_des.resolve_type, '__iter__'), "collection_des should resolve to an interable type" assert collection_des.resolve_type[0] == element_des.resolve_type, "Resolve type of collection and element" \ "don't match" self.collection_des = collection_des self.element_des = element_des self._current_elements = None
def __init__(self, robot, room, found_people_designator, look_range=(-np.pi/2, np.pi/2), look_steps=8): """ Constructor :param robot: robot object :param area: (str) if a waypoint "<area>_waypoint" is present in the world model, the robot will navigate to this waypoint. Else, it will navigate to the room called "<area>" :param name: (str) Name of the person to look for :param discard_other_labels: (bool) Whether or not to discard faces based on label :param found_person_designator: (Designator) A designator that will resolve to the found object """ smach.StateMachine.__init__(self, outcomes=["found", "not_found"]) waypoint_designator = ds.EntityByIdDesignator(robot=robot, id=room + "_waypoint") room_designator = ds.EntityByIdDesignator(robot=robot, id=room) ds.check_type(found_people_designator, [Entity]) ds.is_writeable(found_people_designator) with self: smach.StateMachine.add("DECIDE_NAVIGATE_STATE", _DecideNavigateState(robot=robot, waypoint_designator=waypoint_designator, room_designator=room_designator), transitions={"waypoint": "NAVIGATE_TO_WAYPOINT", "room": "NAVIGATE_TO_ROOM", "none": "not_found"}) smach.StateMachine.add("NAVIGATE_TO_WAYPOINT", states.NavigateToWaypoint(robot=robot, waypoint_designator=waypoint_designator, radius=0.15), transitions={"arrived": "FIND_PEOPLE", "unreachable": "not_found", "goal_not_defined": "not_found"}) smach.StateMachine.add("NAVIGATE_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "FIND_PEOPLE", "unreachable": "not_found", "goal_not_defined": "not_found"}) # Wait for the operator to appear and detect what he's pointing at smach.StateMachine.add("FIND_PEOPLE", FindPeople(robot=robot, query_entity_designator=room_designator, found_people_designator=found_people_designator, speak=True, look_range=look_range, look_steps=look_steps), transitions={"found": "found", "failed": "not_found"})
def __init__(self, knowledge, room_des, cleanup_locations): """ Determine cleaning locations on runtime :param knowledge: challenge knowledge :param room_des: Designator resolving to room :type room_des: Designator(str) :param cleanup_locations: Writable designator, which will be filled with a list of cleaning locations """ smach.State.__init__(self, outcomes=["done"]) ds.check_type(room_des, str) ds.is_writeable(cleanup_locations) assert (hasattr(knowledge, "cleaning_locations")) self._knowledge = knowledge self._room_des = room_des self._cleanup_locations = cleanup_locations
def __init__(self, robot, spec_designator, speech_result_designator, timeout=10, look_at_standing_person=True): smach.State.__init__(self, outcomes=["heard", "no_result"]) self.robot = robot ds.check_resolve_type(spec_designator, str) ds.check_resolve_type(speech_result_designator, HMIResult) ds.is_writeable(speech_result_designator) self.spec_designator = spec_designator self.speech_result_designator = speech_result_designator self.timeout = timeout self.look_at_standing_person = look_at_standing_person
def __init__(self, robot, spec_designator, choices_designator, speech_result_designator, time_out=rospy.Duration(10), look_at_standing_person=True): smach.State.__init__(self, outcomes=["heard", "no_result"]) self.robot = robot ds.check_resolve_type(spec_designator, str) ds.check_resolve_type(choices_designator, dict) ds.check_resolve_type(speech_result_designator, GetSpeechResponse) ds.is_writeable(speech_result_designator) self.spec_designator = spec_designator self.choices_designator = choices_designator self.speech_result_designator = speech_result_designator self.time_out = time_out self.look_at_standing_person = look_at_standing_person
def __init__(self, robot, segmented_entity_ids_designator, entity_to_inspect_designator, segmentation_area="on_top_of", unknown_threshold=0.0, filter_threshold=0.0): """ Constructor :param robot: robot object :param segmented_entity_ids_designator: designator that is used to store the segmented objects :param entity_to_inspect_designator: EdEntityDesignator indicating the (furniture) object to inspect :param segmentation_area: string defining where the objects are w.r.t. the entity, default = on_top_of :param unknown_threshold: Entities whose classification score is lower than this float are not marked with a type :param filter_threshold: Entities whose classification score is lower than this float are ignored (i.e. are not added to the segmented_entity_ids_designator) """ smach.State.__init__(self, outcomes=["done"]) self.robot = robot self.unknown_threshold = unknown_threshold self.filter_threshold = filter_threshold ds.check_resolve_type(entity_to_inspect_designator, Entity) self.entity_to_inspect_designator = entity_to_inspect_designator ds.check_type(segmentation_area, str) if isinstance(segmentation_area, str): self.segmentation_area_des = ds.VariableDesignator( segmentation_area) elif isinstance(segmentation_area, ds.Designator): self.segmentation_area_des = segmentation_area else: raise RuntimeError( "This shouldn't happen. Wrong types should have raised an exception earlier" ) ds.check_resolve_type(segmented_entity_ids_designator, [ClassificationResult]) ds.is_writeable(segmented_entity_ids_designator) self.segmented_entity_ids_designator = segmented_entity_ids_designator
def __init__(self, robot, properties=None, query_entity_designator=None, found_people_designator=None, look_distance=10.0, speak=False, strict=True, nearest=False, attempts=1, search_timeout=60, look_range=(-np.pi/2, np.pi/2), look_steps=8): """ Initialization method :param robot: robot api object :param properties: (dict) (default: None) keyvalue pair of the properties a person must possess. None as a value for a property would search for all possible values of the property. :param query_entity_designator: (default: None) An entity designator to match all found people to :param found_people_designator: (default: None) A designator to write the search result to. The designator always has a list of found people written to it. :param look_distance: (float) (default: 10.0) The distance (radius) which the robot must look at :param speak: (bool) (default: False) If True, the robot will speak while trying to find a named person :param strict: (bool) (default: True) Only used if properties is not None AND the {key:value} pair of a property has non None values. If set to True then only people with all specified properties are returned, else all people with at least one true property. Example: properties = {'tags': ['LWave', 'RWave', 'LHolding', 'RHolding']} strict = True This will return a list of people who have the tags: 'LWave' AND 'RWave' AND 'LHolding' AND 'RHolding' strict = False This will return a list of people who have the tags: 'LWave' OR 'RWave' OR 'LHolding' OR 'RHolding' :param nearest: (bool) (default: False) If True, selects the people nearest to the robot who match the requirements posed using the properties, query_entity_designator, look distance and strict arguments :param attempts: (int) (default: 1) Max number of search attempts :param search_timeout: (float) (default: 60) maximum time the robot is allowed to search :param look_range: (tuple of size 2) (default: (-np.pi/2, np.pi/2)) from what to what head angle should the robot search :param look_steps: (int) (default: 8) How many steps does it take in that range """ smach.State.__init__(self, outcomes=['found', 'failed']) self._robot = robot self._properties = properties self._look_distance = look_distance self._speak = speak self._strict = strict self._nearest = nearest self._attempts = attempts self._search_timeout = search_timeout self._look_angles = np.linspace(look_range[0], look_range[1], look_steps) if found_people_designator: ds.is_writeable(found_people_designator) ds.check_type(found_people_designator, [Entity]) self._found_people_designator = found_people_designator if query_entity_designator: ds.check_type(query_entity_designator, Entity) self._query_entity_designator = query_entity_designator
def __init__(self, robot, designator): smach.State.__init__(self, outcomes=["done"]) self.robot = robot ds.is_writeable(designator) self.designator = designator
def __init__(self, iterable_designator, element_designator): smach.State.__init__(self, outcomes=['next', 'stop_iteration']) self.iterable_designator = iterable_designator ds.is_writeable(element_designator) self.element_designator = element_designator
def __init__(self, robot, furniture_designator): # type: (Hero, VariableDesignator) -> None StateMachine.__init__(self, outcomes=['done'], output_keys=["laser_dot"]) is_writeable(furniture_designator) def _show_view(timeout=5): rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo() robot.hmi.show_image_from_msg(rgb, timeout) return rgb, depth, depth_info @cb_interface(outcomes=['done']) def _prepare_operator(_): global OPERATOR OPERATOR = None robot.ed.reset() robot.head.reset() robot.speech.speak("Let's point, please stand in front of me!", block=False) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) robot.speech.speak( "Please point at the object you want me to hand you", block=False) # hmm, weird sentence rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=2) rospy.sleep(0.4) _show_view(timeout=1) robot.speech.speak("Three") _show_view(timeout=1) robot.speech.speak("Two") _show_view(timeout=1) robot.speech.speak("One") return 'done' @cb_interface(outcomes=['done']) def _get_operator(_): global OPERATOR def _is_operator(person): if person.position.z > 2.5: robot.speech.speak( "Please stand in my view with your full body") return False if person.position.z < 1.5: robot.speech.speak( "Please stand in my view with your full body") return False if "is_pointing" not in person.tags: robot.speech.speak("Please point with your arm stretched") return False return True while not rospy.is_shutdown() and OPERATOR is None: persons = robot.perception.detect_person_3d(*_show_view()) if persons: persons = sorted(persons, key=lambda x: x.position.z) person = persons[0] if _is_operator(person): OPERATOR = person # robot.speech.speak("I see an operator at %.2f meter in front of me" % OPERATOR.position.z) rospy.loginfo("I see an operator at %.2f meter in front of me" % OPERATOR.position.z) return 'done' @cb_interface(outcomes=['done', 'failed'], output_keys=['laser_dot']) def _get_furniture(user_data): global OPERATOR final_result = None while not rospy.is_shutdown() and final_result is None: result = None while not rospy.is_shutdown() and result is None: try: map_pose = robot.tf_listener.transformPose( "map", PoseStamped(header=Header( frame_id=OPERATOR.header.frame_id, stamp=rospy.Time.now() - rospy.Duration.from_sec(0.5)), pose=OPERATOR.pointing_pose)) result = robot.ed.ray_trace(map_pose) except Exception as e: rospy.logerr( "Could not get ray trace from closest person: {}". format(e)) rospy.sleep(1.) # result.intersection_point type: PointStamped # result.entity_id: string rospy.loginfo( "There is a ray intersection with {i} at ({p.x:.4}, {p.y:.4}, {p.z:.4})" .format(i=result.entity_id, p=result.intersection_point.point)) if result.entity_id in all_possible_furniture: final_result = result else: rospy.loginfo("{} is not furniture".format( result.entity_id)) robot.speech.speak("That's not furniture, you dummy.") rospy.sleep(3) OPERATOR = None robot.get_arm().send_joint_goal("reset") return 'failed' robot.speech.speak("You pointed at %s" % final_result.entity_id) robot.get_arm().send_joint_goal("reset") # Fill the designator and user data the furniture inspection furniture_designator.write( robot.ed.get_entity(final_result.entity_id)) user_data['laser_dot'] = result.intersection_point return 'done' with self: self.add('PREPARE_OPERATOR', CBState(_prepare_operator), transitions={'done': 'GET_OPERATOR'}) self.add('GET_OPERATOR', CBState(_get_operator), transitions={'done': 'GET_FURNITURE'}) self.add('GET_FURNITURE', CBState(_get_furniture), transitions={ 'done': 'done', 'failed': 'GET_OPERATOR' })
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 write_category(ud, des_read, des_write): # type: (object, ds.Designator, ds.Designator) -> str assert (ds.is_writeable(des_write)) assert (des_write.resolve_type == des_read.resolve_type) des_write.write(des_read.resolve()) return 'done'
def __init__(self, robot, found_person_designator, properties=None, query_entity_designator=None, look_distance=10.0, speak=False, strict=True, nearest=False, attempts=1, search_timeout=60, look_range=(-np.pi/2, np.pi/2), look_steps=8): """ Initialization method :param robot: robot api object :param found_person_designator: A designator to write the search result to. :param properties: (dict) (default: None) keyvalue pair of the properties a person must possess. None as a value for a property would search for all possible values of the property. :param query_entity_designator: (default: None) An entity designator to match all found people to :param look_distance: (float) (default: 10.0) The distance (radius) which the robot must look at :param speak: (bool) (default: False) If True, the robot will speak while trying to find a named person :param strict: (bool) (default: True) Only used if properties is not None AND the {key:value} pair of a property has non None values. If set to True then only people with all specified properties are returned, else all people with at least one true property. Example: properties = {'tags': ['LWave', 'RWave', 'LHolding', 'RHolding']} strict = True This will return a list of people who have the tags: 'LWave' AND 'RWave' AND 'LHolding' AND 'RHolding' strict = False This will return a list of people who have the tags: 'LWave' OR 'RWave' OR 'LHolding' OR 'RHolding' :param nearest: (bool) (default: False) If True, selects the people nearest to the robot who match the requirements posed using the properties, query_entity_designator, look distance and strict arguments :param attempts: (int) (default: 1) Max number of search attempts :param search_timeout: (float) (default: 60) maximum time the robot is allowed to search :param look_range: (tuple of size 2) (default: (-np.pi/2, np.pi/2)) from what to what head angle should the robot search :param look_steps: (int) (default: 8) How many steps does it take in that range """ super(FindFirstPerson, self).__init__(outcomes=["found", "failed"]) ds.is_writeable(found_person_designator) ds.check_type(found_person_designator, Entity) found_people_designator = ds.VariableDesignator(resolve_type=[Entity], name='new_people') with self: self.add("FIND_PEOPLE", FindPeople( robot=robot, properties=properties, query_entity_designator=query_entity_designator, found_people_designator=found_people_designator.writeable, look_distance=look_distance, speak=speak, strict=strict, nearest=nearest, attempts=attempts, search_timeout=search_timeout, look_range=look_range, look_steps=look_steps), transitions={ 'found': 'GET_FIRST_ITERATE', 'failed': 'failed' }) self.add("GET_FIRST_ITERATE", states.IterateDesignator(found_people_designator, found_person_designator), transitions={'next': 'found', 'stop_iteration': 'failed'})