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])