Merge pull request #967 from tue-robotics/rwc2019_challenge_receptionist

Rwc2019 challenge receptionist

challenge_receptionist/README.md

@@ -2,6 +2,9 @@
 
 The robot must introduce new guests to the home owner (John) and offer them a seat.
 
+Before the challenge, someone has to be taught to the robot under the name 'john',
+so that the robot will introduce new guests to John (and other already introduced guests)
+
 1. Learn the name of the new guest (who arrives through the door)
 2. Ask new guests for their favorite drink
 3. Guide guest to the living room
@@ -11,3 +14,10 @@ The robot must introduce new guests to the home owner (John) and offer them a se
     - stating their name
     - stating their favorite drink
 6. Find an empty seat for the new guest
+
+## TODO:
+- [x] Person detection does not work all the time...
+- [x] Fallback for failing person detection?
+- [x] Fix launch files so that ED does not crash due to missing services (mentioned by Rein)
+- [x] Challenge crashes (ED detect people not available)
+- [ ] Rise robot before human-robot interaction

challenge_receptionist/param/vizbox.yaml

@@ -4,4 +4,5 @@ story:
         - "To to the entrance and wait for a guest"
         - "Ask the guest's name and favourite drink"
         - "Guide the guest to the operator and introduce the guest"
+        - "Find an empty seat for the guest"
         - "Repeat for another guest"

challenge_receptionist/src/challenge_receptionist/find_empty_seat.py

@@ -0,0 +1,141 @@
+#! /usr/bin/env python
+import rospy
+import robot_smach_states as states
+import robot_smach_states.util.designators as ds
+import smach
+from robot_skills.util.entity import Entity
+from robot_skills.util.volume import Volume
+from robot_skills.classification_result import ClassificationResult
+
+
+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 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, room, seat_is_for=None):
+        smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
+
+        seats = SeatsInRoomDesignator(robot, seats_to_inspect, room, "seats_in_room")
+        seat_ent_des = ds.VariableDesignator(resolve_type=Entity)
+        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',
+                                   states.SayFormatted(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',
+                                   states.IterateDesignator(seats, seat_ent_des.writeable),
+                                   transitions={'next': 'CHECK_SEAT_EMPTY',
+                                                'stop_iteration': 'SAY_NO_EMPTY_SEATS'})
+
+            smach.StateMachine.add('CHECK_SEAT_EMPTY',
+                                   states.CheckVolumeEmpty(robot, seat_ent_des, 'on_top_of', 0.2),
+                                   transitions={'occupied': 'ITERATE_NEXT_SEAT',
+                                                'empty': 'POINT_AT_EMPTY_SEAT',
+                                                'partially_occupied': 'POINT_AT_PARTIALLY_OCCUPIED_SEAT',
+                                                'failed': 'ITERATE_NEXT_SEAT'})
+
+            smach.StateMachine.add('POINT_AT_EMPTY_SEAT',
+                                   states.PointAt(robot=robot,
+                                                  arm_designator=ds.UnoccupiedArmDesignator(robot, {'required_goals':['point_at']}),
+                                                  point_at_designator=seat_ent_des,
+                                                  look_at_designator=seat_ent_des),
+                                   transitions={"succeeded": "SAY_SEAT_EMPTY",
+                                                "failed": "SAY_SEAT_EMPTY"})
+
+            smach.StateMachine.add('SAY_SEAT_EMPTY',
+                                   states.SayFormatted(robot,
+                                                       ["Please sit on the {seat}, {name}"],
+                                                       name=seat_is_for,
+                                                       seat=ds.AttrDesignator(seat_ent_des, 'id', resolve_type=str),
+                                                       block=True),
+                                   transitions={'spoken': 'RESET_SUCCESS'})
+
+            smach.StateMachine.add('POINT_AT_PARTIALLY_OCCUPIED_SEAT',
+                                   states.PointAt(robot=robot,
+                                                  arm_designator=ds.UnoccupiedArmDesignator(robot, {'required_goals':['point_at']}),
+                                                  point_at_designator=seat_ent_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',
+                                   states.SayFormatted(robot,
+                                                       ["I think there's some space left here where you can sit {name}"],
+                                                       name=seat_is_for,
+                                                       block=True),
+                                   transitions={'spoken': 'RESET_SUCCESS'})
+
+            smach.StateMachine.add('SAY_NO_EMPTY_SEATS',
+                                   states.SayFormatted(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',
+                                   states.ResetArms(robot),
+                                   transitions={'done': 'failed'})
+
+            smach.StateMachine.add('RESET_SUCCESS',
+                                   states.ResetArms(robot),
+                                   transitions={'done': 'succeeded'})
+
+
+if __name__ == "__main__":
+    import sys
+    from robot_skills import get_robot
+
+    if len(sys.argv) < 4:
+        print "Please provide robot_name, room and seats_to_inspect as arguments. Eg. 'hero livingroom dinner_table bar dinnertable",
+        sys.exit(1)
+
+    robot_name = sys.argv[1]
+    room = sys.argv[2]
+    seats_to_inspect = sys.argv[3:]
+
+    rospy.init_node('test_find_emtpy_seat')
+    robot = get_robot(robot_name)
+
+    sm = FindEmptySeat(robot,
+                       seats_to_inspect=seats_to_inspect,
+                       room=ds.EntityByIdDesignator(robot, room))
+    sm.execute()

challenge_receptionist/src/challenge_receptionist/introduce_guest.py

@@ -0,0 +1,149 @@
+import rospy
+import robot_smach_states as states
+import robot_smach_states.util.designators as ds
+import smach
+from robocup_knowledge import load_knowledge
+from robot_skills.util.entity import Entity
+from math import radians
+
+challenge_knowledge = load_knowledge('challenge_receptionist')
+
+
+class GuestDescriptionStrDesignator(ds.Designator):
+    def __init__(self, guest_name_des, drinkname, name=None):
+        super(GuestDescriptionStrDesignator, self).__init__(resolve_type=str, name=name)
+
+        ds.check_type(guest_name_des, str)
+        ds.check_type(drinkname, str)
+
+        self.guest_name_des = guest_name_des
+        self.drinkname = drinkname
+
+    def _resolve(self):
+        name = self.guest_name_des.resolve()
+        drinkname = self.drinkname.resolve()
+        return "This is {name} whose favourite drink is {drink}".format(name=name, drink=drinkname)
+
+
+class IntroduceGuest(smach.StateMachine):
+    def __init__(self, robot, guest_ent_des, guest_name_des, guest_drinkname_des, assume_john=False):
+        smach.StateMachine.__init__(self, outcomes=['succeeded', 'abort'])
+
+        ds.check_type(guest_name_des, str)
+        ds.check_type(guest_drinkname_des, str)
+        ds.check_type(guest_ent_des, Entity)
+
+        all_old_guests = ds.VariableDesignator(resolve_type=[Entity], name='all_old_guests')
+        current_old_guest = ds.VariableDesignator(resolve_type=Entity, name='current_old_guest')
+
+        # For each person:
+        #   0. Go to the person (old guest)
+        #   1. Look at the person and point at the guest
+        #   2. Say 'Hi <person name>, this is <guest name>
+
+        with self:
+            smach.StateMachine.add('SAY_INTRO',
+                                   states.SayFormatted(robot,
+                                                       ["Hi {name}, let me introduce you our new guest {guest_name}. I'll show you in a bit"],
+                                                       name=ds.Designator(challenge_knowledge.operator_name) if assume_john else ds.Designator("folks"),
+                                                       guest_name=guest_name_des,
+                                                       block=False),
+                                   transitions={'spoken': 'FIND_OLD_GUESTS'})
+
+            smach.StateMachine.add('FIND_OLD_GUESTS',
+                                   states.FindPeopleInRoom(robot,
+                                                           room=challenge_knowledge.waypoint_livingroom['id'],
+                                                           found_people_designator=all_old_guests.writeable),
+                                   transitions = {'found': 'ITERATE_OLD_GUESTS',
+                                                   'not_found': 'ITERATE_OLD_GUESTS'})
+
+            smach.StateMachine.add('ITERATE_OLD_GUESTS',
+                                   states.IterateDesignator(all_old_guests,
+                                                            current_old_guest.writeable),
+                                   transitions={'next': 'GOTO_OPERATOR',
+                                                'stop_iteration': 'succeeded'})
+
+            smach.StateMachine.add('GOTO_OPERATOR',
+                                   states.NavigateToObserve(robot,
+                                                            current_old_guest,
+                                                            radius=1.0,
+                                                            margin=1.0),  # Makes the robot go within 2m of current_old_guest
+                                   transitions={'arrived': 'SAY_LOOK_AT_GUEST',
+                                                'unreachable': 'SAY_LOOK_AT_GUEST',
+                                                'goal_not_defined': 'SAY_LOOK_AT_GUEST'})
+
+            smach.StateMachine.add('SAY_LOOK_AT_GUEST',
+                                   states.SayFormatted(robot,
+                                                       ["Hi {name}, let me show you our guest"],
+                                                       name=ds.Designator(challenge_knowledge.operator_name) if assume_john else ds.AttrDesignator(current_old_guest, "person_properties.name", resolve_type=str),
+                                                       block=True),
+                                   transitions={'spoken': 'TURN_TO_GUEST'})
+
+            smach.StateMachine.add('TURN_TO_GUEST',
+                                   states.Turn(robot=robot,
+                                               radians=radians(180)),
+                                   transitions={"turned": "FIND_GUEST"})
+
+            smach.StateMachine.add('FIND_GUEST',
+                                   states.FindPerson(robot=robot,
+                                                     person_label=guest_name_des,
+                                                     search_timeout=30,
+                                                     found_entity_designator=guest_ent_des.writeable,
+                                                     speak_when_found=False),
+                                   transitions={"found": "POINT_AT_GUEST",
+                                                "failed": "INTRODUCE_GUEST_WITHOUT_POINTING"})
+
+            smach.StateMachine.add('POINT_AT_GUEST',
+                                   states.PointAt(robot=robot,
+                                                  arm_designator=ds.UnoccupiedArmDesignator(robot,{'required_goals':['point_at']}),
+                                                  point_at_designator=guest_ent_des,
+                                                  look_at_designator=current_old_guest),
+                                   transitions={"succeeded": "INTRODUCE_GUEST_BY_POINTING",
+                                                "failed": "INTRODUCE_GUEST_WITHOUT_POINTING"})
+
+            smach.StateMachine.add('INTRODUCE_GUEST_BY_POINTING',
+                                   states.Say(robot, GuestDescriptionStrDesignator(guest_name_des, guest_drinkname_des),
+                                              block=True,
+                                              look_at_standing_person=True),
+                                   transitions={'spoken': 'RESET_ARM'})
+
+            smach.StateMachine.add('INTRODUCE_GUEST_WITHOUT_POINTING',
+                                   states.SayFormatted(robot,
+                                                       "Our new guest is {name} who likes {drink}",
+                                                       name=guest_name_des, drink=guest_drinkname_des,
+                                                       block=True,
+                                                       look_at_standing_person=True),
+                                   transitions={'spoken': 'RESET_ARM'})
+
+            smach.StateMachine.add('RESET_ARM',
+                                   states.ResetArms(robot),
+                                   transitions={'done': 'succeeded' if assume_john else 'ITERATE_OLD_GUESTS'})
+
+
+if __name__ == "__main__":
+    import sys
+    from robot_skills import get_robot
+
+    if len(sys.argv) < 3:
+        print("Please provide robot_name, room and seats_to_inspect as arguments. Eg. 'hero livingroom dinner_table bar dinnertable")
+        sys.exit(1)
+
+    robot_name = sys.argv[1]
+    room = sys.argv[2]
+    seats_to_inspect = sys.argv[3:]
+
+    rospy.init_node('test_find_emtpy_seat')
+    robot = get_robot(robot_name)
+
+    guest_entity_des = ds.VariableDesignator(resolve_type=Entity, name='guest_entity')
+    guest_name_des = ds.VariableDesignator('dummy_guest', name='guest_name')
+    guest_drinkname_des = ds.VariableDesignator('dummy_drink', name='guest_drinkname')
+
+    sm = IntroduceGuest(robot,
+                        guest_entity_des,
+                        guest_name_des,
+                        guest_drinkname_des)
+
+    sm.execute()
+
+    rospy.loginfo("Guest is {}".format(guest_entity_des.resolve()))