From 1fa8c65081c63a3cabba277dc813fbb6c41ede81 Mon Sep 17 00:00:00 2001
From: Daniel Parra <dparra1@ufl.edu>
Date: Fri, 8 Nov 2024 06:59:21 -0500
Subject: [PATCH] Updated wildlife mission logic

---
 .../nodes/thrust_mapper.py                    |  5 +-
 .../navigator_missions/wildlife_2024.py       | 62 +++++++++++--------
 mil_common/axros                              |  2 +-
 3 files changed, 38 insertions(+), 31 deletions(-)

diff --git a/NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py b/NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py
index 0e7e5218a..e093c23d3 100755
--- a/NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py
+++ b/NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py
@@ -109,10 +109,7 @@ def publish_thrusts(self) -> None:
                 rospy.logfatal("Number of thrusts does not equal number of publishers")
                 return
             for i, pub in enumerate(self.publishers):
-                if pub.name == "/BL_motor/cmd":
-                    commands[i].setpoint = thrusts[i] * 0.85
-                else:
-                    commands[i].setpoint = thrusts[i]
+                commands[i].setpoint = thrusts[i]
         if not self.is_vrx and not self.is_sim:
             for i in range(len(self.publishers)):
                 self.joint_state_msg.effort[i] = commands[i].setpoint
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 b1318ab79..d95de503d 100644
--- a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py
+++ b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py
@@ -9,6 +9,8 @@
 
 from .navigator import NaviGatorMission
 
+CIRCLE_ANIMAL = "red_python_buoy"
+
 
 class MoveState(Enum):
     NOT_STARTED = 1
@@ -189,32 +191,31 @@ async def explore_closest_until(self, is_done, filter_and_sort) -> dict:
                 move_id_tuple = (move, objects[potential_candidate].id)
                 print("USING POTENTIAL CANDIDATE")
 
-    async def circle_animals(self, animals):
-        for animal in animals:
-            object = animal[0]
-            position = animal[1]
-            label = object.labeled_classification
-
-            # Go to point and Circle animal
-            await self.move.d_spiral_point(
-                position,
-                6,
-                4,
-                1,
-                (
-                    "cw"
-                    if label == "green_iguana_buoy" or label == "red_python_buoy"
-                    else "ccw"
-                ),
-                theta_offset=(
-                    1.57
-                    if label == "green_iguana_buoy" or label == "red_python_buoy"
-                    else -1.57
-                ),
-            )
+    async def circle_animal(self, animal):
+        object = animal[0]
+        position = animal[1]
+        label = object.labeled_classification
+
+        # Go to point and Circle animal
+        await self.move.d_spiral_point(
+            position,
+            6,
+            4,
+            1,
+            (
+                "ccw"
+                if label == "green_iguana_buoy" or label == "red_python_buoy"
+                else "cw"
+            ),
+            theta_offset=(
+                -1.57
+                if label == "green_iguana_buoy" or label == "red_python_buoy"
+                else 1.57
+            ),
+        )
 
-            # Update explore dict
-            self.animals_observed[label] = True
+        # Update explore dict
+        self.animals_observed[label] = True
 
     def get_indices_of_most_confident_animals(
         self,
@@ -263,7 +264,16 @@ def is_done(objects, positions):
         )
 
         # Go to each object and circle them accordingly
-        await self.circle_animals(animals)
+        for animal in animals:
+            object = animal[0]
+            # position = animal[1]
+            label = object.labeled_classification
+            if label == CIRCLE_ANIMAL:
+                print(f"CIRCLING ANIMAL {CIRCLE_ANIMAL}")
+                await self.circle_animal(animal)
+                if label == "red_python_buoy":
+                    await self.circle_animal(animal)
+            self.animals_observed[label] = True
 
         # # Check if all wildlife has been circled
         # IF not all wildlife has been found call this function again
diff --git a/mil_common/axros b/mil_common/axros
index a86842c93..d096c6746 160000
--- a/mil_common/axros
+++ b/mil_common/axros
@@ -1 +1 @@
-Subproject commit a86842c93370a62c0dad3fba0e6ef250137edf21
+Subproject commit d096c6746d597c6964e69933ddab0156096bfa78