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 ae826e041..03c08f701 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py @@ -3,6 +3,7 @@ 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 @@ -185,32 +186,29 @@ 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 - ), - ) - - # Update explore dict - self.animals_observed[label] = True + async def circle_animal(self, animal): + object = animal[0] + position = animal[1] + label = object.labeled_classification + self.send_feedback(f"Circling {label}...") + + # 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 + ), + ) def get_indices_of_most_confident_animals( self, @@ -259,7 +257,15 @@ 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] + label = object.labeled_classification + # Update explore dict + self.animals_observed[label] = True + if label == self.chosen_animal: + await self.circle_animal(animal) + if self.chosen_animal == "red_python_buoy": + await self.circle_animal(animal) # # Check if all wildlife has been circled # IF not all wildlife has been found call this function again @@ -277,6 +283,7 @@ 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") 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/VRX/vrx b/NaviGator/simulation/VRX/vrx index e2dc3de4f..edecd95e3 160000 --- a/NaviGator/simulation/VRX/vrx +++ b/NaviGator/simulation/VRX/vrx @@ -1 +1 @@ -Subproject commit e2dc3de4f369a5c88747508da2c7f85b58ff0cbe +Subproject commit edecd95e38ff68e59d66f647448415222a4529ce diff --git a/SubjuGator/drivers/waterlinked_a50_ros_driver b/SubjuGator/drivers/waterlinked_a50_ros_driver new file mode 160000 index 000000000..fa1895247 --- /dev/null +++ b/SubjuGator/drivers/waterlinked_a50_ros_driver @@ -0,0 +1 @@ +Subproject commit fa1895247409cdfcf1b51059835fd88783220584 diff --git a/mil_common/perception/yolov7-ros b/mil_common/perception/yolov7-ros index 800224501..dba012183 160000 --- a/mil_common/perception/yolov7-ros +++ b/mil_common/perception/yolov7-ros @@ -1 +1 @@ -Subproject commit 800224501ed4a9b7aa4ba71a2dbd67d59a3373b8 +Subproject commit dba012183a5d2ab1035c232fa7174debdfdf37e9