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, designator):
smach.State.__init__(self, outcomes=["done"])
self.robot = robot
ds.is_writeable(designator)
self.designator = 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'})
