merge: pull request #119 from jws-1/receptionist-point-at-empty-seat

receptionist: pointing and minor seating fixes

tasks/receptionist/config/motions.yaml

@@ -29,4 +29,19 @@ play_motion:
       joints: [head_1_joint, head_2_joint]
       points:
       - positions: [-0.5, -0.25]
+        time_from_start: 0.0
+    raise_torso:
+      joints: [torso_lift_joint]
+      points:
+      - positions: [0.30]
+        time_from_start: 0.0
+    point:
+      joints: [arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint]
+      points:
+      - positions: [1.46, -1.47, -3.01, 1.73, -1.62, -0.12, -0.03]
+        time_from_start: 0.0
+    back_to_default_head:
+      joints: [head_1_joint, head_2_joint]
+      points:
+      - positions: [0.0, 0.0]
         time_from_start: 0.0

tasks/receptionist/src/receptionist/sm.py

@@ -20,7 +20,7 @@ def __init__(self):
         self.userdata.area_polygon = [[1.94, 0.15], [2.98, 0.28], [3.08, -0.68], [2.06, -0.84]]
         self.userdata.depth_topic = "/xtion/depth_registered/points"
         with self:
-            # smach.StateMachine.add('START', Start(self.default), transitions={'succeeded' : 'GO_TO_WAIT_FOR_PERSON'})
+            smach.StateMachine.add('START', Start(self.default), transitions={'succeeded' : 'GO_TO_WAIT_FOR_PERSON'})
             smach.StateMachine.add("GO_TO_WAIT_FOR_PERSON",GoToWaitForPerson(self.default), transitions={'succeeded': 'WAIT_FOR_PERSON'})
             smach.StateMachine.add('WAIT_FOR_PERSON', WaitForPersonInArea() ,transitions={'succeeded' : 'GO_TO_PERSON', 'failed' : 'failed'})
             smach.StateMachine.add('GO_TO_PERSON', GoToPerson(self.default),transitions={'succeeded':'ASK_FOR_NAME'})

tasks/receptionist/src/receptionist/speech_helper.py

@@ -33,7 +33,7 @@ def get_drink(default):
     # if resp['intent']['name'] != 'fav_drink':
     #     return "unknown"
     drink = resp["entities"].get("drink",[])
-    if drink is None: 
+    if drink is None or not drink: 
         return "unknown"
     drink = drink[0]["value"].lower()
     return str(drink)
@@ -43,7 +43,7 @@ def get_name(default):
     # if resp['intent']['name'] != 'name':
     #     return "unknown"
     name = resp["entities"].get("name",[])
-    if name is None: 
+    if name is None or not name: 
         return "unknown"
     name = name[0]["value"].lower()
     return str(name)

tasks/receptionist/src/receptionist/states/look_for_seats.py

@@ -8,6 +8,7 @@
 from geometry_msgs.msg import Point
 from shapely.geometry import Polygon
 from lasr_skills import DetectObjects3D, LookToPoint
+from copy import copy
 
 class LookForSeats(smach.StateMachine):
 
@@ -17,10 +18,12 @@ class Look(smach.State):
         def __init__(self, default):
             smach.State.__init__(self, outcomes=['done', 'not_done'], input_keys=['look_motion'])
             self.default = default
-            self.motions = ['look_down_center']
+            self.motions = ['look_down_left', 'look_down_center', 'look_down_right']
+            self.remaining_motions = copy(self.motions)
 
         def execute(self, userdata):
-            if not self.motions:
+            if not self.remaining_motions:
+                self.remaining_motions = copy(self.motions)
                 return 'done'
             pm_goal = PlayMotionGoal(motion_name=self.motions.pop(0), skip_planning=True)
             self.default.pm.send_goal_and_wait(pm_goal)
@@ -67,6 +70,7 @@ def execute(self, userdata):
                     self.default.voice.sync_tts("Please sit down on this chair to my left")
                 else:
                     self.default.voice.sync_tts("Please sit down on this chair")
+                    self.remaining_motions = []
                 return 'succeeded'
             for (det2, _) in userdata.bulk_people_detections_3d:
                 if not self.is_person_sitting_in_chair(np.array(det2.xyseg).reshape(-1, 2), np.array(det1.xyseg).reshape(-1, 2)):
@@ -84,9 +88,37 @@ def execute(self, userdata):
                         self.default.voice.sync_tts("Please sit down on this chair to my left")
                     else:
                         self.default.voice.sync_tts("Please sit down on this chair")
+                    self.remaining_motions = []
                     return 'succeeded'
             return 'not_done'
 
+    class PointToChair(smach.State):
+
+        def __init__(self, default):
+            smach.State.__init__(self, outcomes=['succeeded'], input_keys=['point'])
+            self.default = default
+
+        def execute(self, userdata):
+
+            pm_goal = PlayMotionGoal(motion_name="back_to_default_head", skip_planning=True)
+            self.default.pm.send_goal_and_wait(pm_goal)
+
+            self.default.controllers.base_controller.sync_face_to(userdata.point.x, userdata.point.y)
+
+            pm_goal = PlayMotionGoal(motion_name="raise_torso", skip_planning=True)
+            self.default.pm.send_goal_and_wait(pm_goal)
+
+            pm_goal = PlayMotionGoal(motion_name="point", skip_planning=False)
+            self.default.pm.send_goal_and_wait(pm_goal)
+
+            rospy.sleep(5.0)
+
+            pm_goal = PlayMotionGoal(motion_name="home", skip_planning=False)
+            self.default.pm.send_goal_and_wait(pm_goal)
+
+            return 'succeeded'
+
+
     def __init__(self, default):
         smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
         self.default = default
@@ -100,7 +132,7 @@ def __init__(self, default):
             smach.StateMachine.add('DETECT_OBJECTS_3D', DetectObjects3D(), transitions={'succeeded' : 'PROCESS_DETECTIONS', 'failed' : 'failed'})
             smach.StateMachine.add('PROCESS_DETECTIONS', self.ProcessDetections(), transitions={'succeeded' : 'LOOK'})
             smach.StateMachine.add('CHECK_SEAT', self.CheckSeat(self.default), transitions={'succeeded' : 'FINALISE_SEAT', 'failed' : 'failed', 'not_done': 'CHECK_SEAT'})
-            smach.StateMachine.add('FINALISE_SEAT', LookToPoint(), transitions={'succeeded' : 'succeeded'})
+            smach.StateMachine.add('FINALISE_SEAT', self.PointToChair(self.default), transitions={'succeeded' : 'succeeded'})
 
 if __name__ == "__main__":
     from receptionist import Default