From eccde99eea14369603643e84bac949d5b5270f5e Mon Sep 17 00:00:00 2001
From: Jared Swift <j.w.swift@outlook.com>
Date: Wed, 17 Jul 2024 12:10:04 +0200
Subject: [PATCH] feat: return waypoints in result.

---
 .../lasr_person_following/CMakeLists.txt          |  2 ++
 .../lasr_person_following/action/Follow.action    |  1 +
 .../src/lasr_person_following/person_following.py | 15 +++++++++++++--
 3 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/common/navigation/lasr_person_following/CMakeLists.txt b/common/navigation/lasr_person_following/CMakeLists.txt
index 660acec3d..5888e92ec 100644
--- a/common/navigation/lasr_person_following/CMakeLists.txt
+++ b/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
 )
 
 ################################################
diff --git a/common/navigation/lasr_person_following/action/Follow.action b/common/navigation/lasr_person_following/action/Follow.action
index 97e350ec9..8c3685d5a 100644
--- a/common/navigation/lasr_person_following/action/Follow.action
+++ b/common/navigation/lasr_person_following/action/Follow.action
@@ -1,4 +1,5 @@
 ---
 #result definition
+geometry_msgs/Pose[] waypoints
 ---
 #feedback
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 4af388364..7651cf942 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
@@ -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