From d881ca744063115f69253764d28b1d8241787dcf Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Sun, 30 Apr 2023 11:22:12 +0100 Subject: [PATCH 1/5] start with test on find by querying people --- .../find_empty_seat2.py | 214 ++++++++++++++++++ 1 file changed, 214 insertions(+) create mode 100755 challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py diff --git a/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py b/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py new file mode 100755 index 000000000..ebf377843 --- /dev/null +++ b/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py @@ -0,0 +1,214 @@ +#! /usr/bin/env python + +from __future__ import print_function + +from typing import List + +import rospy +from ed.entity import Entity + +from pykdl_ros import VectorStamped + +from robot_smach_states.human_interaction import Say +from robot_smach_states.designator_iterator import IterateDesignator +from robot_smach_states.world_model import CheckVolumeEmpty +from robot_smach_states.reset import ResetArms +from challenge_receptionist.point_at_receptionist import PointAtReception +import robot_smach_states.util.designators as ds +import smach + + +class SeatsInRoomDesignator(ds.Designator): + def __init__(self, robot, seat_ids, room, name=None): + super(SeatsInRoomDesignator, self).__init__(resolve_type=[Entity], name=name) + + self.robot = robot + + ds.check_type(seat_ids, [str]) + ds.check_type(room, Entity) + + self.room = room + self.seat_ids = seat_ids + + def _resolve(self): + room = self.room.resolve() if hasattr(self.room, 'resolve') else self.room # type: Entity + if not room: + rospy.logwarn("Room is None, so cannot find seats there") + return None + seats = [self.robot.ed.get_entity(seat_id) for seat_id in self.seat_ids] # type: List[Entity] + + true_seats = [seat for seat in seats if seat is not None] # get_entity returns None if entity does not exist + + seats_in_room = room.entities_in_volume(true_seats, "in") + + return seats_in_room + + def __repr__(self): + return "SeatsInRoomDesignator({}, {}, {}, {})".format(self.robot, self.seat_ids, self.room, self.name) + + +class PersonInSeatDesignator(ds.Designator): + def __init__(self, robot, seat: Entity, room: Entity=None, name=None): + super(PersonInSeatDesignator, self).__init__(resolve_type=[Entity], name=name) + + self.robot = robot + + ds.check_type(seat, Entity) + if room is not None: + ds.check_type(room, Entity) + + self.room = room + self.seat = seat + + def _resolve(self) -> Entity: + + if self.room: + room = self.room.resolve() if hasattr(self.room, 'resolve') else self.room # type: Entity + if not room: + rospy.logwarn("Room is None, ignoring room constraints") + + seat = self.seat.resolve() if hasattr(self.seat, 'resolve') else self.seat # type: Entity + if not seat: + rospy.logwarn("Seat is None, so cannot find seats there") + return None + if not seat.pose: + rospy.logwarn("Seat does not have a pose") + return None + + seat_point = VectorStamped(seat.pose.frame.p, seat.pose.header.stamp, seat.pose.header.frame_id) + person_string="person" + + # query the people within 1m of the furniture: TODO adapt to the size of the furniture. i.e. couch/sofa + people: List(Entity) = self.robot.ed.get_entities(etype="person", center_point=seat_point, radius=1.0, ignore_z=True) + + #TODO ignore people not within the specified room. i.e spectators + return people + + def __repr__(self): + return "PersonInSeatDesignator({}, {}, {}, {})".format(self.robot, self.seat, self.room, self.name) + + +class FindEmptySeat(smach.StateMachine): + """ + Iterate over all seat-type objects and check that their 'on-top-of' volume is empty + That can be done with an Inspect and then query for any Entities inside that volume. + If there are none, then the seat is empty + """ + + def __init__(self, robot, seats_to_inspect: dict, room, fit_supporting_entity=False, seat_is_for=None): + smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) + + seats_volumes_des = ds.VariableDesignator(seats_to_inspect) + seats = SeatsInRoomDesignator(robot, list(seats_to_inspect.keys()), room, "seats_in_room") + seat_ent_des = ds.VariableDesignator(resolve_type=Entity) + seat_ent_uuid_des = ds.AttrDesignator(seat_ent_des, 'uuid', resolve_type=str) + volumes_des = ds.ValueByKeyDesignator(seats_volumes_des, seat_ent_uuid_des, resolve_type=[str], + name="volumes_des") + volume_des = ds.VariableDesignator(resolve_type=str) + + if seat_is_for: + ds.check_type(seat_is_for, str) + else: + seat_is_for = ds.Designator(' ') + + with self: + smach.StateMachine.add('SAY_LETS_FIND_SEAT', + Say(robot, + ["Let me find a place for {name} to sit. Please be patient while I check " + "out where there's place to sit"], + name=seat_is_for, + block=False), + transitions={'spoken': 'ITERATE_NEXT_SEAT'}) + + smach.StateMachine.add('ITERATE_NEXT_SEAT', + IterateDesignator(seats, seat_ent_des.writeable), + transitions={'next': 'ITERATE_NEXT_VOLUME', + 'stop_iteration': 'SAY_NO_EMPTY_SEATS'}) + + smach.StateMachine.add('ITERATE_NEXT_VOLUME', + IterateDesignator(volumes_des, volume_des.writeable), + transitions={'next': 'CHECK_SEAT_EMPTY', + 'stop_iteration': 'ITERATE_NEXT_SEAT'}) + + smach.StateMachine.add('CHECK_SEAT_EMPTY', + CheckVolumeEmpty(robot, seat_ent_des, volume_des, 0.6, None), + transitions={'occupied': 'ITERATE_NEXT_VOLUME', + 'empty': 'POINT_AT_EMPTY_SEAT', + 'partially_occupied': 'POINT_AT_PARTIALLY_OCCUPIED_SEAT', + 'failed': 'ITERATE_NEXT_VOLUME'}) + + smach.StateMachine.add('POINT_AT_EMPTY_SEAT', + PointAtReception(robot=robot, + arm_designator=ds.UnoccupiedArmDesignator(robot, { + 'required_goals': ['point_at']}), + point_at_designator=seat_ent_des, + volume=volume_des, + look_at_designator=seat_ent_des), + transitions={"succeeded": "SAY_SEAT_EMPTY", + "failed": "SAY_SEAT_EMPTY"}) + + smach.StateMachine.add('SAY_SEAT_EMPTY', + Say(robot, + ["Please sit on the {seat}, {name}!"], + name=seat_is_for, + seat=ds.AttrDesignator(seat_ent_des, 'uuid', resolve_type=str), + block=True), + transitions={'spoken': 'RESET_SUCCESS'}) + + smach.StateMachine.add('POINT_AT_PARTIALLY_OCCUPIED_SEAT', + PointAtReception(robot=robot, + arm_designator=ds.UnoccupiedArmDesignator(robot, { + 'required_goals': ['point_at']}), + point_at_designator=seat_ent_des, + volume=volume_des, + look_at_designator=seat_ent_des), + transitions={"succeeded": "SAY_SEAT_PARTIALLY_OCCUPIED", + "failed": "SAY_SEAT_PARTIALLY_OCCUPIED"}) + + smach.StateMachine.add('SAY_SEAT_PARTIALLY_OCCUPIED', + Say(robot, + ["I think there's some space left here where you can sit {name}," + "Please leave some space for any potential future guests!"], + name=seat_is_for, + block=True), + transitions={'spoken': 'RESET_SUCCESS'}) + + smach.StateMachine.add('SAY_NO_EMPTY_SEATS', + Say(robot, + ["Sorry, there are no empty seats. I guess you just have to stand {name}"], + name=seat_is_for, + block=True), + transitions={'spoken': 'RESET_FAIL'}) + + smach.StateMachine.add('RESET_FAIL', + ResetArms(robot), + transitions={'done': 'failed'}) + + smach.StateMachine.add('RESET_SUCCESS', + ResetArms(robot), + transitions={'done': 'succeeded'}) + + +if __name__ == "__main__": + import sys + from robot_skills import get_robot + + if len(sys.argv) < 3: + print( + "Please provide robot_name, and seat to inspect as arguments. Eg. 'hero sofa") + sys.exit(1) + + from robot_smach_states.util.designators import EntityByIdDesignator + robot_name = sys.argv[1] + seat = sys.argv[2] + + + rospy.init_node('test_find_emtpy_seat') + robot = get_robot(robot_name) + + seat_entity = EntityByIdDesignator(robot, seat) + + people_designator = PersonInSeatDesignator(robot, seat_entity, name="people_in_seat_designator") + found_people = people_designator.resolve() + + rospy.loginfo(f"Designator {people_designator} resolved to {found_people}") \ No newline at end of file From d1b9645df6e9e7389b6de63da4d0d79ca4eaea00 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Mon, 1 May 2023 14:39:16 +0100 Subject: [PATCH 2/5] move check people in seat to smach state --- .../find_empty_seat2.py | 78 +++++-------------- 1 file changed, 20 insertions(+), 58 deletions(-) diff --git a/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py b/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py index ebf377843..d06733ba5 100755 --- a/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py +++ b/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py @@ -7,12 +7,12 @@ import rospy from ed.entity import Entity -from pykdl_ros import VectorStamped - from robot_smach_states.human_interaction import Say from robot_smach_states.designator_iterator import IterateDesignator -from robot_smach_states.world_model import CheckVolumeEmpty +from robot_smach_states.util.designators import LockingDesignator +from robot_smach_states.world_model import CheckSeatEmpty from robot_smach_states.reset import ResetArms +from robot_smach_states.utility import LockDesignator from challenge_receptionist.point_at_receptionist import PointAtReception import robot_smach_states.util.designators as ds import smach @@ -47,47 +47,6 @@ def __repr__(self): return "SeatsInRoomDesignator({}, {}, {}, {})".format(self.robot, self.seat_ids, self.room, self.name) -class PersonInSeatDesignator(ds.Designator): - def __init__(self, robot, seat: Entity, room: Entity=None, name=None): - super(PersonInSeatDesignator, self).__init__(resolve_type=[Entity], name=name) - - self.robot = robot - - ds.check_type(seat, Entity) - if room is not None: - ds.check_type(room, Entity) - - self.room = room - self.seat = seat - - def _resolve(self) -> Entity: - - if self.room: - room = self.room.resolve() if hasattr(self.room, 'resolve') else self.room # type: Entity - if not room: - rospy.logwarn("Room is None, ignoring room constraints") - - seat = self.seat.resolve() if hasattr(self.seat, 'resolve') else self.seat # type: Entity - if not seat: - rospy.logwarn("Seat is None, so cannot find seats there") - return None - if not seat.pose: - rospy.logwarn("Seat does not have a pose") - return None - - seat_point = VectorStamped(seat.pose.frame.p, seat.pose.header.stamp, seat.pose.header.frame_id) - person_string="person" - - # query the people within 1m of the furniture: TODO adapt to the size of the furniture. i.e. couch/sofa - people: List(Entity) = self.robot.ed.get_entities(etype="person", center_point=seat_point, radius=1.0, ignore_z=True) - - #TODO ignore people not within the specified room. i.e spectators - return people - - def __repr__(self): - return "PersonInSeatDesignator({}, {}, {}, {})".format(self.robot, self.seat, self.room, self.name) - - class FindEmptySeat(smach.StateMachine): """ Iterate over all seat-type objects and check that their 'on-top-of' volume is empty @@ -99,7 +58,7 @@ def __init__(self, robot, seats_to_inspect: dict, room, fit_supporting_entity=Fa smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) seats_volumes_des = ds.VariableDesignator(seats_to_inspect) - seats = SeatsInRoomDesignator(robot, list(seats_to_inspect.keys()), room, "seats_in_room") + seats = LockingDesignator(SeatsInRoomDesignator(robot, list(seats_to_inspect.keys()), room, "seats_in_room")) seat_ent_des = ds.VariableDesignator(resolve_type=Entity) seat_ent_uuid_des = ds.AttrDesignator(seat_ent_des, 'uuid', resolve_type=str) volumes_des = ds.ValueByKeyDesignator(seats_volumes_des, seat_ent_uuid_des, resolve_type=[str], @@ -118,7 +77,10 @@ def __init__(self, robot, seats_to_inspect: dict, room, fit_supporting_entity=Fa "out where there's place to sit"], name=seat_is_for, block=False), - transitions={'spoken': 'ITERATE_NEXT_SEAT'}) + transitions={'spoken': 'LOCK_DESIGNATOR'}) + + smach.StateMachine.add('LOCK_DESIGNATOR', LockDesignator(seats), + transitions={'locked': 'ITERATE_NEXT_SEAT'}) smach.StateMachine.add('ITERATE_NEXT_SEAT', IterateDesignator(seats, seat_ent_des.writeable), @@ -131,7 +93,7 @@ def __init__(self, robot, seats_to_inspect: dict, room, fit_supporting_entity=Fa 'stop_iteration': 'ITERATE_NEXT_SEAT'}) smach.StateMachine.add('CHECK_SEAT_EMPTY', - CheckVolumeEmpty(robot, seat_ent_des, volume_des, 0.6, None), + CheckSeatEmpty(robot, seat_ent_des, volume_des, 0.6, None), transitions={'occupied': 'ITERATE_NEXT_VOLUME', 'empty': 'POINT_AT_EMPTY_SEAT', 'partially_occupied': 'POINT_AT_PARTIALLY_OCCUPIED_SEAT', @@ -193,22 +155,22 @@ def __init__(self, robot, seats_to_inspect: dict, room, fit_supporting_entity=Fa import sys from robot_skills import get_robot - if len(sys.argv) < 3: + if len(sys.argv) < 4: print( - "Please provide robot_name, and seat to inspect as arguments. Eg. 'hero sofa") + "Please provide robot_name, room and seats_to_inspect as arguments. Eg. 'hero livingroom dinner_table bar " + "dinnertable", ) sys.exit(1) - from robot_smach_states.util.designators import EntityByIdDesignator robot_name = sys.argv[1] - seat = sys.argv[2] + room = sys.argv[2] + seats_to_inspect = sys.argv[3:] + + seats_to_inspect = {seat: ['on_top_of'] for seat in seats_to_inspect} - rospy.init_node('test_find_emtpy_seat') robot = get_robot(robot_name) - seat_entity = EntityByIdDesignator(robot, seat) - - people_designator = PersonInSeatDesignator(robot, seat_entity, name="people_in_seat_designator") - found_people = people_designator.resolve() - - rospy.loginfo(f"Designator {people_designator} resolved to {found_people}") \ No newline at end of file + sm = FindEmptySeat(robot, + seats_to_inspect=seats_to_inspect, + room=ds.EntityByIdDesignator(robot, room)) + sm.execute() From 513055b0db07f30004a6811eb3acbc287b69225a Mon Sep 17 00:00:00 2001 From: shanki98 Date: Sat, 3 Jun 2023 14:54:08 +0200 Subject: [PATCH 3/5] Checking condition for PeopleInSeatDesignator --- .../find_empty_seat2.py | 1 + .../robot_smach_states/world_model/people.py | 19 +++++++++++-------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py b/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py index d06733ba5..04eb2832b 100755 --- a/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py +++ b/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py @@ -92,6 +92,7 @@ def __init__(self, robot, seats_to_inspect: dict, room, fit_supporting_entity=Fa transitions={'next': 'CHECK_SEAT_EMPTY', 'stop_iteration': 'ITERATE_NEXT_SEAT'}) + # ToDO: THis has to be modified once the CheckSeatEmpty state is ready! smach.StateMachine.add('CHECK_SEAT_EMPTY', CheckSeatEmpty(robot, seat_ent_des, volume_des, 0.6, None), transitions={'occupied': 'ITERATE_NEXT_VOLUME', diff --git a/robot_smach_states/src/robot_smach_states/world_model/people.py b/robot_smach_states/src/robot_smach_states/world_model/people.py index bc9ca0aae..b2fe01631 100644 --- a/robot_smach_states/src/robot_smach_states/world_model/people.py +++ b/robot_smach_states/src/robot_smach_states/world_model/people.py @@ -8,6 +8,7 @@ from ed.entity import Entity from robot_smach_states.util import designators as ds from robot_smach_states.human_interaction import FindPeople +from .world_model import Inspect class PeopleInSeatDesignator(ds.Designator): @@ -24,11 +25,6 @@ def __init__(self, robot, seat: Entity, room: Entity = None, name=None): self.seat = seat def _resolve(self) -> Entity: - if self.room: - room = ds.value_or_resolve(self.room) - if not room: - rospy.logwarn("Room is None, ignoring room constraints") - seat = ds.value_or_resolve(self.seat) if not seat: rospy.logwarn("Seat is None, so cannot find seats there") @@ -43,9 +39,13 @@ def _resolve(self) -> Entity: people: List[Entity] = self.robot.ed.get_entities( etype="person", center_point=seat_point, radius=1.0, ignore_z=True ) + if self.room: + room = ds.value_or_resolve(self.room) + check_entity = room.resolve() + people = [person for person in people if check_entity.in_volume(VectorStamped.from_framestamped(person.pose),'in')] + else: + rospy.logwarn("Room is None, ignoring room constraints") - # TODO ignore people not within the specified room. i.e spectators - people = [person for person in people if person.pose.frame.p] return people def __repr__(self): @@ -102,7 +102,11 @@ def __init__(self, robot, entity_des, number_of_people_des=None): people_in_seat_des = PeopleInSeatDesignator(robot, entity_des) with self: + # TODO add state to turn head towards the entity. + smach.StateMachine.add( + "LOOK_AT_SEAT", + ) smach.StateMachine.add( "INSPECT_SEAT", FindPeople( @@ -115,7 +119,6 @@ def __init__(self, robot, entity_des, number_of_people_des=None): # strict=True, # nearest=False, attempts=1, - # search_timeout=60, look_range=(0.0, 0.0), look_steps=1, ), From 652f5681b7ecbb11c71c1793410d39e3ed8bb42b Mon Sep 17 00:00:00 2001 From: shanki98 Date: Sat, 3 Jun 2023 15:10:26 +0200 Subject: [PATCH 4/5] Fix Checking condition for PeopleInSeatDesignator --- .../src/robot_smach_states/world_model/people.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/robot_smach_states/src/robot_smach_states/world_model/people.py b/robot_smach_states/src/robot_smach_states/world_model/people.py index b2fe01631..6629f2af8 100644 --- a/robot_smach_states/src/robot_smach_states/world_model/people.py +++ b/robot_smach_states/src/robot_smach_states/world_model/people.py @@ -41,12 +41,11 @@ def _resolve(self) -> Entity: ) if self.room: room = ds.value_or_resolve(self.room) - check_entity = room.resolve() - people = [person for person in people if check_entity.in_volume(VectorStamped.from_framestamped(person.pose),'in')] - else: - rospy.logwarn("Room is None, ignoring room constraints") - - return people + if not room: + rospy.logwarn("Room is None, ignoring room constraints") + return people + people = [person for person in people if room.in_volume(VectorStamped.from_framestamped(person.pose),'in')] + return people def __repr__(self): return "PersonInSeatDesignator({}, {}, {}, {})".format(self.robot, self.seat, self.room, self.name) From f1d0a1c7a4ed6fa117a11cd8ee268c6260e7ec04 Mon Sep 17 00:00:00 2001 From: shanki98 Date: Sat, 3 Jun 2023 16:23:04 +0200 Subject: [PATCH 5/5] Added LookAtEntity to look at person --- .../find_empty_seat2.py | 2 +- .../robot_smach_states/world_model/people.py | 75 ++++++++++++++----- 2 files changed, 56 insertions(+), 21 deletions(-) diff --git a/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py b/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py index 04eb2832b..bd399d7a8 100755 --- a/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py +++ b/challenge_receptionist/src/challenge_receptionist/find_empty_seat2.py @@ -94,7 +94,7 @@ def __init__(self, robot, seats_to_inspect: dict, room, fit_supporting_entity=Fa # ToDO: THis has to be modified once the CheckSeatEmpty state is ready! smach.StateMachine.add('CHECK_SEAT_EMPTY', - CheckSeatEmpty(robot, seat_ent_des, volume_des, 0.6, None), + CheckSeatEmpty(robot, seat_ent_des, volume_des, None), transitions={'occupied': 'ITERATE_NEXT_VOLUME', 'empty': 'POINT_AT_EMPTY_SEAT', 'partially_occupied': 'POINT_AT_PARTIALLY_OCCUPIED_SEAT', diff --git a/robot_smach_states/src/robot_smach_states/world_model/people.py b/robot_smach_states/src/robot_smach_states/world_model/people.py index 6629f2af8..02bcf452f 100644 --- a/robot_smach_states/src/robot_smach_states/world_model/people.py +++ b/robot_smach_states/src/robot_smach_states/world_model/people.py @@ -1,3 +1,6 @@ +# System +import time + from typing import List import rospy @@ -8,20 +11,50 @@ from ed.entity import Entity from robot_smach_states.util import designators as ds from robot_smach_states.human_interaction import FindPeople -from .world_model import Inspect +from .world_model import look_at_segmentation_area + + +class LookAtEntity(smach.State): + """ Look at an entity. This assumes the robot is already in front of the object """ + + def __init__(self, robot, entity, volume=None): + """ Constructor + + :param robot: robot object + :param entity: Ed Entity indicating the entity to look at + :param volume: Volume string indicating the specific volume to look at (e.g., 'on_top_of') + """ + smach.State.__init__(self, outcomes=["done"]) + self.robot = robot + ds.check_type(entity, Entity) + self.entity = ds.value_or_resolve(entity) + if volume is not None: + ds.check_type(volume, Entity) + self.volume = ds.value_or_resolve(volume) + + def execute(self): + """ Looks at the entity and updates its pose using the update kinect service """ + if self.volume: + look_at_segmentation_area(self.robot, self.entity, self.volume) + else: + look_at_segmentation_area(self.robot, self.entity) + # This is needed because the head is not entirely still when the look_at_point function finishes + time.sleep(0.5) + # Return + return "done" class PeopleInSeatDesignator(ds.Designator): - def __init__(self, robot, seat: Entity, room: Entity = None, name=None): + def __init__(self, robot, seat: Entity, volume: Entity = None, name=None): super(PeopleInSeatDesignator, self).__init__(resolve_type=[Entity], name=name) self.robot = robot ds.check_type(seat, Entity) - if room is not None: - ds.check_type(room, Entity) + if volume is not None: + ds.check_type(volume, Entity) - self.room = room + self.volume = volume self.seat = seat def _resolve(self) -> Entity: @@ -39,16 +72,17 @@ def _resolve(self) -> Entity: people: List[Entity] = self.robot.ed.get_entities( etype="person", center_point=seat_point, radius=1.0, ignore_z=True ) - if self.room: - room = ds.value_or_resolve(self.room) - if not room: - rospy.logwarn("Room is None, ignoring room constraints") + if self.volume: + volume = ds.value_or_resolve(self.volume) + if not volume: + rospy.logwarn("Volume is None, ignoring volume constraints") return people - people = [person for person in people if room.in_volume(VectorStamped.from_framestamped(person.pose),'in')] + people = [person for person in people if + volume.in_volume(VectorStamped.from_framestamped(person.pose), 'in')] return people def __repr__(self): - return "PersonInSeatDesignator({}, {}, {}, {})".format(self.robot, self.seat, self.room, self.name) + return "PersonInSeatDesignator({}, {}, {}, {})".format(self.robot, self.seat, self.volume, self.name) class _CheckPeople(smach.State): @@ -82,12 +116,13 @@ def execute(self, ud=None): class CheckSeatEmpty(smach.StateMachine): - def __init__(self, robot, entity_des, number_of_people_des=None): + def __init__(self, robot, seat_ent_des, volume_des, number_of_people_des=None): """ Constructor :param robot: robot object - :param entity_des: EdEntityDesignator indicating the (furniture) object to check + :param seat_ent_des: EdEntityDesignator indicating the (furniture) object to check + :param volume_des: EdEntityDesignator indicating the (room) location to constraint the search :param number_of_people_des: designator which will result in the number of people near the entity, or None in case of state fails. """ # TODO implement logic for percent vs volume check in state machine rather than in the states themselves @@ -98,22 +133,22 @@ def __init__(self, robot, entity_des, number_of_people_des=None): else: number_of_people_des = ds.VariableDesignator(resolve_type=int) - people_in_seat_des = PeopleInSeatDesignator(robot, entity_des) + people_in_seat_des = PeopleInSeatDesignator(robot, seat_ent_des, volume_des) with self: - - # TODO add state to turn head towards the entity. smach.StateMachine.add( "LOOK_AT_SEAT", + LookAtEntity(robot=robot, entity=seat_ent_des, volume=volume_des), + transitions={"done": "INSPECT_SEAT"} ) smach.StateMachine.add( "INSPECT_SEAT", FindPeople( robot=robot, - # properties=None, - # query_entity_designator=None, - # found_people_designator=None, - # look_distance=10.0, + properties=None, + query_entity_designator=volume_des, + found_people_designator=people_in_seat_des, + look_distance=1.0, # speak=False, # strict=True, # nearest=False,