diff --git a/tasks/receptionist/config/motions.yaml b/tasks/receptionist/config/motions.yaml index 0f4a34aa0..de45b5d18 100644 --- a/tasks/receptionist/config/motions.yaml +++ b/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 \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/sm.py b/tasks/receptionist/src/receptionist/sm.py index 173c507c3..b9cfe1b2f 100755 --- a/tasks/receptionist/src/receptionist/sm.py +++ b/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'}) diff --git a/tasks/receptionist/src/receptionist/speech_helper.py b/tasks/receptionist/src/receptionist/speech_helper.py index 939f73830..40878e807 100755 --- a/tasks/receptionist/src/receptionist/speech_helper.py +++ b/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) \ No newline at end of file diff --git a/tasks/receptionist/src/receptionist/states/look_for_seats.py b/tasks/receptionist/src/receptionist/states/look_for_seats.py index e3389ca74..aea147463 100755 --- a/tasks/receptionist/src/receptionist/states/look_for_seats.py +++ b/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