Carry My Luggage (Part 2) (#215)

* WIP: improve following.

* Reorganise, and be more verbose.

* TODOs.

* feat: add criteria to begin following a person (they are the closest person inside a cone in front of the robot).

* fix: comment

* TODOs

* More TODOs.

* Updates after real robot experiment.

* feat: allow HandoverObject to do so vertically, and pass object through constructor.

* feat: full CML state machine)?).

* WIP: follower changes.

* fix: bodypix.

* feat: additional motions.

* fix: stop importing a file that doesn't exist.

* fix: get rid of non-existent debug flag.

* feat: rework handover and receive.

* fix: remove pointing_service from launch, and add bodypix.

* feat: full state machine.

* refactor: cleanup.

* refactor: use existing GetImage skill.

common/navigation/lasr_person_following/launch/person_following.launch

@@ -4,10 +4,11 @@
     <param name="forest_file" value="$(find leg_tracker)/config/trained_leg_detector_res=0.33.yaml" />
     <param name="scan_topic" value="/scan" />
     <param name="fixed_frame" value="odom"/>
-    <param name="scan_frequency" value="15"/>
-    <param name="max_detect_distance" value="7.5"/>
+    <param name="scan_frequency" value="10"/>
+    <param name="max_detect_distance" value="5.0"/>
     <param name="dist_travelled_together_to_initiate_leg_pair" value="1.0"/>
     <param name="display_detected_people" value="true"/>
+    <param name="max_leg_pairing_dist" value="0.5"/>
  </include> 
 
  <node pkg="lasr_person_following" type="person_following.py" name="person_following" output="screen" />

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

@@ -16,10 +16,13 @@
 from move_base_msgs.msg import MoveBaseAction, MoveBaseActionGoal, MoveBaseGoal
 from actionlib_msgs.msg import GoalStatus
 
-
+import rosservice
 import tf2_ros as tf
 import tf2_geometry_msgs  # type: ignore
 from tf2_geometry_msgs.tf2_geometry_msgs import do_transform_pose
+from nav_msgs.srv import GetPlan
+from nav_msgs.msg import Path
+
 
 from math import atan2
 import numpy as np
@@ -28,22 +31,35 @@
 
 class PersonFollower:
 
-    _track_sub: Union[None, rospy.Subscriber]
     _track_id: Union[None, int]
-    _breadcrumbs: List[PoseStamped]
-    _epsilon: float = 0.1
-    _n_secs_static: float = 5.0
-    _min_time_between_goals: float = 2.0
-    _stopping_distance: float = 0.5
+    _start_following_radius: float
+    _min_distance_between_tracks: float
+    _n_secs_static: float
+    _min_time_between_goals: float
+    _stopping_distance: float
     _move_base_client: actionlib.SimpleActionClient
     _buffer: tf.Buffer
     _listener: tf.TransformListener
     _goal_pose_pub: rospy.Publisher
+    _make_plan: rospy.ServiceProxy
+
+    def __init__(
+        self,
+        start_following_radius: float = 2.0,
+        start_following_angle: float = 45.0,
+        min_distance_between_tracks: float = 1.0,
+        n_secs_static: float = 15.0,
+        min_time_between_goals: float = 1.0,
+        stopping_distance: float = 1.0,
+    ):
+        self._start_following_radius = start_following_radius
+        self._start_following_angle = start_following_angle
+        self._min_distance_between_tracks = min_distance_between_tracks
+        self._n_secs_static = n_secs_static
+        self._min_time_between_goals = min_time_between_goals
+        self._stopping_distance = stopping_distance
 
-    def __init__(self):
-        self._track_sub = None
         self._track_id = None
-        self._breadcrumbs = []
 
         self._move_base_client = actionlib.SimpleActionClient(
             "move_base", MoveBaseAction
@@ -56,6 +72,9 @@ def __init__(self):
             "/follow_poses", PoseArray, queue_size=1000000, latch=True
         )
 
+        rospy.wait_for_service("/move_base/make_plan")
+        self._make_plan = rospy.ServiceProxy("/move_base/make_plan", GetPlan)
+
     def _tf_pose(self, pose: PoseStamped, target_frame: str):
         trans = self._buffer.lookup_transform(
             target_frame, pose.header.frame_id, rospy.Time(0), rospy.Duration(1.0)
@@ -81,77 +100,57 @@ def begin_tracking(self) -> bool:
         if len(people.people) == 0:
             return False
 
+        min_dist = np.inf
+        closest_person = None
         robot_pose = self._robot_pose_in_odom()
-
-        closest_person: Person = min(
-            people.people,
-            key=lambda person: self._euclidian_distance(person.pose, robot_pose.pose),
-        )
+        for person in people.people:
+            dist = self._euclidian_distance(person.pose, robot_pose.pose)
+
+            face_quat = self._compute_face_quat(robot_pose.pose, person.pose)
+            robot_dir = self._quat_to_dir(robot_pose.pose.orientation)
+            person_dir = self._quat_to_dir(face_quat)
+            angle = np.degrees(self._angle_between_vectors(robot_dir, person_dir))
+            rospy.loginfo(f"Person {person.id} is at {dist}m and {angle} degrees")
+            if (
+                dist < self._start_following_radius
+                and abs(angle) < self._start_following_angle
+            ):
+                if dist < min_dist:
+                    min_dist = dist
+                    closest_person = person
+
+        if not closest_person:
+            return False
 
         self._track_id = closest_person.id
 
         rospy.loginfo(f"Tracking person with ID {self._track_id}")
-        self._update_breadcrumbs(people)
-
         return True
 
     def _recover_track(self) -> bool:
-        self._track_sub.unregister()
-
-        people: PersonArray = rospy.wait_for_message("/people_tracked", PersonArray)
-
-        if len(people.people) == 0:
-            return False
-
-        if self._breadcrumbs:
-
-            closest_person: Person = min(
-                people.people,
-                key=lambda person: self._euclidian_distance(
-                    person.pose, self._breadcrumbs[0].pose
-                ),
-            )
-
-        else:
-
-            robot_pose: PoseStamped = self._robot_pose_in_odom()
-
-            closest_person: Person = min(
-                people.people,
-                key=lambda person: self._euclidian_distance(
-                    person.pose, robot_pose.pose
-                ),
-            )
-
-        self._track_id = closest_person.id
-
-        rospy.loginfo(f"Recovery: tracking person with ID {self._track_id}")
-        self._breadcrumbs = []
-        self._update_breadcrumbs(people)
-
-        self._track_sub = rospy.Subscriber(
-            "/people_tracked", PersonArray, self._update_breadcrumbs
-        )
-
         return True
 
-    def _update_breadcrumbs(self, tracks: PersonArray) -> None:
-        relevant_tracks: List[Person] = [
-            track for track in tracks.people if track.id == self._track_id
-        ]
-        poses = [
-            (PoseStamped(pose=track.pose, header=tracks.header))
-            for track in relevant_tracks
-        ]
-
-        self._breadcrumbs.extend(poses)
-
     def _euclidian_distance(self, p1: Pose, p2: Pose) -> float:
         return np.linalg.norm(
             np.array([p1.position.x, p1.position.y])
             - np.array([p2.position.x, p2.position.y])
         ).astype(float)
 
+    def _quat_to_dir(self, q: Quaternion):
+        x, y, z, w = q.x, q.y, q.z, q.w
+        forward = np.array(
+            [1 - 2 * (y**2 + z**2), 2 * (x * y - z * w), 2 * (x * z + y * w)]
+        )
+        return forward
+
+    def _angle_between_vectors(self, v1, v2):
+        dot_product = np.dot(v1, v2)
+        norms_product = np.linalg.norm(v1) * np.linalg.norm(v2)
+        cos_theta = dot_product / norms_product
+        cos_theta = np.clip(cos_theta, -1.0, 1.0)
+        angle = np.arccos(cos_theta)
+        return angle
+
     def _compute_face_quat(self, p1: Pose, p2: Pose) -> Quaternion:
         dx: float = p2.position.x - p1.position.x
         dy: float = p2.position.y - p1.position.y
@@ -166,42 +165,158 @@ def _cancel_goal(self) -> None:
         ]:
             self._move_base_client.cancel_goal()
 
+    def _check_finished(self) -> bool:
+        # TODO: ask the person if they are finished via speech.
+        return False
+
     def follow(self) -> None:
 
         prev_track: Union[None, Person] = None
         prev_goal: Union[None, MoveBaseActionGoal] = None
         poses = []
+
+        static_time: Union[None, rospy.Time] = None
+        static_for: Union[None, float] = None
+
         while not rospy.is_shutdown():
             tracks = rospy.wait_for_message("/people_tracked", PersonArray)
             current_track = next(
                 filter(lambda track: track.id == self._track_id, tracks.people), None
             )
             if current_track is None:
                 rospy.loginfo(f"Lost track of person with ID {self._track_id}")
+                # TODO: recovery behaviour, maybe begin_tracking with a reduced radius?
                 break
 
-            if prev_track is not None:
-                # If the poses are significantly different, update the most recent pose
-                if self._euclidian_distance(prev_track.pose, current_track.pose) < 0.5:
-                    rospy.loginfo("Person too close to previous one, skipping")
-                    continue
-
             robot_pose = self._robot_pose_in_odom()
 
-            if self._euclidian_distance(robot_pose.pose, current_track.pose) < 1.0:
-                rospy.loginfo("Person too close to robot, skipping")
+            too_soon: bool = False
+            too_close: bool = False
+            too_close_to_prev: bool = False
+
+            time_since_last_goal: float = (
+                rospy.Time.now().secs - prev_goal.target_pose.header.stamp.secs
+                if prev_goal is not None
+                else np.inf
+            )
+            too_soon = (
+                time_since_last_goal < self._min_time_between_goals
+                and self._move_base_client.get_state()
+                in [
+                    GoalStatus.PENDING,
+                    GoalStatus.ACTIVE,
+                ]
+            )
+
+            dist_to_goal: float = self._euclidian_distance(
+                robot_pose.pose, current_track.pose
+            )
+
+            too_close = dist_to_goal < self._stopping_distance
+
+            dist_to_prev = (
+                self._euclidian_distance(current_track.pose, prev_track.pose)
+                if prev_track is not None
+                else np.inf
+            )
+
+            too_close_to_prev = dist_to_prev < self._min_distance_between_tracks
+
+            if too_close_to_prev:
+                if static_time is None:
+                    static_time = rospy.Time.now()
+                static_for = rospy.Time.now().secs - static_time.secs
+
+            rospy.loginfo(
+                f"Too soon: {too_soon} ({time_since_last_goal}), too close: {too_close} ({dist_to_goal}), too close to prev: {too_close_to_prev} ({dist_to_prev} for {static_for})"
+            )
+
+            if too_close_to_prev:
+                if static_for >= self._n_secs_static:
+                    rospy.loginfo(
+                        f"Person has been static for {static_for} seconds, checking if finished"
+                    )
+                    if self._check_finished():
+                        rospy.loginfo("Stopping...")
+                        return
+                    else:
+                        rospy.loginfo("Person not finished, continuing")
+                        static_time = rospy.Time.now()
+
+                rospy.loginfo("Person too close to previous, skipping")
+                continue
+
+            if too_close:
+                rospy.loginfo("Person too close to robot, facing them and skipping")
+                pose = PoseStamped(header=tracks.header)
+                pose.pose = robot_pose.pose
+                pose.pose.orientation = self._compute_face_quat(
+                    robot_pose.pose, current_track.pose
+                )
+                goal = MoveBaseGoal()
+                goal.target_pose = self._tf_pose(pose, "map")
+                goal.target_pose.header.stamp = rospy.Time.now()
+                self._move_base_client.send_goal(goal)
+                prev_track = current_track
+                prev_goal = goal
+                continue
+
+            if too_soon:
+                rospy.loginfo("Too soon, skipping")
+                prev_track = current_track
                 continue
 
-            goal_pose = PoseStamped(pose=current_track.pose, header=tracks.header)
+            static_time = None
+            static_for = None
+
+            robot_pose_map = self._tf_pose(robot_pose, "map")
+            current_track_pose_map = self._tf_pose(
+                PoseStamped(pose=current_track.pose, header=tracks.header), "map"
+            )
+
+            if self._move_base_client.get_state() in [
+                GoalStatus.PENDING,
+                GoalStatus.ACTIVE,
+            ]:
+                self._cancel_goal()
+            try:
+                plan = self._make_plan(
+                    robot_pose_map, current_track_pose_map, self._stopping_distance
+                ).plan
+            except rospy.ServiceException:
+                rospy.loginfo("Failed to find a plan, skipping")
+                continue
+            if len(plan.poses) == 0:
+                rospy.loginfo("Failed to find a plan, skipping")
+                rospy.loginfo(robot_pose_map)
+                rospy.loginfo(current_track_pose_map)
+                continue
+
+            print(plan.poses)
+
+            # select a pose that is stopping_distance away from the goal_pose
+            for pose in reversed(plan.poses):
+                if (
+                    self._euclidian_distance(pose.pose, current_track_pose_map.pose)
+                    > self._stopping_distance
+                ):
+                    goal_pose = pose
+                    print(f"Selected pose {goal_pose.pose.position}")
+                    break
+
             goal = MoveBaseGoal()
-            goal.target_pose = self._tf_pose(goal_pose, "map")
+            goal_pose.pose.orientation = self._compute_face_quat(
+                goal_pose.pose, current_track_pose_map.pose
+            )
+            goal.target_pose = goal_pose
             goal.target_pose.header.stamp = rospy.Time.now()
 
             poses.append(goal.target_pose.pose)
             self._move_base_client.send_goal(goal)
+            rospy.loginfo(f"Sent goal to {goal.target_pose.pose.position}")
 
             prev_track = current_track
-
+            prev_goal = goal
             pose_array = PoseArray()
             pose_array.header = goal.target_pose.header
             pose_array.poses = poses