diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife.py b/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife.py index 77dc480aa..9e3225bb8 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife.py +++ b/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) 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 66f685a9a..ae826e041 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/wildlife_2024.py +++ b/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,7 +47,7 @@ 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: @@ -55,7 +55,7 @@ async def inspect_object(self, position): 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() 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 new file mode 100644 index 000000000..b65d8b47b --- /dev/null +++ b/NaviGator/simulation/navigator_gazebo/worlds/wildlife/wildlife_20m_triangle_no_timer.world @@ -0,0 +1,198 @@ + + + + + + + EARTH_WGS84 + ENU + -33.724223 + 150.679736 + 0.0 + 0.0 + + + + model://sun + + + + -478.101746 148.200836 13.203143 0.0 0.248344 2.936862 + orbit + + + + + + 0 0 0.2 0 0 0 + model://sydney_regatta + + + + post_0 + -535.916809 154.362869 0.675884 -0.17182 0.030464 -0.005286 + model://post + + + post_1 + -527.48999 153.854782 0.425844 -0.1365 0 0 + model://post + + + post_2 + -544.832825 156.671951 0.499025 -0.162625 0 0 + model://post + + + + -531.063721 147.668579 1.59471 -0.068142 0 -0.1 + model://antenna + + + + ground_station_0 + -540.796448 146.493744 1.671421 -0.00834 0.01824 1.301726 + model://ground_station + + + ground_station_1 + -537.622681 145.827896 1.681931 -0.00603 0.018667 1.301571 + model://ground_station + + + ground_station_2 + -534.550537 144.910400 1.720474 -0.004994 0.020798 1.301492 + model://ground_station + + + + blue_projectile + -545 60 0.03 0 0 0 + model://blue_projectile + + + true + + false + 30 + 1000 1000 + 50 50 + + PMS + 2.0 + 3 + 2.5 + 1.0 + 1.0 0.0 + 0.5 + 2.0 + 0.0 + + 0.0 + + + + + + true + 0.1 + 0.2 + 0.2 + + PMS + 2.0.0 + 3 + 2.5 + 1.0 + 1.0 0.0 + 0.5 + 2.0 + 0.0 + + 0.0 + + + + + 2.5 2.5 1 + model://ocean_waves/meshes/mesh.dae + + + + + + -1 + + + 0 0 -0.05 0 0 0 + + + + 2.5 2.5 1 + model://ocean_waves/meshes/mesh_below.dae + + + + + + -1 + + + + + + + + wamv + wamv/base_link + .5 .5 .33 + + + 180 + + 8.0 + 8.0 + 20 + 10 + + 10 + /vrx/debug/wind/speed + /vrx/debug/wind/direction + + + + + 12 + + + 0 + 0 + + +1 1 1 1 + + + + model://python_buoy + -510 215 0 0 0 0 + + + iguana_buoy + -490 215 0 0 0 0 + model://iguana_buoy + + + manatee_buoy + -500 233 0 0 0 0 + model://manatee_buoy + + + diff --git a/mil_common/axros b/mil_common/axros index 30d6decda..a86842c93 160000 --- a/mil_common/axros +++ b/mil_common/axros @@ -1 +1 @@ -Subproject commit 30d6decda1d6595d6a5d99ae78ed13845985a3af +Subproject commit a86842c93370a62c0dad3fba0e6ef250137edf21 diff --git a/mil_common/perception/yolov7-ros b/mil_common/perception/yolov7-ros index 7ddd6b4f9..dba012183 160000 --- a/mil_common/perception/yolov7-ros +++ b/mil_common/perception/yolov7-ros @@ -1 +1 @@ -Subproject commit 7ddd6b4f9d04fde7048ed409aa2fa97c522b9678 +Subproject commit dba012183a5d2ab1035c232fa7174debdfdf37e9