Daniel updated wildlife

NaviGator/gnc/navigator_controller/nodes/mrac_controller.py

@@ -1,6 +1,7 @@
 #!/usr/bin/python3
 
 import traceback
+import rich
 
 import numpy as np
 import numpy.linalg as npl
@@ -591,6 +592,7 @@ def increment_reference(self) -> None:
         self.a_ref = (wrench_saturated[:2] - drag_ref[:2]) / self.mass_ref
         self.aa_ref = (wrench_saturated[5] - drag_ref[2]) / self.inertia_ref
         self.p_ref = self.p_ref + (self.v_ref * self.timestep)
+        # print(f"p_ref: {self.p_ref}")
         self.q_ref = trns.quaternion_from_euler(
             0,
             0,

NaviGator/gnc/navigator_path_planner/navigator_path_planner/params.py

@@ -12,7 +12,7 @@
 
 # BEHAVIOR CONTROL
 
-real_tol = [0.5, 0.5, np.deg2rad(10), np.inf, np.inf, np.inf]
+real_tol = [1, 1, np.deg2rad(10), np.inf, np.inf, np.inf]
 pointshoot_tol = np.deg2rad(20)  # rad
 free_radius = 6  # m
 basic_duration = 1  # s
@@ -43,7 +43,7 @@
 
 # SPEED AND THRUST LIMITS
 
-velmax_pos = np.array([2.4, 1.2, 0.22])  # (m/s, m/s, rad/s), body-frame forward
+velmax_pos = np.array([1.0, 1.2, 0.18])  # (m/s, m/s, rad/s), body-frame forward
 velmax_neg = np.array([-1.2, -1.2, -0.22])  # (m/s, m/s, rad/s), body-frame backward
 thrust_max = np.array([220, 220, 220, 220])  # N, per thruster
 

NaviGator/gnc/navigator_path_planner/nodes/path_planner.py

@@ -148,8 +148,10 @@ def __init__(
         rospy.sleep(1)
 
         # Timers
-        rospy.Timer(rospy.Duration(self.revisit_period), self.publish_ref)
+        # rospy.Timer(rospy.Duration(self.revisit_period), self.publish_ref)
         rospy.Timer(rospy.Duration(self.revisit_period), self.action_check)
+        # self.custom_task = threading.Thread(target=self.simple_timer)
+        # self.custom_task.start()
 
     def reset(self) -> None:
         """
@@ -1381,6 +1383,8 @@ def publish_ref(self, *args) -> None:
         publisher. Also publishes the reference effort as a WrenchStamped
         message to the effort publisher.
         """
+
+        # start = time.time()
         # Make sure a plan exists
         last_update_time = self.last_update_time
         if self.get_ref is None or last_update_time is None:

NaviGator/gnc/navigator_thrust_mapper/nodes/thrust_mapper.py

@@ -108,7 +108,7 @@ def publish_thrusts(self) -> None:
             if len(thrusts) != len(self.publishers):
                 rospy.logfatal("Number of thrusts does not equal number of publishers")
                 return
-            for i in range(len(self.publishers)):
+            for i, pub in enumerate(self.publishers):
                 commands[i].setpoint = thrusts[i]
         if not self.is_vrx and not self.is_sim:
             for i in range(len(self.publishers)):

NaviGator/hardware_drivers/navigator_kill_board/nodes/kill_board_driver.py

@@ -57,7 +57,7 @@ def __init__(self):
             except serial.SerialException as e:
                 rospy.logerr(f"Cannot connect to kill board. {e}")
                 self.publish_diagnostics(e)
-                rospy.sleep(1)
+                rospy.sleep(60)
         rospy.loginfo("Board connected!")
         self.board_status = {}
         for kill in constants["KILLS"]:

NaviGator/mission_control/navigator_launch/config/pcodar.yaml

@@ -26,4 +26,4 @@ intensity_filter_min_intensity: 10
 intensity_filter_max_intensity: 100
 
 # yamllint disable-line rule:line-length
-classifications: ["white_cylinder", "black_cylinder", "red_cylinder", "green_cylinder", "UNKNOWN"]
+classifications: ["white_cylinder", "black_cylinder", "red_cylinder", "green_cylinder", "blue_manatee_buoy", "green_iguana_buoy", "red_python_buoy", "UNKNOWN"]

NaviGator/mission_control/navigator_launch/config/pcodar_vrx.yaml

@@ -28,4 +28,4 @@ intensity_filter_max_intensity: 1
 # yamllint disable-line rule:line-length
 # classifications: ["buoy", "dock", "stc_platform", "red_totem", "green_totem", "blue_totem", "yellow_totem", "black_totem", "surmark46104", "surmark950400", "surmark950410", "UNKNOWN"]
 # yamllint disable-line rule:line-length
-classifications: ["mb_marker_buoy_red", "mb_marker_buoy_green", "mb_marker_buoy_black", "mb_marker_buoy_white", "mb_round_buoy_black", "mb_round_buoy_orange", "stc_platform", "UNKNOWN"]
+classifications: ["mb_marker_buoy_red", "mb_marker_buoy_green", "mb_marker_buoy_black", "mb_marker_buoy_white", "mb_round_buoy_black", "mb_round_buoy_orange", "stc_platform", "red_python_buoy", "blue_manatee_buoy", "green_iguana_buoy", "UNKNOWN"]

NaviGator/mission_control/navigator_launch/launch/master.launch

@@ -10,13 +10,13 @@
     <include if="$(arg online-bagger)" file="$(find navigator_launch)/launch/online_bagger.launch" />
     <include unless="$(arg simulation)" file="$(find navigator_launch)/launch/hardware.launch"/>
     <include unless="$(arg simulation)" file="$(find navigator_launch)/launch/perception/classifier.launch" >
-        <arg name="main_image_topic" value="/camera/front/left/image_color"/>
+        <arg name="main_image_topic" value="/camera/front/right/image_color"/>
         <arg name="use_yolo_model1" value="True"/>
         <arg name="use_yolo_model2" value="False"/>
-        <arg name="weights_model1" value="$(find yolov7_ros)/src/mil_weights/robotx.pt" />
-        <arg name="weights_model2" value="$(find yolov7_ros)/src/mil_weights/robotx.pt" />
-	<arg name="data_yaml_model1" value="$(find yolov7_ros)/src/data/buoys.yaml" />
-	<arg name="data_yaml_model2" value="$(find yolov7_ros/src/data/buoys.yaml" />
+        <arg name="weights_model1" value="$(find yolov7_ros)/src/mil_weights/robotx2024.pt" />
+        <arg name="weights_model2" value="$(find yolov7_ros)/src/mil_weights/robotx2024.pt" />
+	<arg name="data_yaml_model1" value="$(find yolov7_ros)/src/data/robotx2024.yaml" />
+	<arg name="data_yaml_model2" value="$(find yolov7_ros/src/data/robotx2024.yaml" />
     </include>
 
     <include file="$(find navigator_launch)/launch/gnc.launch">

NaviGator/mission_control/navigator_launch/launch/perception/classifier.launch

@@ -1,68 +1,67 @@
 <?xml version="1.0"?>
 <launch>
 
-    <arg name="weights_model1" default="$(find yolov7_ros)/src/mil_weights/robotx.pt" />
-    <arg name="weights_model2" default="$(find yolov7_ros)/src/mil_weights/robotx.pt" />
-    <arg name="weights_model3" default="$(find yolov7_ros)/src/mil_weights/stc.pt" />
-    <arg name="data_yaml_model1" default="$(find yolov7_ros)/src/data/vrx_buoys.yaml" />
-    <arg name="data_yaml_model2" default="$(find yolov7_ros)/src/data/vrx_buoys.yaml" />
-    <arg name="data_yaml_model3" default="$(find yolov7_ros)/src/data/stc.yaml" />
-    <arg name="main_image_topic" default="/camera/front/left/image_color"/>
-    <arg name="image_topic_model1" default="/yolov7/model1" />
-    <arg name="image_topic_model2" default="/yolov7/model2" />
-    <arg name="out_topic_model1" default="detections_model1" />
-    <arg name="out_topic_model2" default="detections_model2" />
-    <arg name="out_topic_model3" default="stc_detections_model" />
-    <arg name="conf_thresh" default="0.25" />
-    <arg name="image_size" default="640" />
-    <arg name="device" default="cpu" />
-    <arg name="use_yolo_model1" default="False"/>
-    <arg name="use_yolo_model2" default="False"/>
-    <arg name="use_yolo_model3" default="True"/>
+  <arg name="weights_model1" default="$(find yolov7_ros)/src/mil_weights/robotx2024.pt" />
+  <arg name="weights_model2" default="$(find yolov7_ros)/src/mil_weights/robotx.pt" />
+  <arg name="weights_model3" default="$(find yolov7_ros)/src/mil_weights/stc.pt" />
+  <arg name="data_yaml_model1" default="$(find yolov7_ros)/src/data/robotx2024.yaml" />
+  <arg name="data_yaml_model2" default="$(find yolov7_ros)/src/data/vrx_buoys.yaml" />
+  <arg name="data_yaml_model3" default="$(find yolov7_ros)/src/data/stc.yaml" />
+  <arg name="main_image_topic" default="/camera/front/right/image_color" />
+  <arg name="image_topic_model1" default="/yolov7/model1" />
+  <arg name="image_topic_model2" default="/yolov7/model2" />
+  <arg name="out_topic_model1" default="detections_model1" />
+  <arg name="out_topic_model2" default="detections_model2" />
+  <arg name="out_topic_model3" default="stc_detections_model" />
+  <arg name="conf_thresh" default="0.70" />
+  <arg name="image_size" default="640" />
+  <arg name="device" default="cpu" />
+  <arg name="use_yolo_model1" default="True" />
+  <arg name="use_yolo_model2" default="False" />
+  <arg name="use_yolo_model3" default="False" />
 
-    <node name="testing_arbiter" pkg="topic_tools" type="demux"
-		args="$(arg main_image_topic) $(arg image_topic_model1) $(arg image_topic_model2) /yolov7/none">
-		<remap from="demux" to="/yolov7"/>
-	</node>
+  <node name="testing_arbiter" pkg="topic_tools" type="demux" args="$(arg main_image_topic) $(arg image_topic_model1) $(arg image_topic_model2) /yolov7/none">
+    <remap from="demux" to="/yolov7" />
+  </node>
 
-    <!-- Use YOLOv7 -->
-    <include if="$(arg use_yolo_model1)" file="$(find yolov7_ros)/launch/yolov7.launch">
-        <arg name="weights" value="$(arg weights_model1)"/>
-        <arg name="image_topic" value="$(arg image_topic_model1)"/>
-        <arg name="out_topic" value="$(arg out_topic_model1)"/>
-        <arg name="conf_thresh" value="$(arg conf_thresh)"/>
-        <arg name="image_size" value="$(arg image_size)"/>
-	<arg name="device" value="$(arg device)"/>
-	<arg name="data_yaml" value="$(arg data_yaml_model1)"/>
-    </include>
+  <!-- Use YOLOv7 -->
+  <include if="$(arg use_yolo_model1)" file="$(find yolov7_ros)/launch/yolov7.launch">
+    <arg name="weights" value="$(arg weights_model1)" />
+    <arg name="image_topic" value="$(arg image_topic_model1)" />
+    <arg name="out_topic" value="$(arg out_topic_model1)" />
+    <arg name="conf_thresh" value="$(arg conf_thresh)" />
+    <arg name="image_size" value="$(arg image_size)" />
+    <arg name="device" value="$(arg device)" />
+    <arg name="data_yaml" value="$(arg data_yaml_model1)" />
+  </include>
 
-    <!-- Use YOLOv7 -->
-    <include if="$(arg use_yolo_model2)" file="$(find yolov7_ros)/launch/yolov7.launch">
-        <arg name="weights" value="$(arg weights_model2)"/>
-        <arg name="image_topic" value="$(arg image_topic_model2)"/>
-        <arg name="out_topic" value="$(arg out_topic_model2)"/>
-        <arg name="conf_thresh" value="$(arg conf_thresh)"/>
-        <arg name="image_size" value="$(arg image_size)"/>
-	<arg name="device" value="$(arg device)"/>
-	<arg name="data_yaml" value="$(arg data_yaml_model2)"/>
-    </include>
+  <!-- Use YOLOv7 -->
+  <include if="$(arg use_yolo_model2)" file="$(find yolov7_ros)/launch/yolov7.launch">
+    <arg name="weights" value="$(arg weights_model2)" />
+    <arg name="image_topic" value="$(arg image_topic_model2)" />
+    <arg name="out_topic" value="$(arg out_topic_model2)" />
+    <arg name="conf_thresh" value="$(arg conf_thresh)" />
+    <arg name="image_size" value="$(arg image_size)" />
+    <arg name="device" value="$(arg device)" />
+    <arg name="data_yaml" value="$(arg data_yaml_model2)" />
+  </include>
 
-    <!-- Use YOLOv7 -->
-    <include if="$(arg use_yolo_model3)" file="$(find yolov7_ros)/launch/yolov7.launch">
-        <arg name="weights" value="$(arg weights_model3)"/>
-        <arg name="image_topic" value="/stc_mask_debug"/>
-        <arg name="out_topic" value="$(arg out_topic_model3)"/>
-        <arg name="conf_thresh" value="$(arg conf_thresh)"/>
-        <arg name="image_size" value="$(arg image_size)"/>
-	<arg name="device" value="$(arg device)"/>
-	<arg name="data_yaml" value="$(arg data_yaml_model3)"/>
-    </include>
+  <!-- Use YOLOv7 -->
+  <include if="$(arg use_yolo_model3)" file="$(find yolov7_ros)/launch/yolov7.launch">
+    <arg name="weights" value="$(arg weights_model3)" />
+    <arg name="image_topic" value="/stc_mask_debug" />
+    <arg name="out_topic" value="$(arg out_topic_model3)" />
+    <arg name="conf_thresh" value="$(arg conf_thresh)" />
+    <arg name="image_size" value="$(arg image_size)" />
+    <arg name="device" value="$(arg device)" />
+    <arg name="data_yaml" value="$(arg data_yaml_model3)" />
+  </include>
 
-    <arg name="train" default="False" />
-    <node pkg="navigator_vision" type="classifier.py" name="classifier" output="screen">
-        <param name="debug" value="True" />
-        <param name="image_topic" value="$(arg main_image_topic)" />
-	    <param name="model_location" value="config/model" />
-        <param name="train" value="$(arg train)" />
-    </node>
+  <arg name="train" default="False" />
+  <node pkg="navigator_vision" type="classifier.py" name="classifier" output="screen">
+    <param name="debug" value="True" />
+    <param name="image_topic" value="$(arg main_image_topic)" />
+    <param name="model_location" value="config/model" />
+    <param name="train" value="$(arg train)" />
+  </node>
 </launch>

NaviGator/mission_control/navigator_missions/navigator_missions/__init__.py

@@ -25,6 +25,7 @@
 from .killed import Killed
 from .move import Move
 from .navigation import Navigation
+from .navigation_gatefinder import NavigationGatefinder
 from .navigator import NaviGatorMission
 from .obstacle_avoid import ObstacleAvoid
 from .pinger import PingerMission
@@ -41,6 +42,9 @@
 from .teleop import Teleop
 from .wildlife import Wildlife
 
+# RobotX 2024 Missions
+from .wildlife_2024 import Wildlife2024
+
 # Currently breaks mission server, TODO: fix or delete
 # from .track_target import TrackTarget
 

NaviGator/mission_control/navigator_missions/navigator_missions/autonomous_2024.py

@@ -6,6 +6,7 @@
 
 from .docking import Docking
 from .entrance_gate2 import EntranceGate2
+from .go_to_poi import GoToPOI
 from .navigation import Navigation
 from .navigator import NaviGatorMission
 from .scan_the_code import ScanTheCodeMission
@@ -17,28 +18,75 @@ class Autonomous2024(NaviGatorMission):
     # timeout (in secs)
     TIMEOUT = 180
 
-    async def run_mission(self, mission_cls: type[NaviGatorMission], name: str):
+    async def run_mission(
+        self,
+        mission_cls: type[NaviGatorMission],
+        name: str,
+        *args,
+        str_arg: str = "",
+        **kwargs,
+    ):
         rospy.loginfo(f"[autonomous] beginning {name}...")
         try:
-            await asyncio.wait_for(mission_cls().run(""), self.TIMEOUT)
+            await asyncio.wait_for(
+                mission_cls().run(str_arg, *args, **kwargs), self.TIMEOUT,
+            )
         except asyncio.TimeoutError:
             rospy.logwarn(f"[autonomous] ran out of time on {name}!")
 
     async def run(self, args: str):
         # Step 1: Entrance and exit gates
-        await self.run_mission(EntranceGate2, "entrance gate")
+        await self.send_feedback("[AUTONOMOUS] GOING TO ENTRANCE GATE")
+        await self.run_mission(GoToPOI, "entrance_gate", str_arg="entrance_gate")
+        await self.send_feedback("[AUTONOMOUS] STARTING AUTONOMOUS MISSION")
+        await self.run_mission(EntranceGate2, "entrance gate", scan_code=True)
+
+        # Step 1.5: Launch the drone
+        # FILL IN HERE
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-45 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(10)
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-35 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(10)
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-25 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(10)
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-15 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(10)
+        await self.send_feedback(
+            "[AUTONOMOUS] LAUNCHING THE DRONE, T-5 SEC TO CONTINUE",
+        )
+        await asyncio.sleep(5)
 
         # Step 2: Scan the Code
+        await self.send_feedback("[AUTONOMOUS] GOING TO SCAN THE CODE")
+        await self.run_mission(GoToPOI, "scan_the_code poi", str_arg="scan_the_code")
         await self.run_mission(ScanTheCodeMission, "scan the code")
 
         # Step 3: Wildlife Mission
+        await self.send_feedback("[AUTONOMOUS] GOING TO WILDLIFE")
+        await self.run_mission(GoToPOI, "wildlife poi", str_arg="wildlife")
         await self.run_mission(Wildlife, "wildlife")
 
         # Step 4: Navigation Mission
+        await self.send_feedback("[AUTONOMOUS] GOING TO NAVIGATION")
+        await self.run_mission(GoToPOI, "navigation poi", str_arg="navigation")
         await self.run_mission(Navigation, "navigation")
 
+        # Step 4.5: Receive the drone
+        # FILL IN HERE
+
         # Step 5: Dock Mission
+        await self.send_feedback("[AUTONOMOUS] GOING TO DOCKING")
+        await self.run_mission(GoToPOI, "docking poi", str_arg="docking")
         await self.run_mission(Docking, "docking")
 
-        # Step 6: UAV Mission
-        pass
+        # Step 6: Leave the course
+        await self.send_feedback("[AUTONOMOUS] LEAVING COURSE")
+        await self.run_mission(GoToPOI, "exit_gate poi", str_arg="exit_gate")

NaviGator/mission_control/navigator_missions/navigator_missions/entrance_gate2.py

@@ -3,23 +3,21 @@
 
 import numpy as np
 from mil_tools import quaternion_matrix
-from std_srvs.srv import SetBoolRequest
 
 from .navigator import NaviGatorMission
 
 
 class EntranceGate2(NaviGatorMission):
-    async def run(self, args):
+    async def run(self, args, *, scan_code=False):
         # Parameters:
-        scan_code = False
         return_to_start = True
-        circle_radius = 5
+        circle_radius = 10
         circle_direction = "cw"
         yaw_offset = 1.57
         self.traversal_distance = 3
 
-        await self.set_classifier_enabled.wait_for_service()
-        await self.set_classifier_enabled(SetBoolRequest(data=True))
+        # await self.set_classifier_enabled.wait_for_service()
+        # await self.set_classifier_enabled(SetBoolRequest(data=True))
 
         # Inspect Gates
         await self.change_wrench("/wrench/autonomous")

NaviGator/mission_control/navigator_missions/navigator_missions/go_to_poi.py

@@ -31,5 +31,5 @@ async def run(self, poi):
             f"Moving to {poi} at {position[0:2]} with orientation {orientation}",
         )
         await self.change_wrench("autonomous")
-        await self.move.to_pose(Pose(Point(*position), Quaternion(*orientation))).go()
+        await self.move.to_pose(Pose(Point(*position), Quaternion(*orientation))).go(blind=True)
         return "Success"

NaviGator/mission_control/navigator_missions/navigator_missions/move.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import asyncio
 import numpy as np
 from axros import types, util
 from mil_misc_tools import ThrowingArgumentParser
@@ -170,7 +171,7 @@ async def run(self, args):
 
                 msg = f"Moving {command} " if trans_move else f"Yawing {command[4:]} "
                 self.send_feedback(msg + f"{amount}{unit}")
-                res = await movement(float(amount), unit).go(**action_kwargs)
+                res = await asyncio.wait_for(movement(float(amount), unit).go(**action_kwargs), timeout=25.0)
             if not isinstance(res, types.ActionResult) and res.failure_reason != "":
                 raise Exception(f"Move failed. Reason: {res.failure_reason}")
         return "Move completed successfully!"