Just. make. it. work...

tasks/qualification/action/HandoverObject.action

@@ -1,2 +1,4 @@
+string name
+string object
 ---
 ---

tasks/qualification/action/ReceiveObject.action

@@ -1,2 +1,3 @@
+string object
 ---
 ---

tasks/qualification/data/command_list.txt

@@ -2,11 +2,13 @@ Please guide me to the lab.
 Please guide me to the kitchen.
 Guide me to the lift please.
 Follow me to the lab.
+Assist Jared.
+Go help Jared.
+Take the protein bars to Nicole.
 Go to the lab and find me a desk.
 Could you please find Jared and ask if they need assistance.
 Please locate Jared and ask if he has seen Matt.
 Bring Jared a cup of coffee please.
-Go to the kitchen and find Jared and ask him if he needs help.
 Robot please lead William to the bookcase, you may find him at the bookcase
 Get the tray from the end table and deliver it to Mary at the dining table
 Robot please face Linda at the back door and introduce her to everyone in the corridor.

tasks/qualification/launch/better_qualification.launch

@@ -3,12 +3,6 @@
         <arg name="debug" value="true" />
     </include>
 
-    <node pkg="lasr_speech_recognition_whisper" type="transcribe_microphone_server" name="transcribe_microphone_server" output="screen" args="--mic_device 9 --end_timeout 2.0 --model_name small.en"/>
-    <include file="$(find lasr_vision_yolov8)/launch/service.launch">
-        <arg name="debug" value="true" />
-        <arg name="preload" value="['yolov8n-seg.pt']" />
-    </include>
-
     <node pkg="unsafe_traversal" type="unsafe_traversal" name="unsafe_traversal"/>
     <node pkg="tf_module" type="tf_transforms_base.py" name="tf_module" output="screen"/>
 

tasks/qualification/launch/slow.launch

@@ -0,0 +1,7 @@
+<launch>
+    <node pkg="lasr_speech_recognition_whisper" type="transcribe_microphone_server" name="transcribe_microphone_server" output="screen" args="--mic_device 5 --end_timeout 2.0 --model_name small.en"/>
+    <include file="$(find lasr_vision_yolov8)/launch/service.launch">
+        <arg name="debug" value="true" />
+        <arg name="preload" value="['yolov8n-seg.pt']" />
+    </include>
+</launch>

tasks/qualification/nodes/actions/detect_people

@@ -41,6 +41,7 @@ class DetectPeople:
         tf_req.target_frame = String("map")
         tf_req.point = centroid
         response = self.tf(tf_req)
+        print(response.target_point.point)
         return response.target_point.point
 
     def execute_cb(self, _):

tasks/qualification/nodes/actions/find_person

@@ -3,6 +3,10 @@
 import rospy
 import actionlib
 
+import numpy as np
+import math
+from scipy.spatial.transform import Rotation as R
+
 from qualification.msg import (
     FindPersonAction,
     FindPersonResult,
@@ -14,7 +18,7 @@ from qualification.msg import (
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 from actionlib import SimpleActionClient, SimpleActionServer
 from pal_interaction_msgs.msg import TtsGoal, TtsAction
-from geometry_msgs.msg import Pose, Point, Quaternion
+from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped
 from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
 from trajectory_msgs.msg import JointTrajectoryPoint
 
@@ -26,7 +30,7 @@ class FindPerson:
             "pose": Pose(
                 position=Point(1.7, -0.72, 0.0),
                 orientation=Quaternion(0.0, 0.0, -0.45, 0.89),
-            )
+            ),
         }
     }
 
@@ -74,7 +78,7 @@ class FindPerson:
         head_goal = FollowJointTrajectoryGoal()
         head_goal.trajectory.joint_names = ["head_1_joint", "head_2_joint"]
 
-        tts_goal.rawtext.text = f"Finding person {goal.name} in the {goal.location}..."
+        tts_goal.rawtext.text = f"Finding {goal.name} in the {goal.location}..."
         self.tts.send_goal_and_wait(tts_goal)
 
         move_base_goal.target_pose.header.stamp = rospy.Time.now()
@@ -104,10 +108,21 @@ class FindPerson:
                 head_goal.trajectory.points = [(point)]
                 self.head_controller.send_goal_and_wait(head_goal)
 
+                orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
+                robot_pose = rospy.wait_for_message(
+                    "/amcl_pose", PoseWithCovarianceStamped
+                ).pose.pose
+                dist_x = person_point.x - robot_pose.position.x
+                dist_y = person_point.y - robot_pose.position.y
+                theta_deg = np.degrees(math.atan2(dist_y, dist_x))
+                orientation = Quaternion(
+                    *R.from_euler("z", theta_deg, degrees=True).as_quat()
+                )
+
                 move_base_goal.target_pose.header.stamp = rospy.Time.now()
                 move_base_goal.target_pose.pose = Pose(
-                    position=Point(person_point.x, person_point.y + 1.0, 0.0),
-                    orientation=self.KB[goal.location]["pose"].orientation,
+                    position=Point(person_point.x - 0.3, person_point.y + 1, 0.0),
+                    orientation=orientation,
                 )
                 self.move_base.send_goal_and_wait(move_base_goal)
                 break

tasks/qualification/nodes/actions/greet

@@ -34,20 +34,24 @@ class Greet:
             auto_start=False,
         )
 
+        self.greeted = []
+
         self._action_server.start()
 
     def execute_cb(self, goal):
+        if goal.name in self.greeted:
+            return
         tts_goal = TtsGoal()
         tts_goal.rawtext.lang_id = "en_GB"
         if goal.name:
             tts_goal.rawtext.text = (
                 f"Hello, {goal.name}. {random.choice(self.known_greeting_suffixes)}"
             )
+            self.greeted.append(goal.name)
         else:
             tts_goal.rawtext.text = random.choice(self.unknown_greetings)
 
         self.tts.send_goal_and_wait(tts_goal)
-
         self._action_server.set_succeeded(GreetResult())
 
 

tasks/qualification/nodes/actions/guide

@@ -16,13 +16,13 @@ class Guide:
     KB = {
         "lab": {
             "pose": Pose(
-                position=Point(2.58, 2.07, 0.0),
-                orientation=Quaternion(0.0, 0.0, 0.94, 0.50),
+                position=Point(2.61, 1.33, 0.0),
+                orientation=Quaternion(0.0, 0.0, 0.77, 0.63),
             ),
             "edges": {
                 "corridor": Pose(
-                    position=Point(4.68, 6.87, 0.0),
-                    orientation=Quaternion(0.0, 0.0, 0.59, 0.80),
+                    position=Point(4.27, 5.80, 0.0),
+                    orientation=Quaternion(0.0, 0.0, -0.89, 0.45),
                 )
             },
         },
@@ -33,8 +33,8 @@ class Guide:
             ),
             "edges": {
                 "lab": Pose(
-                    position=Point(4.09, 8.21, 0.0),
-                    orientation=Quaternion(0.0, 0.0, 0.91, 0.40),
+                    position=Point(4.29, 8.81, 0.0),
+                    orientation=Quaternion(0.0, 0.0, -0.59, 0.81),
                 ),
                 "kitchen": None,
             },

tasks/qualification/nodes/actions/handover_object

@@ -40,14 +40,15 @@ class HandoverObject:
         pm_goal = PlayMotionGoal(motion_name="reach_arm", skip_planning=False)
         self.play_motion.send_goal_and_wait(pm_goal)
 
-        tts_goal.rawtext.text = "Please grab the object in my hand, and say 'Done' when you are rady for me to release it."
+        tts_goal.rawtext.text = f"{goal.name}, please grab the {goal.object} in my hand, and say 'Done' when you are rady for me to release it."
         self.tts.send_goal_and_wait(tts_goal)
         while True:
             self.transcribe_speech.send_goal(TranscribeSpeechGoal())
             self.transcribe_speech.wait_for_result()
             result = self.transcribe_speech.get_result()
             if "done" in result.sequence.lower():
                 break
+        rospy.sleep(rospy.Duration(2))
         pm_goal = PlayMotionGoal(motion_name="open_gripper", skip_planning=False)
         self.play_motion.send_goal_and_wait(pm_goal)
         pm_goal = PlayMotionGoal(motion_name="home", skip_planning=False)

tasks/qualification/nodes/actions/identify

@@ -38,8 +38,11 @@ class Identify:
         print(resp.detections)
         result = IdentifyResult()
         if resp.detections:
-            result.name = resp.detections[0].name
-            result.success = True
+            if resp.detections[0].confidence > 0.55:
+                result.name = resp.detections[0].name
+                result.success = True
+            else:
+                result.success = False
         else:
             result.success = False
 

tasks/qualification/nodes/actions/learn_face

@@ -80,7 +80,7 @@ class LearnFace:
         req = LFRequest()
         req.dataset = "qualification"
         req.name = goal.name
-        req.n_images = 25
+        req.n_images = 10
         self.learn_face(req)
 
         self._action_server.set_succeeded(LearnFaceResult())

tasks/qualification/nodes/actions/receive_object

@@ -44,7 +44,7 @@ class ReceiveObject:
         pm_goal = PlayMotionGoal(motion_name="open_gripper", skip_planning=False)
         self.play_motion.send_goal_and_wait(pm_goal)
         tts_goal.rawtext.text = (
-            "Please place the object in my hand, and say 'Done' afterwards."
+            f"Please place the {goal.object} in my hand, and say 'Done' afterwards."
         )
         self.tts.send_goal_and_wait(tts_goal)
         # Wait for speech

tasks/qualification/nodes/actions/wait_greet

@@ -34,7 +34,13 @@ class WaitGreet:
         while not rospy.is_shutdown():
             self.transcribe_speech.send_goal(TranscribeSpeechGoal())
             self.transcribe_speech.wait_for_result()
-            result = self.transcribe_speech.get_result().sequence.lower()
+            result = (
+                self.transcribe_speech.get_result()
+                .sequence.lower()
+                .replace(".", "")
+                .replace("!", "")
+                .replace(",", "")
+            )
             if "hello" in result.split(" ") or "hi" in result.split(" "):
                 self._action_server.set_succeeded(WaitGreetResult())
                 rospy.loginfo("Finished waiting")