Merge master

.github/workflows/ci.yaml

@@ -120,7 +120,7 @@ jobs:
       # We need to install rsync for GitHub Pages deploy action
       - name: Install rsync
         run: |
-          sudo apt-get upgrade && sudo apt-get update && sudo apt-get install -y rsync
+          sudo apt-get update && sudo apt-get upgrade && sudo apt-get install -y rsync
 
       # Publish the artifact to the GitHub Pages branch
       - name: Push docs to GitHub Pages

.pre-commit-config.yaml

@@ -11,41 +11,41 @@ ci:
 
 repos:
   - repo: https://github.com/adrienverge/yamllint.git
-    rev: v1.31.0
+    rev: v1.32.0
     hooks:
       - id: yamllint
   - repo: https://github.com/psf/black
-    rev: 23.3.0
+    rev: 23.7.0
     hooks:
       - id: black
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v16.0.2
+    rev: v16.0.6
     hooks:
       - id: clang-format
   - repo: https://github.com/PyCQA/autoflake
-    rev: v2.1.1
+    rev: v2.2.0
     hooks:
       - id: autoflake
         args: [--remove-all-unused-imports, --ignore-init-module-imports]
   - repo: https://github.com/shellcheck-py/shellcheck-py
-    rev: v0.9.0.2
+    rev: v0.9.0.5
     hooks:
       - id: shellcheck
         exclude: ^docker|deprecated|NaviGator/simulation/VRX
         args: [--severity=warning, --exclude=SC1090]
   - repo: https://github.com/scop/pre-commit-shfmt
-    rev: v3.6.0-2
+    rev: v3.7.0-1
     hooks:
       - id: shfmt
         exclude: ^docker|deprecated|NaviGator/simulation/VRX
-  - repo: https://github.com/charliermarsh/ruff-pre-commit
+  - repo: https://github.com/astral-sh/ruff-pre-commit
     # Ruff version.
-    rev: 'v0.0.263'
+    rev: 'v0.0.282'
     hooks:
       - id: ruff
         args: [--fix, --exit-non-zero-on-fix]
   - repo: https://github.com/codespell-project/codespell
-    rev: v2.2.4
+    rev: v2.2.5
     hooks:
       - id: codespell
         args:

SubjuGator/command/subjugator_launch/config/adaptive_controller.yaml

@@ -1,6 +1,6 @@
 ---
-kp: 1000, 1000, 1000, 5000, 5000, 5000
-kd: 150, 150, 150, 50, 100, 25
-kg: 5,5,5,5,5,5
-ki: 5,5,5,5,10,5
-use_learned: false
+kp: 120, 150, 200, 100, 50, 50
+kd: 25, 25, 25, 25, 25, 25
+kg: 2.5,2.5,2.5,2.5,2.5,2.5
+ki: 2.5,2.5,2.5,2.5,2.5,2.5
+use_learned: true

SubjuGator/command/subjugator_launch/config/adaptive_controller_gazebo.yaml

@@ -0,0 +1,6 @@
+---
+kp: 1000, 1000, 1000, 5000, 5000, 5000
+kd: 150, 150, 150, 50, 100, 25
+kg: 5,5,5,5,5,5
+ki: 5,5,5,5,10,5
+use_learned: false

SubjuGator/command/subjugator_launch/launch/bag_debugging_controller.launch

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="prefix_name" default="controller_debug" />
+  <arg name="bag_name" default="$(find subjugator_controller)/debug_bags/$(arg prefix_name)" />
+  <node name="record" pkg="rosbag" type="record" args="-o $(arg bag_name) /adaptive_controller/pose_error /adaptive_controller/twist_error /adaptive_controller/adaptation /adaptive_controller/dist /adaptive_controller/drag /wrench /trajectory /odom /imu/data_raw /dvl/range">
+  </node>
+</launch>

SubjuGator/command/subjugator_launch/launch/can.launch

@@ -1,15 +1,13 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <!-- To run in simulated mode, set /is_simulation param True -->
-  <node pkg="mil_usb_to_can" type="driver.py" name="usb_to_can_driver">
+  <node pkg="mil_usb_to_can" type="sub8_driver.py" name="usb_to_can_driver">
     <rosparam command="delete" />
     <rosparam>
     # Path of serial device
-    port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A800GHCF-if00-port0
+    port: /dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A50285BI-if00-port0
     # Baudrate of device, should leave as is
     baudrate: 115200
-    # The CAN device id of the usb-can board
-    can_id: 0
     # List of python device handle classes
     device_handles:
       "18": sub8_thrust_and_kill_board.ThrusterAndKillBoard

SubjuGator/command/subjugator_launch/launch/sub8.launch

@@ -39,7 +39,9 @@
   <include if="$(eval environment != 'dynsim')" file="$(find subjugator_launch)/launch/subsystems/odometry.launch"/>
   <include file="$(find subjugator_launch)/launch/subsystems/thruster_mapper.launch"/>
 
-  <include if="$(arg use_adaptive_controller)" file="$(find subjugator_launch)/launch/subsystems/adaptive_controller.launch"/>
+  <include if="$(arg use_adaptive_controller)" file="$(find subjugator_launch)/launch/subsystems/adaptive_controller.launch">
+    <arg name="environment" value="$(arg environment)" />
+  </include>
   <include unless="$(arg use_adaptive_controller)" file="$(find subjugator_launch)/launch/subsystems/rise.launch"/>
 
   <include file="$(find subjugator_launch)/launch/subsystems/path_planner.launch" />

SubjuGator/command/subjugator_launch/launch/subsystems/adaptive_controller.launch

@@ -1,8 +1,10 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-  <arg name="debug" default="True" />
+  <arg name="debug" default="True"/>
+  <arg name="environment" default="real"/>
   <node pkg="subjugator_controller" type="adaptive_controller" name="adaptive_controller">
-    <rosparam param="" file="$(find subjugator_launch)/config/adaptive_controller.yaml" />
-    <param name="debug" value="$(arg debug)" />
+    <rosparam if="$(eval environment == 'real')" param="" file="$(find subjugator_launch)/config/adaptive_controller.yaml"/>
+    <rosparam if="$(eval environment == 'gazebo')" param="" file="$(find subjugator_launch)/config/adaptive_controller_gazebo.yaml"/>
+    <param name="debug" value="$(arg debug)"/>
   </node>
 </launch>

SubjuGator/command/subjugator_launch/package.xml

@@ -5,8 +5,10 @@
   <description>The subjugator_launch package</description>
   <maintainer email="[email protected]">Jacob Panikulam</maintainer>
   <license>MIT</license>
+  <build_depend>rosbag</build_depend>
   <build_depend>roslaunch</build_depend>
   <run_depend>roslaunch</run_depend>
+  <run_depend>rosbag</run_depend>
   <run_depend>robot_state_publisher</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>c3_trajectory_generator</run_depend>

SubjuGator/command/subjugator_launch/scripts/depth_conn

@@ -1,2 +1,2 @@
 #!/bin/sh
-exec socat -d -d pty,link=/tmp/depth,raw,echo=0 tcp:mil-sub-gumstix.ad.mil.ufl.edu:33056
+exec socat -d -d pty,link=/tmp/depth,raw,echo=0 tcp:192.168.37.61:33056

SubjuGator/command/subjugator_launch/scripts/dvl_conn

@@ -1,2 +1,2 @@
 #!/bin/sh
-exec socat -d -d pty,link=/tmp/dvl,raw,echo=0 tcp:mil-sub-gumstix.ad.mil.ufl.edu:349
+exec socat -d -d pty,link=/tmp/dvl,raw,echo=0 tcp:192.168.37.61:349

SubjuGator/command/subjugator_launch/scripts/imu_conn

@@ -1,2 +1,2 @@
 #!/bin/sh
-exec socat -d -d pipe:/tmp/imu,wronly=1 tcp:mil-sub-gumstix.ad.mil.ufl.edu:1382
+exec socat -d -d pipe:/tmp/imu,wronly=1 tcp:192.168.37.61:1382

SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/handle.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 import rospy
-from mil_usb_to_can import CANDeviceHandle
+from mil_usb_to_can.sub8 import CANDeviceHandle
 
 from sub8_actuator_board.srv import SetValve, SetValveRequest
 

SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/packets.py

@@ -1,6 +1,6 @@
 import struct
 
-from mil_usb_to_can import ApplicationPacket
+from mil_usb_to_can.sub8 import ApplicationPacket
 
 # CAN ID for the channel from MOBO to actuator board
 SEND_ID = 0x51

SubjuGator/drivers/sub8_actuator_board/sub8_actuator_board/simulation.py

@@ -1,5 +1,5 @@
 #!/usr/bin/python3
-from mil_usb_to_can import SimulatedCANDevice
+from mil_usb_to_can.sub8 import SimulatedCANDevice
 
 from .packets import SEND_ID, CommandMessage, FeedbackMessage
 

SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/handle.py

@@ -1,6 +1,6 @@
 #!/usr/bin/python3
 import rospy
-from mil_usb_to_can import CANDeviceHandle
+from mil_usb_to_can.sub8 import CANDeviceHandle
 from ros_alarms import AlarmBroadcaster, AlarmListener
 from ros_alarms_msgs.msg import Alarm
 from rospy.timer import TimerEvent
@@ -26,7 +26,7 @@ class ThrusterAndKillBoard(CANDeviceHandle):
     def __init__(self, *args, **kwargs):
         super().__init__(*args, **kwargs)
         # Initialize thruster mapping from params
-        self.thrusters = make_thruster_dictionary(
+        self.thrusters, self.name_to_id = make_thruster_dictionary(
             rospy.get_param("/thruster_layout/thrusters"),
         )
         # Tracks last hw-kill alarm update
@@ -93,7 +93,7 @@ def on_hw_kill(self, alarm: Alarm) -> None:
         Update the classes' hw-kill alarm to the latest update.
 
         Args:
-            alarm (:class:`~ros_alarms.msg._Alarm.Alarm`): The alarm message to update with.
+            alarm (:class:`~ros_alarms_msgs.msg._Alarm.Alarm`): The alarm message to update with.
         """
         self._last_hw_kill = alarm
 
@@ -118,7 +118,7 @@ def on_command(self, msg: Thrust) -> None:
             effort = self.thrusters[cmd.name].effort_from_thrust(cmd.thrust)
             # Send packet to command specified thruster the specified force
             packet = ThrustPacket.create_thrust_packet(
-                ThrustPacket.ID_MAPPING[cmd.name],
+                self.name_to_id[cmd.name],
                 effort,
             )
             self.send_data(bytes(packet), can_id=THRUST_SEND_ID)

SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/packets.py

@@ -1,7 +1,7 @@
 import struct
 from collections import namedtuple
 
-from mil_usb_to_can import ApplicationPacket
+from mil_usb_to_can.sub8 import ApplicationPacket
 
 # CAN channel to send thrust messages to
 THRUST_SEND_ID = 0x21
@@ -250,41 +250,9 @@ class ThrustPacket(ApplicationPacket):
     Attributes:
         IDENTIFIER (int): The packet identifier, equal to the ordinal value of "T,"
             or 84.
-        ID_MAPPING (Dict[str, int]): A dictionary mapping 3-letter thruster codes
-            to their respective IDs:
-
-            +--------+------+
-            |  Name  |  ID  |
-            +========+======+
-            |  FLH   |  0   |
-            +--------+------+
-            |  FRH   |  1   |
-            +--------+------+
-            |  FLV   |  2   |
-            +--------+------+
-            |  FRV   |  3   |
-            +--------+------+
-            |  BLH   |  4   |
-            +--------+------+
-            |  BRH   |  5   |
-            +--------+------+
-            |  BLV   |  6   |
-            +--------+------+
-            |  BRV   |  7   |
-            +--------+------+
     """
 
     IDENTIFIER = ord("T")
-    ID_MAPPING = {
-        "FLH": 0,
-        "FRH": 1,
-        "FLV": 2,
-        "FRV": 3,
-        "BLH": 4,
-        "BRH": 5,
-        "BLV": 6,
-        "BRV": 7,
-    }
 
     @classmethod
     def create_thrust_packet(cls, thruster_id: int, command: float):

SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/simulation.py

@@ -1,6 +1,6 @@
 #!/usr/bin/python3
 import rospy
-from mil_usb_to_can import SimulatedCANDevice
+from mil_usb_to_can.sub8 import SimulatedCANDevice
 from std_srvs.srv import SetBool, SetBoolRequest, SetBoolResponse
 
 from .packets import (

SubjuGator/drivers/sub8_thrust_and_kill_board/sub8_thrust_and_kill_board/thruster.py

@@ -1,16 +1,21 @@
-from typing import Any, Dict
+from __future__ import annotations
+
+from typing import Any
 
 from numpy import clip, polyval
 
 
-def make_thruster_dictionary(dictionary):
+def make_thruster_dictionary(dictionary) -> tuple[dict[str, Thruster], dict[str, int]]:
     """
-    Make a dictionary mapping thruster names to :class:`Thruster` objects.
+    Make a dictionary mapping thruster names to :class:`Thruster` objects
+    and a dictionary mapping thruster names to node IDs.
     """
     ret = {}
+    name_id_map = {}
     for thruster, content in dictionary.items():
         ret[thruster] = Thruster.from_dict(content)
-    return ret
+        name_id_map[thruster] = content["node_id"]
+    return ret, name_id_map
 
 
 class Thruster:
@@ -27,7 +32,7 @@ def __init__(self, forward_calibration, backward_calibration):
         self.backward_calibration = backward_calibration
 
     @classmethod
-    def from_dict(cls, data: Dict[str, Dict[str, Any]]):
+    def from_dict(cls, data: dict[str, dict[str, Any]]):
         """
         Constructs the class from a dictionary. The dictionary should be formatted
         as so:

SubjuGator/drivers/sub9_thrust_and_kill_board/CMakeLists.txt

@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(sub9_thrust_and_kill_board)
+find_package(catkin REQUIRED COMPONENTS
+  mil_usb_to_can
+)
+add_rostest(test/simulated_board.test)
+catkin_python_setup()
+catkin_package()

SubjuGator/drivers/sub9_thrust_and_kill_board/package.xml

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>sub9_thrust_and_kill_board</name>
+  <version>0.0.0</version>
+  <description>The sub9_thrust_and_kill_board package</description>
+  <maintainer email="[email protected]">Cameron Brown</maintainer>
+  <license>MIT</license>
+  <author>Cameron Brown</author>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>mil_usb_to_can</build_depend>
+  <build_export_depend>mil_usb_to_can</build_export_depend>
+  <exec_depend>mil_usb_to_can</exec_depend>
+</package>

SubjuGator/drivers/sub9_thrust_and_kill_board/setup.py

@@ -0,0 +1,11 @@
+# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from catkin_pkg.python_setup import generate_distutils_setup
+from setuptools import setup
+
+# Fetch values from package.xml
+setup_args = generate_distutils_setup(
+    packages=["sub9_thrust_and_kill_board"],
+)
+
+setup(**setup_args)

SubjuGator/drivers/sub9_thrust_and_kill_board/sub9_thrust_and_kill_board/__init__.py

@@ -0,0 +1,11 @@
+from .handle import ThrusterAndKillBoard
+from .packets import (
+    HeartbeatReceivePacket,
+    HeartbeatSetPacket,
+    KillReceivePacket,
+    KillSetPacket,
+    KillStatus,
+    ThrustSetPacket,
+)
+from .simulation import ThrusterAndKillBoardSimulation
+from .thruster import Thruster