From 1ce6dfab12242012ddd40705c19cfae97036eada Mon Sep 17 00:00:00 2001
From: fireblonde <nicollehchevska@gmail.com>
Date: Wed, 17 Jul 2024 15:23:38 +0100
Subject: [PATCH] feat: successful run with many many people before inspection

---
 .../lasr_person_following/person_following.py | 44 +++++++++----------
 1 file changed, 22 insertions(+), 22 deletions(-)

diff --git a/common/navigation/lasr_person_following/src/lasr_person_following/person_following.py b/common/navigation/lasr_person_following/src/lasr_person_following/person_following.py
index 7651cf94..71f9a5f2 100644
--- a/common/navigation/lasr_person_following/src/lasr_person_following/person_following.py
+++ b/common/navigation/lasr_person_following/src/lasr_person_following/person_following.py
@@ -420,27 +420,27 @@ def follow(self) -> FollowResult:
                 rospy.loginfo("Lost track of person, recovering...")
                 person_trajectory = PoseArray()
                 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(robot_pose, prev_goal)
-                    )
-                else:
-                    self._recover_track(say=True)
+                self._recover_track(say=True)
+                # 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(robot_pose, prev_goal)
+                #     )
+                # else:
+                #     self._recover_track(say=True)
 
                 prev_track = None
                 continue
@@ -498,7 +498,7 @@ def follow(self) -> FollowResult:
             ):
                 rospy.logwarn("Goal was aborted, retrying")
                 self._move_base(prev_goal)
-                result.waypoints.append(prev_goal)
+                #result.waypoints.append(prev_goal)
                 # self._waypoints.append(prev_goal)
             # check if the person has been static for too long
             elif (