diff --git a/NaviGator/mission_control/navigator_launch/config/camera_calibration/dell.yaml b/NaviGator/mission_control/navigator_launch/config/camera_calibration/dell.yaml index ca760aabe..3f540b3d9 100644 --- a/NaviGator/mission_control/navigator_launch/config/camera_calibration/dell.yaml +++ b/NaviGator/mission_control/navigator_launch/config/camera_calibration/dell.yaml @@ -1,6 +1,6 @@ --- -image_width: 1920 -image_height: 1080 +image_width: 640 +image_height: 480 camera_name: head_camera camera_matrix: rows: 3 diff --git a/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/dell_right.launch b/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/dell_right.launch index c9c48093a..f60658313 100644 --- a/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/dell_right.launch +++ b/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/dell_right.launch @@ -1,19 +1,18 @@ - - + + - - - + + + - - + diff --git a/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/front_cameras.launch b/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/front_cameras.launch index 9bd832f5c..c4c0de246 100644 --- a/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/front_cameras.launch +++ b/NaviGator/mission_control/navigator_launch/launch/hardware/cameras/front_cameras.launch @@ -1,6 +1,6 @@ - + diff --git a/NaviGator/mission_control/navigator_launch/launch/hardware/kill_board.launch b/NaviGator/mission_control/navigator_launch/launch/hardware/kill_board.launch index 55cc23647..0cd3c3c01 100644 --- a/NaviGator/mission_control/navigator_launch/launch/hardware/kill_board.launch +++ b/NaviGator/mission_control/navigator_launch/launch/hardware/kill_board.launch @@ -1,6 +1,6 @@ - + diff --git a/NaviGator/mission_control/navigator_launch/launch/master.launch b/NaviGator/mission_control/navigator_launch/launch/master.launch index 80c0cc849..639593a62 100644 --- a/NaviGator/mission_control/navigator_launch/launch/master.launch +++ b/NaviGator/mission_control/navigator_launch/launch/master.launch @@ -10,7 +10,7 @@ - + diff --git a/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch b/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch index f5d795b50..6f365998e 100644 --- a/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch +++ b/NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch @@ -1,68 +1,67 @@ - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - - + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - + + + + + + + diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py index b40136d8d..f51865b0b 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigation.py @@ -290,10 +290,18 @@ async def explore_closest_until(self, is_done, filter_and_sort): # and find closest one within 25 meters (max width of a gate). if cone_buoys_investigated > 0 and potential_candidate is None: for i in range(len(objects)): - print(positions[i]) + obj_vol = ( + objects[i].scale.x * objects[i].scale.y * objects[i].scale.z + ) + + # Catch case where we invesitegate the shore + if obj_vol > 3: + continue + if ( objects[i].id not in investigated and "round" not in objects[i].labeled_classification + and obj_vol < 3 ): distance = np.linalg.norm(positions[i] - self.pose[0]) if distance < shortest_distance and distance <= 25: @@ -308,9 +316,18 @@ async def explore_closest_until(self, is_done, filter_and_sort): # if that doesn't produce any results, literally just go to closest buoy if potential_candidate is None: for i in range(len(objects)): + obj_vol = ( + objects[i].scale.x * objects[i].scale.y * objects[i].scale.z + ) + + # Catch case where we invesitegate the shore + if obj_vol > 3: + continue + if ( objects[i].id not in investigated and "round" not in objects[i].labeled_classification + and obj_vol < 3 ): distance = np.linalg.norm(positions[i] - self.pose[0]) if distance < shortest_distance: diff --git a/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py index 36c78fcbc..47ce2ac8a 100644 --- a/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py +++ b/NaviGator/mission_control/navigator_missions/navigator_missions/navigator.py @@ -3,6 +3,7 @@ import asyncio import os +from enum import IntEnum from typing import Callable import genpy @@ -42,6 +43,16 @@ from .pose_editor import PoseEditor2 +class UAVModes(IntEnum): + """ + Enumerates constants of friendly uav mode names to ints + """ + + STOWED = 1 + DEPLOYED = 2 + FAULTED = 3 + + class MissionResult: NoResponse = 0 ParamNotFound = 1 @@ -130,6 +141,7 @@ async def setup_base(cls, mission_runner): ) cls.pose = None + cls.uav_status = UAVModes.STOWED def odom_set(odom: Odometry): return setattr(cls, "pose", mil_tools.odometry_to_numpy(odom)[0])