feat: return waypoints in result.

common/navigation/lasr_person_following/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   genmsg
   actionlib_msgs
   actionlib
+  geometry_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -73,6 +74,7 @@ add_action_files(
 generate_messages(
   DEPENDENCIES
   actionlib_msgs
+  geometry_msgs
 )
 
 ################################################

common/navigation/lasr_person_following/action/Follow.action

@@ -1,4 +1,5 @@
 ---
 #result definition
+geometry_msgs/Pose[] waypoints
 ---
 #feedback

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

@@ -40,6 +40,13 @@
 
 from pal_interaction_msgs.msg import TtsGoal, TtsAction
 
+from lasr_person_following.msg import (
+    FollowAction,
+    FollowGoal,
+    FollowResult,
+    FollowFeedback,
+)
+
 MAX_VISION_DIST: float = 5.0
 
 
@@ -381,7 +388,9 @@ def _tts(self, text: str, wait: bool) -> None:
             else:
                 self._tts_client.send_goal(tts_goal)
 
-    def follow(self) -> None:
+    def follow(self) -> FollowResult:
+
+        result = FollowResult()
 
         person_trajectory: PoseArray = PoseArray()
         person_trajectory.header.frame_id = "odom"
@@ -489,7 +498,8 @@ def follow(self) -> None:
             ):
                 rospy.logwarn("Goal was aborted, retrying")
                 self._move_base(prev_goal)
-                self._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 (
                 (
@@ -522,3 +532,4 @@ def follow(self) -> None:
             rospy.loginfo("")
             rospy.loginfo(np.mean([np.linalg.norm(vel) for vel in track_vels]))
             rospy.loginfo("")
+        return result