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 (