Merge remote-tracking branch 'base/gpsr-state-machine-factory' into HEAD

Author: jws-1

common/navigation/lasr_person_following/nodes/person_following.py

@@ -47,7 +47,7 @@ def __init__(self) -> None:
 
     def _execute_cb(self, _: FollowGoal) -> None:
         print("Executing follow_person action")
-        while not self._follower.begin_tracking(ask=False):
+        while not self._follower.begin_tracking():
             rospy.logwarn("No people found, retrying...")
             rospy.sleep(rospy.Duration(1.0))
         warnings.warn(

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

@@ -26,7 +26,7 @@
 from play_motion_msgs.msg import PlayMotionAction
 from play_motion_msgs.msg import PlayMotionGoal
 
-from lasr_vision_msgs.srv import DetectWaveRequest
+from lasr_vision_msgs.srv import DetectWaveRequest, DetectWaveResponse
 
 from math import atan2
 import numpy as np
@@ -79,17 +79,17 @@ def __init__(
         self,
         start_following_radius: float = 2.0,
         start_following_angle: float = 45.0,
-        n_secs_static_finished: float = 8.0,
-        n_secs_static_plan_close: float = 10.0,
         new_goal_threshold: float = 2.0,
         stopping_distance: float = 1.0,
         static_speed: float = 0.0015,
+        max_speed: float = 0.5,
     ):
         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._track_id = None
 
@@ -147,7 +147,9 @@ def _tf_pose(self, pose: PoseStamped, target_frame: str):
     def _robot_pose_in_odom(self) -> Union[PoseStamped, None]:
         try:
             current_pose: PoseWithCovarianceStamped = rospy.wait_for_message(
-                "/robot_pose", PoseWithCovarianceStamped
+                "/robot_pose",
+                PoseWithCovarianceStamped,
+                timeout=rospy.Duration.from_sec(2.0),
             )
         except rospy.ROSException:
             return None
@@ -208,10 +210,10 @@ def begin_tracking(self) -> bool:
 
     def _recover_track(self, say) -> bool:
         if not say:
-            self._tts("I SAW A person waving", wait=True)
+            self._tts("I see you are waving", wait=True)
 
         if self._tts_client_available and say:
-            self._tts("I lost track of you, please come back", wait=True)
+            self._tts("Please could you come back...", wait=True)
 
         while not self.begin_tracking() and not rospy.is_shutdown():
             rospy.loginfo("Recovering track...")
@@ -221,57 +223,62 @@ def _recover_track(self, say) -> bool:
 
         return True
 
-    # recover with vision, look up and check if person is waving
-    def _recover_vision(self, prev_pose: PoseStamped) -> bool:
-        # look up with playmotion and detect wave service
-        # if detected, begin tracking
-
-        # use play motion to look up
-        self._cancel_goal()
-
-        goal = PlayMotionGoal()
-        goal.motion_name = "look_centre"
-        self._play_motion.send_goal_and_wait(goal)
-
-        self._tts("Can you wave at me so that i can try to find you easily", wait=True)
-
-        # detect wave
+    def _detect_waving_person(self) -> DetectWaveResponse:
         try:
             pcl = rospy.wait_for_message("/xtion/depth_registered/points", PointCloud2)
             req = DetectWaveRequest()
             req.pcl_msg = pcl
             req.dataset = "resnet50"
             req.confidence = 0.1
             response = self._detect_wave(req)
-            if response.wave_detected:
-                rospy.loginfo("Wave detected, beginning tracking")
-                if np.isnan(response.wave_position.point.x) or np.isnan(
-                    response.wave_position.point.y
-                ):
-                    return False
-                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,
-                            ),
-                            orientation=Quaternion(0, 0, 0, 1),
-                        ),
-                        header=pcl.header,
-                    ),
-                    "map",
-                )
-                rospy.loginfo(goal_pose.pose.position)
-                goal_pose.pose.orientation = self._compute_face_quat(
-                    prev_pose.pose, goal_pose.pose
-                )
-                self._move_base(goal_pose)
-                return True
+            return response
         except rospy.ServiceException as e:
             rospy.loginfo(f"Error detecting wave: {e}")
-            return False
+            return DetectWaveResponse()
+
+    # recover with vision, look around and check if person is waving
+    def _recover_vision(self, 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)
+
+        for motion in self._vision_recovery_motions:
+            rospy.loginfo(f"Performing motion: {motion}")
+            goal = PlayMotionGoal()
+            goal.motion_name = motion
+            self._play_motion.send_goal_and_wait(goal)
+            for _ in range(self._vision_recovery_attempts):
+                response = self._detect_waving_person()
+                if response.wave_detected:
+                    if np.isnan(response.wave_position.point.x) or np.isnan(
+                        response.wave_position.point.y
+                    ):
+                        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,
+                                    ),
+                                    orientation=Quaternion(0, 0, 0, 1),
+                                ),
+                                header=prev_pose.header,
+                            ),
+                            "map",
+                        )
+                        goal_pose.pose.orientation = self._compute_face_quat(
+                            prev_pose.pose, goal_pose.pose
+                        )
+                        rospy.loginfo(goal_pose.pose.position)
+                        self._move_base(goal_pose)
+                        return True
+
+        return False
 
     def _euclidian_distance(self, p1: Pose, p2: Pose) -> float:
         return np.linalg.norm(
@@ -375,7 +382,7 @@ def follow(self) -> None:
         prev_goal: Union[None, PoseStamped] = None
         last_goal_time: Union[None, rospy.Time] = None
         going_to_person: bool = False
-        track_vels: [float] = []
+        track_vels: List[float] = []
 
         while not rospy.is_shutdown():
 
@@ -395,8 +402,27 @@ def follow(self) -> None:
             if track is None:
                 rospy.loginfo("Lost track of person, recovering...")
                 person_trajectory = PoseArray()
-                recovery = self._recover_vision(prev_goal)
-                self._recover_track(say=not recovery)
+                ask_back: bool = False
+
+                if prev_track is not None:
+                    robot_pose: PoseStamped = self._robot_pose_in_odom()
+                    if robot_pose:
+                        dist: float = self._euclidian_distance(
+                            robot_pose.pose, prev_track.pose
+                        )
+                        rospy.loginfo(f"Distance to last known position: {dist}")
+                        if dist >= MAX_VISION_DIST:
+                            ask_back = True
+                    else:
+                        ask_back = True
+                else:
+                    ask_back = True
+
+                if not ask_back:
+                    self._recover_track(say=not self._recover_vision(prev_goal))
+                else:
+                    self._recover_track(say=True)
+
                 prev_track = None
                 continue
 
@@ -418,6 +444,9 @@ 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:
+                self._tts("Please walk slower, I am struggling to keep up", wait=False)
+
             # Distance to the previous pose
             dist_to_prev = (
                 self._euclidian_distance(track.pose, prev_track.pose)
@@ -448,10 +477,51 @@ def follow(self) -> None:
             elif (
                 np.mean([np.linalg.norm(vel) for vel in track_vels])
                 < self._static_speed
-            ):
-
-                rospy.logwarn("Person has been static for too long, stopping")
+            ) and len(track_vels) == 10:
+                rospy.logwarn(
+                    "Person has been static for too long, going to them and stopping"
+                )
+                # cancel current goal
                 self._cancel_goal()
+
+                # 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

@@ -290,12 +290,12 @@ def parse_args() -> dict:
         help="Disable warming up the model by running inference on a test file.",
     )
 
-    # parser.add_argument(
-    #     "--energy_threshold",
-    #     type=int,
-    #     default=600,
-    #     help="Energy threshold for silence detection. Using this disables automatic adjustment",
-    # )
+    parser.add_argument(
+        "--energy_threshold",
+        type=Optional[int],
+        default=None,
+        help="Energy threshold for silence detection. Using this disables automatic adjustment",
+    )
 
     parser.add_argument(
         "--pause_threshold",

common/third_party/leg_tracker

@@ -152,10 +152,10 @@ def detect_3d(
                 f"Detected point: {detection.point} of object {detection.name}"
             )
 
-        markers.create_and_publish_marker(
-            debug_point_publisher,
-            PointStamped(point=detection.point, header=pcl_map.header),
-        )
+        # markers.create_and_publish_marker(
+        #     debug_point_publisher,
+        #     PointStamped(point=detection.point, header=pcl_map.header),
+        # )
 
         detected_objects.append(detection)
 

common/vision/lasr_vision_yolov8/src/lasr_vision_yolov8/yolo.py

@@ -31,7 +31,7 @@
     <include 
         file="$(find lasr_vision_cropped_detection)/launch/cropped_detection.launch"
     />
-    <include file="$(find lasr_vision_bodypix)/launch/bodypix.launch">
+    <include file="$(find lasr_vision_bodypix)/launch/bodypix.launch"/>
 
     <!-- To find the most similar commands -->
     <node 

tasks/gpsr/launch/setup.launch

@@ -0,0 +1,73 @@
+priors:
+  names:
+    - Adel
+    - Angel
+    - Axel
+    - Charlie
+    - Jane
+    - Jules
+    - Morgan
+    - Paris
+    - Robin
+    - Simone
+  drinks:
+    - cola
+    - iced tea
+    - juice pack
+    - milk
+    - orange juice
+    - red wine
+    - tropical juice
+  
+
+#WAIT POSE LAB:
+wait_pose: 
+  position: 
+    x: -15.027493553116143
+    y: 8.731164058220495
+    z: 0.0
+  orientation: 
+    x: 0.0
+    y: 0.0
+    z: -0.7865759968179794
+    w: 0.6174934827427752
+
+
+
+#556918144226074
+
+
+#0.478893309417269
+#0.8778731105321406
+
+
+#WAIT AREA LAB:
+# From robot POV: [top left, top right,bottom right, bottom left ] 
+wait_area: [[-15.9, 8.19], [-17.2, 8.11], [-16.2, 11.2], [-14.9, 10.0]]
+
+#Where to position self for seating guests
+seat_pose:
+  position: 
+    x: -13.659998294234812
+    y: 6.421172168922483
+    z: 0.0
+  orientation: 
+    x: 0.0
+    y: 0.0
+    z: -0.6021594916272762
+    w: 0.7983758179223494
+
+
+
+search_motions: ["look_left", "look_right"]
+sofa_point: 
+  x: -15.0
+  y: 4.21
+  z: 0.5
+
+seat_area: [[-12.2, 2.63], [-18.2, 4.61], [-17.4, 7.31], [-11.7, 5.06]]
+max_people_on_sofa: 2
+
+sofa_area: [[-13.8, 4.61], [-14.4, 3.54], [-15.85, 4.16], [-15.4, 5.33]]
+
+sweep: true