Updated debug messages for wild life task

NaviGator/mission_control/navigator_missions/navigator_missions/wildlife.py

@@ -34,4 +34,5 @@ async def run(self, parameters):
 
             print("going to nearest small object")
 
+        print(f"Spiraling the point: {t1}")
         await self.move.d_spiral_point(t1, 5, 4, 1, "ccw", theta_offset=-1.57)

NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py

@@ -1,27 +1,27 @@
 #!/usr/bin/env python3
-import numpy as np
 import asyncio
 from enum import Enum
-from geometry_msgs.msg import Point
+
+import numpy as np
 from mil_msgs.msg import ObjectsInImage
-from mil_msgs.srv import CameraToLidarTransform, CameraToLidarTransformRequest
+from mil_msgs.srv import CameraToLidarTransform
 from mil_tools import rosmsg_to_numpy
 
-from std_srvs.srv import SetBoolRequest
-
 from .navigator import NaviGatorMission
 
+
 class MoveState(Enum):
     NOT_STARTED = 1
     RUNNING = 2
     CANCELLED = 3
     FINISHED = 4
 
+
 class Wildlife2024(NaviGatorMission):
     animals_observed = {
-        "blue_manatee_buoy" : False,    # Manatee => Counter clockwise
-        "green_iguana_buoy" : False,    # Iguana => Clockwise (by choice)
-        "red_python_buoy" : False     # Python => Clockwise
+        "blue_manatee_buoy": False,  # Manatee => Counter clockwise
+        "green_iguana_buoy": False,  # Iguana => Clockwise (by choice)
+        "red_python_buoy": False,  # Python => Clockwise
     }
 
     @classmethod
@@ -33,7 +33,7 @@ async def setup(cls):
             "/wamv/sensors/camera/front_right_cam/image_raw",
             CameraToLidarTransform,
         )
-    
+
     @classmethod
     async def shutdown(cls):
         await cls.camsub.shutdown()
@@ -47,15 +47,15 @@ def get_indices_of_type(self, objects, classifications):
             if o.labeled_classification in classifications
             and o.id not in self.objects_passed
         ]
-    
+
     async def inspect_object(self, position):
         # Go in front of the object, looking directly at it
         try:
             await self.move.look_at(position).set_position(position).backward(6.0).go()
             await self.nh.sleep(5.0)
         except asyncio.CancelledError:
             print("Cancelled Inspection")
-    
+
     @staticmethod
     def object_classified(objects, obj_id):
         """
@@ -67,22 +67,19 @@ def object_classified(objects, obj_id):
             if obj.id == obj_id and obj.labeled_classification != "UNKNOWN":
                 return i
         return -1
-    
+
     def movement_finished(self, task):
         if not task.cancelled():
             print("THE MOVEMENT HAS FINISHED")
             self.current_move_task_state = MoveState.FINISHED
-    
+
     # Explore until we find one of the animals we have not seen before
-    async def explore_closest_until(self, is_done, filter_and_sort)->dict:
+    async def explore_closest_until(self, is_done, filter_and_sort) -> dict:
         """
         @condition func taking in sorted objects, positions
         @object_filter func filters and sorts
         """
-        print("HERE")
         move_id_tuple = None
-        init_boat_pos = self.pose[0]
-        cone_buoys_investigated = 0  # max will be 2
         service_req = None
         investigated = set()
         move_task = None
@@ -146,7 +143,8 @@ async def explore_closest_until(self, is_done, filter_and_sort)->dict:
 
             # Exit if done
             ret = is_done(objects, positions)
-            self.send_feedback(f"Analyzing objects: {ret}")
+            labels = [obj[0].labeled_classification for obj in ret]
+            self.send_feedback(f"Analyzing objects: {labels}")
             if ret is not None:
                 if move_id_tuple is not None:
                     self.send_feedback("Condition met. Canceling investigation")
@@ -166,9 +164,7 @@ async def explore_closest_until(self, is_done, filter_and_sort)->dict:
             shortest_distance = 1000
 
             for i in range(len(objects)):
-                if (
-                    objects[i].id not in investigated
-                ):
+                if objects[i].id not in investigated:
                     distance = np.linalg.norm(positions[i] - self.pose[0])
                     if distance < shortest_distance:
                         shortest_distance = distance
@@ -180,7 +176,7 @@ async def explore_closest_until(self, is_done, filter_and_sort)->dict:
                         )
                         potential_candidate = i
                         print(positions[i])
-            
+
             if potential_candidate is not None:
                 # if there exists a closest buoy, go to it
                 self.send_feedback(f"Investigating {objects[potential_candidate].id}")
@@ -194,27 +190,43 @@ async def circle_animals(self, animals):
             object = animal[0]
             position = animal[1]
             label = object.labeled_classification
-            
+
             # Go to point and Circle animal
-            await self.move.d_spiral_point(position, 10, 4, 1, 
-                                           "cw"
-                                           if label == "green_iguana_buoy" or label == "red_python_buoy"
-                                           else
-                                           "ccw", 
-                                           theta_offset=-1
-                                           )
+            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
+                ),
+            )
 
             # Update explore dict
             self.animals_observed[label] = True
 
-    def get_indices_of_most_confident_animals(self, objects, classifications=["red_python_buoy","blue_manatee_buoy", "green_iguana_buoy"])->dict:
+    def get_indices_of_most_confident_animals(
+        self,
+        objects,
+        classifications=["red_python_buoy", "blue_manatee_buoy", "green_iguana_buoy"],
+    ) -> dict:
         """Pass in sorted list of objects by distance from boat and extract the first instance of classifications"""
-        animals_dict = {classif:-1 for classif in classifications}
+        animals_dict = {classif: -1 for classif in classifications}
         for i, obj in enumerate(objects):
-            if obj.labeled_classification in classifications and animals_dict[obj.labeled_classification] == -1:
+            if (
+                obj.labeled_classification in classifications
+                and animals_dict[obj.labeled_classification] == -1
+            ):
                 animals_dict[obj.labeled_classification] = i
         return animals_dict
-    
+
     async def find_wildlife(self):
         robot_position = (await self.tx_pose())[0]
         self.send_feedback("FINDING WILDLIFE")
@@ -228,7 +240,7 @@ def is_done(objects, positions):
             res = self.get_indices_of_most_confident_animals(objects)
             # It is only done exploring if we detect an animal we have not circled or no objects were found
             found_new_animals = []
-            for key in res.keys():
+            for key in res:
                 if self.animals_observed[key] or res[key] == -1:
                     continue
                 else:
@@ -256,13 +268,12 @@ def is_done(objects, positions):
             if not found:
                 break
             count += 1
-        
+
         if count < 3:
             await self.find_wildlife()
         else:
             print("ALL WILDLIFE OBSERVED!")
 
-
     async def run(self, args):
         # Check nearest objects
         self.objects_passed = set()