CML 08-07 (#248)

Co-authored-by: fireblonde <nicollehchevska@gmail.com>

Author: jws-1

common/navigation/lasr_person_following/src/lasr_person_following/person_following.py

@@ -40,7 +40,6 @@
 
 from pal_interaction_msgs.msg import TtsGoal, TtsAction
 
-
 MAX_VISION_DIST: float = 5.0
 
 
@@ -82,14 +81,16 @@ def __init__(
         new_goal_threshold: float = 2.0,
         stopping_distance: float = 1.0,
         static_speed: float = 0.0015,
-        max_speed: float = 0.5,
+        max_speed: float = 0.55,
+        asking_time_limit: float = 15.0,
     ):
         self._start_following_radius = start_following_radius
         self._start_following_angle = start_following_angle
         self._new_goal_threshold = new_goal_threshold
         self._stopping_distance = stopping_distance
         self._static_speed = static_speed
         self._max_speed = max_speed
+        self._asking_time_limit = asking_time_limit
 
         self._track_id = None
 
@@ -237,12 +238,12 @@ def _detect_waving_person(self) -> DetectWaveResponse:
             return DetectWaveResponse()
 
     # recover with vision, look around and check if person is waving
-    def _recover_vision(self, prev_pose: PoseStamped) -> bool:
+    def _recover_vision(self, robot_pose: PoseStamped, prev_pose: PoseStamped) -> bool:
 
         # cancel current goal
         self._cancel_goal()
 
-        self._tts("Can you wave at me so that i can try to find you easily", wait=True)
+        self._tts("Please come into my view and wave so that I can find you", wait=True)
 
         for motion in self._vision_recovery_motions:
             rospy.loginfo(f"Performing motion: {motion}")
@@ -257,27 +258,30 @@ def _recover_vision(self, prev_pose: PoseStamped) -> bool:
                     ):
                         continue
                     else:
-                        goal_pose = self._tf_pose(
-                            PoseStamped(
-                                pose=Pose(
-                                    position=Point(
-                                        x=response.wave_position.point.x,
-                                        y=response.wave_position.point.y,
-                                        z=response.wave_position.point.z,
+                        goal_pose = self._get_pose_on_path(
+                            self._tf_pose(robot_pose, "map"),
+                            self._tf_pose(
+                                PoseStamped(
+                                    pose=Pose(
+                                        position=Point(
+                                            x=response.wave_position.point.x,
+                                            y=response.wave_position.point.y,
+                                            z=response.wave_position.point.z,
+                                        ),
+                                        orientation=Quaternion(0, 0, 0, 1),
                                     ),
-                                    orientation=Quaternion(0, 0, 0, 1),
+                                    header=prev_pose.header,
                                 ),
-                                header=prev_pose.header,
+                                "map",
                             ),
-                            "map",
-                        )
-                        goal_pose.pose.orientation = self._compute_face_quat(
-                            prev_pose.pose, goal_pose.pose
+                            self._stopping_distance,
                         )
-                        rospy.loginfo(goal_pose.pose.position)
-                        self._move_base(goal_pose)
-                        return True
-
+                        if goal_pose is None:
+                            rospy.logwarn("Could not find a path to the person")
+                            return False
+                        else:
+                            self._move_base(goal_pose)
+                            return True
         return False
 
     def _euclidian_distance(self, p1: Pose, p2: Pose) -> float:
@@ -383,6 +387,7 @@ def follow(self) -> None:
         last_goal_time: Union[None, rospy.Time] = None
         going_to_person: bool = False
         track_vels: List[float] = []
+        asking_time: rospy.Time = rospy.Time.now()
 
         while not rospy.is_shutdown():
 
@@ -419,7 +424,9 @@ def follow(self) -> None:
                     ask_back = True
 
                 if not ask_back:
-                    self._recover_track(say=not self._recover_vision(prev_goal))
+                    self._recover_track(
+                        say=not self._recover_vision(robot_pose, prev_goal)
+                    )
                 else:
                     self._recover_track(say=True)
 
@@ -444,8 +451,14 @@ def follow(self) -> None:
                 last_goal_time = rospy.Time.now()
                 prev_track = track
 
-            if np.mean([np.linalg.norm(vel) for vel in track_vels]) > self._max_speed:
+            # Check if the person is walking too fast and ask them to slow down only
+            # if you haven't asked them in the last 15 seconds
+            if (
+                np.mean([np.linalg.norm(vel) for vel in track_vels]) > self._max_speed
+                and (rospy.Time.now() - asking_time).to_sec() > self._asking_time_limit
+            ):
                 self._tts("Please walk slower, I am struggling to keep up", wait=False)
+                asking_time = rospy.Time.now()
 
             # Distance to the previous pose
             dist_to_prev = (
@@ -465,19 +478,22 @@ def follow(self) -> None:
                 prev_goal = goal_pose
                 prev_track = track
                 last_goal_time = rospy.Time.now()
+            # retry goal if it was aborted
             elif (
                 self._move_base_client.get_state() in [GoalStatus.ABORTED]
                 and prev_goal is not None
             ):
                 rospy.logwarn("Goal was aborted, retrying")
-                rospy.logwarn((rospy.Time.now() - last_goal_time).to_sec())
-                rospy.logwarn(track.pose == prev_track.pose)
-                rospy.logwarn("")
                 self._move_base(prev_goal)
+            # check if the person has been static for too long
             elif (
-                np.mean([np.linalg.norm(vel) for vel in track_vels])
-                < self._static_speed
-            ) and len(track_vels) == 10:
+                (
+                    np.mean([np.linalg.norm(vel) for vel in track_vels])
+                    < self._static_speed
+                )
+                and len(track_vels) == 10
+                and prev_track is not None
+            ):
                 rospy.logwarn(
                     "Person has been static for too long, going to them and stopping"
                 )
@@ -487,41 +503,6 @@ def follow(self) -> None:
                 # clear velocity buffer
                 track_vels = []
 
-                robot_pose: PoseStamped = self._robot_pose_in_odom()
-                dist: float = self._euclidian_distance(track.pose, robot_pose.pose)
-                rospy.loginfo(f"Distance to person: {dist}")
-
-                # If the person is too far away, go to them
-                if dist > self._stopping_distance:
-                    goal_pose = self._get_pose_on_path(
-                        self._tf_pose(robot_pose, "map"),
-                        self._tf_pose(
-                            PoseStamped(
-                                pose=track.pose,
-                                header=tracks.header,
-                            ),
-                            "map",
-                        ),
-                        self._stopping_distance,
-                    )
-                    # If we can't find a path, face them
-                    if goal_pose is None:
-                        rospy.logwarn("Could not find a path to the person")
-                        goal_pose = robot_pose
-                        goal_pose.pose.orientation = self._compute_face_quat(
-                            robot_pose.pose, track.pose
-                        )
-                        goal_pose = self._tf_pose(goal_pose, "map")
-                # Otherwise, face them
-                else:
-                    goal_pose = robot_pose
-                    goal_pose.pose.orientation = self._compute_face_quat(
-                        robot_pose.pose, track.pose
-                    )
-                    goal_pose = self._tf_pose(goal_pose, "map")
-
-                self._move_base(goal_pose)
-
                 if self._check_finished():
                     rospy.loginfo("Finished following person")
                     break

common/speech/lasr_speech_recognition_whisper/nodes/transcribe_microphone_server

@@ -53,9 +53,9 @@ class TranscribeSpeechAction(object):
     _result = lasr_speech_recognition_msgs.msg.TranscribeSpeechResult()
 
     def __init__(
-        self,
-        action_name: str,
-        model_params: speech_model_params,
+            self,
+            action_name: str,
+            model_params: speech_model_params,
     ) -> None:
         """Starts an action server for transcribing speech.
 
@@ -116,9 +116,9 @@ class TranscribeSpeechAction(object):
             )
 
     def _configure_recogniser(
-        self,
-        energy_threshold: Optional[float] = None,
-        pause_threshold: Optional[float] = None,
+            self,
+            energy_threshold: Optional[float] = None,
+            pause_threshold: Optional[float] = None,
     ) -> sr.Recognizer:
         """Configures the speech recogniser object.
 
@@ -197,8 +197,8 @@ class TranscribeSpeechAction(object):
             ).get_wav_data()
         # Magic number 32768.0 is the maximum value of a 16-bit signed integer
         float_data = (
-            np.frombuffer(wav_data, dtype=np.int16).astype(np.float32, order="C")
-            / 32768.0
+                np.frombuffer(wav_data, dtype=np.int16).astype(np.float32, order="C")
+                / 32768.0
         )
 
         if self._action_server.is_preempt_requested():

common/third_party/leg_tracker

@@ -1,3 +1,4 @@
+from .wait import Wait
 from .detect import Detect
 from .detect_3d import Detect3D
 from .detect_3d_in_area import Detect3DInArea
@@ -21,6 +22,5 @@
 from .recognise import Recognise
 from .detect_gesture import DetectGesture
 from .look_at_person import LookAtPerson
-from .wait import Wait
 from .find_gesture_person import FindGesturePerson
 from .adjust_camera import AdjustCamera

skills/src/lasr_skills/__init__.py

@@ -10,6 +10,12 @@
     BodyPixKeypointDetectionRequest,
 )
 from sensor_msgs.msg import Image
+from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, PointStamped
+from std_msgs.msg import Header
+from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_pose
+import tf2_ros as tf
+from visualization_msgs.msg import Marker
+from markers import create_and_publish_marker
 
 
 class DetectGesture(smach.State):
@@ -29,8 +35,8 @@ def __init__(
         smach.State.__init__(
             self,
             outcomes=["succeeded", "missing_keypoints", "failed"],
-            input_keys=["img_msg"],
-            output_keys=["gesture_detected"],
+            input_keys=["img_msg", "detection"],
+            output_keys=["gesture_detected", "person_point"],
         )
         self.gesture_to_detect = gesture_to_detect
         self.bodypix_client = rospy.ServiceProxy(
@@ -46,6 +52,8 @@ def __init__(
             "rightWrist",
             "rightShoulder",
         ]
+        # publish a marker
+        self.person_point_pub = rospy.Publisher("/person_point", Marker, queue_size=1)
 
     def execute(self, userdata):
 
@@ -68,13 +76,15 @@ def execute(self, userdata):
             for keypoint in detected_keypoints
         }
 
+        detected_gesture = "none"
+
         if "leftShoulder" in keypoint_info and "leftWrist" in keypoint_info:
             if (
                 self.gesture_to_detect == "raising_left_arm"
                 or self.gesture_to_detect is None
             ):
                 if keypoint_info["leftWrist"]["y"] < keypoint_info["leftShoulder"]["y"]:
-                    self.gesture_to_detect = "raising_left_arm"
+                    detected_gesture = "raising_left_arm"
             if (
                 self.gesture_to_detect == "pointing_to_the_left"
                 or self.gesture_to_detect is None
@@ -83,7 +93,7 @@ def execute(self, userdata):
                     keypoint_info["leftWrist"]["x"] - self.buffer_width
                     > keypoint_info["leftShoulder"]["x"]
                 ):
-                    self.gesture_to_detect = "pointing_to_the_left"
+                    detected_gesture = "pointing_to_the_left"
 
         if (
             "rightShoulder" in keypoint_info
@@ -99,7 +109,7 @@ def execute(self, userdata):
                     keypoint_info["rightWrist"]["y"]
                     < keypoint_info["rightShoulder"]["y"]
                 ):
-                    self.gesture_to_detect = "raising_right_arm"
+                    detected_gesture = "raising_right_arm"
             if (
                 self.gesture_to_detect == "pointing_to_the_right"
                 or self.gesture_to_detect is None
@@ -108,19 +118,47 @@ def execute(self, userdata):
                     keypoint_info["rightShoulder"]["x"] - self.buffer_width
                     > keypoint_info["rightWrist"]["x"]
                 ):
-                    self.gesture_to_detect = "pointing_to_the_right"
+                    detected_gesture = "pointing_to_the_right"
 
-        if self.gesture_to_detect is None:
-            self.gesture_to_detect = "none"
+        rospy.loginfo(f"Detected gesture: {detected_gesture}")
+        userdata.gesture_detected = detected_gesture
 
-        rospy.loginfo(f"Detected gesture: {self.gesture_to_detect}")
-        userdata.gesture_detected = self.gesture_to_detect
+        if not userdata.detection.point:
+            # take it a meter away from the robot position if no keypoints are detected
+            robot_pose = rospy.wait_for_message("/robot_pose", Pose)
+            userdata.person_point = Point(
+                x=robot_pose.position.x + 1, y=robot_pose.position.y
+            )
+        else:
+            _buffer = tf.Buffer(cache_time=rospy.Duration.from_sec(10.0))
+            _listener = tf.TransformListener(_buffer)
+            person_pose = PoseStamped(
+                header=Header(frame_id="map"),
+                pose=Pose(
+                    position=userdata.detection.point,
+                    orientation=Quaternion(0, 0, 0, 1),
+                ),
+            )
+            trans = _buffer.lookup_transform(
+                "odom", person_pose.header.frame_id, rospy.Time(0), rospy.Duration(1.0)
+            )
+            pose = do_transform_pose(person_pose, trans)
+            # userdata.person_point = pose.pose.position
+            create_and_publish_marker(
+                self.person_point_pub,
+                PointStamped(header=pose.header, point=pose.pose.position),
+                name="person_point_odom",
+                r=0.0,
+                g=0.0,
+                b=1.0,
+            )
+            userdata.person_point = pose.pose.position
 
         cv2_gesture_img = cv2_img.msg_to_cv2_img(userdata.img_msg)
         # Add text to the image
         cv2.putText(
             cv2_gesture_img,
-            self.gesture_to_detect,
+            detected_gesture,
             (10, 30),
             cv2.FONT_HERSHEY_SIMPLEX,
             1,