From 5097dfc4a182ba23bef2521cf6843e1a057c5522 Mon Sep 17 00:00:00 2001
From: Cameron Brown <me@cbrxyz.com>
Date: Fri, 8 Nov 2024 23:43:07 -0500
Subject: [PATCH] navigator_missions: Add TD feedback, color sequence
 derivation for wildlife

---
 .../navigator_launch/config/poi_sim.yaml      |  2 +-
 .../navigator_missions/wildlife_2024.py       | 42 +++++++++++++++++--
 .../wildlife_20m_triangle_no_timer.world      |  2 +-
 3 files changed, 41 insertions(+), 5 deletions(-)

diff --git a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml b/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml
index 273c9c99d..2a27948a7 100644
--- a/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml
+++ b/NaviGator/mission_control/navigator_launch/config/poi_sim.yaml
@@ -1,7 +1,7 @@
 ---
 global_frame: enu
 initial_pois:
-  circle_totems: [26.09821319580078, 59.91523361206055, 0.0]
+  wildlife: [26.09821319580078, 59.91523361206055, 0.0]
   dock: [108.05, -1.07, 0.0]
   entrance_gate: [51.28, -48.69, 0.0]
   obstacle_course: [-32.803, -83.41, 0.0]
diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py
index 03c08f701..7e1b4c81b 100644
--- a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py
+++ b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py
@@ -3,10 +3,10 @@
 from enum import Enum
 
 import numpy as np
-import rospy
 from mil_msgs.msg import ObjectsInImage
 from mil_msgs.srv import CameraToLidarTransform
 from mil_tools import rosmsg_to_numpy
+from navigator_msgs.srv import MessageWildlifeEncounter, MessageWildlifeEncounterRequest
 
 from .navigator import NaviGatorMission
 
@@ -227,7 +227,7 @@ def get_indices_of_most_confident_animals(
 
     async def find_wildlife(self):
         robot_position = (await self.tx_pose())[0]
-        self.send_feedback("FINDING WILDLIFE")
+        self.send_feedback("[wildlife] FINDING WILDLIFE")
 
         def filter_and_sort(objects, positions):
             distances = np.linalg.norm(positions - robot_position, axis=1)
@@ -263,8 +263,26 @@ def is_done(objects, positions):
             # Update explore dict
             self.animals_observed[label] = True
             if label == self.chosen_animal:
+                self.send_feedback(
+                    f"[wildlife] starting circle of {self.chosen_animal}",
+                )
+                td_animal_labels = {
+                    "red_python_buoy": "P",
+                    "green_iguana_buoy": "I",
+                    "blue_manatee_buoy": "M",
+                }
+                self.send_feedback("[wildlife] sending feedback")
+                await self.td_feedback(
+                    MessageWildlifeEncounterRequest(
+                        circling_wildlife=td_animal_labels.get(label, "P"),
+                        clockwise=label == "blue_manatee_buoy",
+                        number_of_circles=2 if label == "red_python_buoy" else 1,
+                    ),
+                )
+                self.send_feedback("[wildlife] sent feedback!")
                 await self.circle_animal(animal)
                 if self.chosen_animal == "red_python_buoy":
+                    self.send_feedback("[wildlife] starting double circle of red")
                     await self.circle_animal(animal)
 
         # # Check if all wildlife has been circled
@@ -283,7 +301,25 @@ def is_done(objects, positions):
     async def run(self, args):
         # Check nearest objects
         self.objects_passed = set()
-        self.chosen_animal = rospy.get_param("chosen_animal", "red_python_buoy")
+        try:
+            self.color_sequence = await self.nh.get_param("color_sequence", str)
+        except TypeError:
+            print("invalid color sequence type")
+            self.color_sequence = None
+        if not self.color_sequence:
+            self.color_sequence = "RGB"
+            self.send_feedback("Invalid color sequence, defaulting to RGB")
+        animals = {
+            "R": "red_python_buoy",
+            "G": "green_iguana_buoy",
+            "B": "blue_manatee_buoy",
+        }
+        self.chosen_animal = animals.get(self.color_sequence[0], "red_python_buoy")
+        self.send_feedback(f"[wildlife] circling {self.chosen_animal}")
+        self.td_feedback = self.nh.get_service_client(
+            "/wildlife_encounter_message",
+            MessageWildlifeEncounter,
+        )
         await self.change_wrench("autonomous")
         # Wait a bit for PCDAR to get setup
         # await self.set_classifier_enabled.wait_for_service()
diff --git a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_20m_triangle_no_timer.world b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_20m_triangle_no_timer.world
index b65d8b47b..1f40bf279 100644
--- a/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_20m_triangle_no_timer.world
+++ b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_20m_triangle_no_timer.world
@@ -158,7 +158,7 @@
       <!-- Wind -->
       <wind_direction>180</wind_direction>
       <!-- in degrees -->
-      <wind_mean_velocity>8.0</wind_mean_velocity>
+      <wind_mean_velocity>4.0</wind_mean_velocity>
       <var_wind_gain_constants>8.0</var_wind_gain_constants>
       <var_wind_time_constants>20</var_wind_time_constants>
       <random_seed>10</random_seed>