feat: successful run with many many people before inspection

LASR-at-Home · Jul 17, 2024 · 1ce6dfa · 1ce6dfa
1 parent eccde99
commit 1ce6dfa
Showing 1 changed file with 22 additions and 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
@@ -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 (