Runnable demo

MBTMBTMBT · Feb 5, 2024 · 134c870 · 134c870
1 parent 7827dc2
commit 134c870
Show file tree

Hide file tree

Showing 5 changed files with 35 additions and 6 deletions.
diff --git a/tasks/receptionist/launch/setup.launch b/tasks/receptionist/launch/setup.launch
@@ -23,7 +23,7 @@
     <node pkg="interaction_module" name="interaction_module_srv" type="interaction_module_srv.py"/>
 
     <arg name="whisper_matcher" default="by-index" />
-    <arg name="whisper_device_param" default="18" />
+    <arg name="whisper_device_param" default="9" />
     <!--<arg name="whisper_device_param" default="13" />-->
     <arg name="rasa_model" default="$(find lasr_rasa)/assistants/receptionist/models"/>
 
@@ -54,7 +54,9 @@
 
 
     <!--      PERCEPTION     -->
-    <node pkg="lasr_vision_yolov8" type="service" name="service" output="screen"/>
+    <node pkg="lasr_vision_yolov8" type="service" name="yolo_service" output="screen"/>
+    <node pkg="lasr_vision_torch" type="service" name="torch_service" output="screen"/>
+    <node pkg="lasr_vision_bodypix" type="service" name="bodypix_service" output="screen"/>
 
 
 

diff --git a/tasks/receptionist/src/receptionist/sm.py b/tasks/receptionist/src/receptionist/sm.py
@@ -9,7 +9,9 @@
 from receptionist.states import GoToSeatingArea
 from receptionist.states import LookForSeats
 from receptionist.states import GoToWaitForPerson
+from receptionist.states import SpeakDescriptions
 from lasr_skills import WaitForPersonInArea
+from lasr_skills import DescribePeople
 #from receptionist.states.end import End
 
 
@@ -25,7 +27,9 @@ def __init__(self):
             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'})
             smach.StateMachine.add('ASK_FOR_NAME', AskForName(self.default),transitions={'failed':'ASK_FOR_NAME','succeeded':'ASK_FOR_DRINK'})
-            smach.StateMachine.add('ASK_FOR_DRINK', AskForDrink(self.default),transitions={'failed':'ASK_FOR_DRINK','succeeded':'GO_TO_SEATING_AREA'})
-            smach.StateMachine.add('GO_TO_SEATING_AREA', GoToSeatingArea(self.default), transitions={'succeeded' : 'LOOK_FOR_SEATS'})
+            smach.StateMachine.add('ASK_FOR_DRINK', AskForDrink(self.default),transitions={'failed':'ASK_FOR_DRINK','succeeded':'DESCRIBE_PEOPLE'})
+            smach.StateMachine.add('DESCRIBE_PEOPLE', DescribePeople(),transitions={'succeeded':'SPEAK_DESCRIPTIONS','failed':'GO_TO_SEATING_AREA'})
+            smach.StateMachine.add('SPEAK_DESCRIPTIONS', SpeakDescriptions(self.default), transitions={'failed':'GO_TO_SEATING_AREA','succeeded':'GO_TO_SEATING_AREA'})
+            smach.StateMachine.add('GO_TO_SEATING_AREA', GoToSeatingArea(self.default), transitions={'succeeded' : 'LOOK_FOR_SEATS'})            
             smach.StateMachine.add('LOOK_FOR_SEATS', LookForSeats(self.default), transitions={'succeeded' : 'GO_TO_WAIT_FOR_PERSON'})
             smach.StateMachine.add('END', End(self.default),transitions={'succeeded':'succeeded'})
diff --git a/tasks/receptionist/src/receptionist/states/__init__.py b/tasks/receptionist/src/receptionist/states/__init__.py
@@ -5,4 +5,5 @@
 from .go_to_seating_area import GoToSeatingArea
 from .go_to_wait_for_person import GoToWaitForPerson
 from .look_for_seats import LookForSeats
-from .start import Start
+from .start import Start
+from .speakdescriptions import SpeakDescriptions
diff --git a/tasks/receptionist/src/receptionist/states/look_for_seats.py b/tasks/receptionist/src/receptionist/states/look_for_seats.py
@@ -25,7 +25,7 @@ def execute(self, userdata):
             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)
+            pm_goal = PlayMotionGoal(motion_name=self.remaining_motions.pop(0), skip_planning=True)
             self.default.pm.send_goal_and_wait(pm_goal)
             rospy.sleep(1.0)
             return 'not_done'

diff --git a/tasks/receptionist/src/receptionist/states/speakdescriptions.py b/tasks/receptionist/src/receptionist/states/speakdescriptions.py
@@ -0,0 +1,22 @@
+import smach
+import rospy
+
+
+class SpeakDescriptions(smach.State):
+    def __init__(self, default):
+        smach.State.__init__(self, outcomes=['succeeded','failed'], input_keys=['people'])
+        self.default = default
+
+    def execute(self, userdata):
+        for person in userdata['people']:
+            self.default.voice.speak('I see a person')
+
+            for feature in person['features']:
+                if feature.label:
+                    if len(feature.colours) == 0:
+                        self.default.voice.speak(f'They have {feature.name}.')
+                        continue
+
+                    self.default.voice.speak(f'They have {feature.name} and it has the colour {feature.colours[0]}')
+
+        return 'succeeded'