Working code for testing apr 14 (mostly) + running ml works now!

.devcontainer/devcontainer.json

@@ -8,7 +8,9 @@
     "vscode": {
       "extensions": [
         "ms-python.python",
-        "ms-python.black-formatter"
+        "ms-python.black-formatter",
+        "ms-vscode.cpptools",
+        "ms-vscode.cmake-tools"
       ]
     }
   }

.vimrc

@@ -2,5 +2,5 @@ syntax enable
 set tabstop=4
 set shiftwidth=4
 set expandtab
-set visualbell
+set novisualbell
 set number

.vscode/c_cpp_properties.json

@@ -5,12 +5,15 @@
             "includePath": [
                 "${workspaceFolder}/**",
                 "/opt/ros/humble/include/**",
-                "${workspaceFolder}/../install/**/include/**"
+                "${workspaceFolder}/../install/**/include/**",
+                "/usr/include/eigen3",
+                "${workspaceFolder}/mrobosub_localization_cpp/include/mrobosub_localization_cpp",
+                "/opt/ros/humble/include"
             ],
             "defines": [],
             "compilerPath": "/usr/bin/gcc",
             "cStandard": "gnu17",
-            "cppStandard": "gnu++14",
+            "cppStandard": "c++17",
             "intelliSenseMode": "linux-gcc-x64"
         }
     ],

Dockerfile

@@ -14,10 +14,12 @@ RUN apt-get update && \
                         less \
                         ros-humble-ros2-control \
                         ros-humble-ros2-controllers \
+                        ros-humble-gtsam \
                         python3-scipy \
                         python3-transforms3d \
                         python3-serial \
-                        libusb-dev
+                        libusb-dev \
+                        pipx
 
 RUN echo "ALL ALL = (ALL) NOPASSWD: ALL" >> /etc/sudoers && \
     useradd -m -s /bin/bash -G sudo ubuntu && \
@@ -31,7 +33,9 @@ RUN pipx install mypy && \
     pipx ensurepath
 
 RUN sudo pip3 install --upgrade mypy typing-extensions
-
+
+# RUN sudo ln -s '/usr/install/libusb-1.0/libusb.h /usr/install/libusb.h'
+
 # Create workspace structure
 RUN mkdir -p /home/ubuntu/ros2_ws/src && \
     cd /home/ubuntu/ros2_ws && \

mrobosub_hal/mrobosub_hal/arduino.py

@@ -1,14 +1,15 @@
 import rclpy
 from serial import Serial
 from std_msgs.msg import Float32, Bool
+from sensor_msgs.msg import FluidPressure
 import time
 import struct
 
-from mrobosub_lib import Node
+from mrobosub_lib.mrobosub_lib import Node
 
 FREQUENCY = 60 # times per second
 BAUD_RATE = 9600
-CONNECTION_NAME = "/dev/ttyACM0"
+CONNECTION_NAME = "/dev/ttyACM1"
 
 class Arduino(Node):
     def __init__(self):
@@ -21,7 +22,7 @@ def __init__(self):
             Bool, "/buttons/strange", qos_profile=1
         )
         self.depth_pub = self.create_publisher(
-            Float32, "/depth", qos_profile=1
+            FluidPressure, "/depth", qos_profile=1
         )
 
         #self.write_timer = self.create_timer(1/FREQUENCY, self.writer)
@@ -64,7 +65,7 @@ def serialConnection(self):
 
                 self.charm_pub.publish(Bool(data=(self.charm == b'\x01')))
                 self.strange_pub.publish(Bool(data=(self.strange == b'\x01')))
-                self.depth_pub.publish(Float32(data=self.depth))
+                self.depth_pub.publish(FluidPressure(fluid_pressure=self.depth))
 
 
 def main():

mrobosub_hal/mrobosub_hal/dvl_publisher.py

@@ -1,16 +1,16 @@
 import socket
 import rclpy
-from mrobosub_msgs.msg import Dvl
 import numpy as np
-from mrobosub_lib import Node
-
+from mrobosub_lib.mrobosub_lib import Node
+from geometry_msgs.msg import TwistWithCovarianceStamped
+from mrobosub_msgs.msg import Dvl
 
-# todo: parameterize this in the launch file
-UDP_IP = "0.0.0.0"
-# UDP_IP = "192.168.2.9"
+UDP_IP =  b"192.168.2.9"
 HOST_IP = b"192.168.2.3"
 UDP_PORT = 27000
+POS_UP = True
 
+ALPHA = 20 # deg from vertical
 
 class DVLPublisher (Node):
     """
@@ -19,7 +19,10 @@ class DVLPublisher (Node):
 
     def __init__(self):
         super().__init__('dvl_publisher')
-        self.pub = self.create_publisher(Dvl, "/dvl/raw_dvl", qos_profile=1)
+
+        self.twist_pub = self.create_publisher(TwistWithCovarianceStamped, "/dvl/twist", qos_profile=1)
+        self.local_pose_pub = self.create_publisher(Dvl, "/dvl/local", qos_profile=1)
+        self.T_beams_xyz = self.construct_transformation_matrix()
 
         # connect to socket containing the DVL information
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@@ -42,16 +45,104 @@ def loop(self):
             data_str = data.decode()
 
             if not data_str.startswith("$DVKFC"):
-                return
-
-            # parse according to spec here https://docs.ceruleansonar.com/c/dvl-50/communicating-with-the-tracker-650/outgoing-message-formats-tracker-650-to-host/usddvkfc-kalman-filter-raw-data-support-message
-            data_list = data_str.split(",")
-            map_list = [*map(float, data_list[10 : 24 + 1 : 7])]
-            self.pub.publish(Dvl(velocity=map_list))
+                self.publish_twist(data)         
+            elif not data_str.startswith("$DVKFC"):
+                self.publish_local_pose(data)         
+
+            self.publish_twist(data)         
 
         except socket.timeout:
             self.get_logger().info("DVL UDP connection timing out, no data recieved from DVL")
 
+    def publish_twist(self, data):
+            # parse according to spec here https://docs.ceruleansonar.com/c/dvl-50/communicating-with-the-tracker-650/outgoing-message-formats-tracker-650-to-host/usddvkfc-kalman-filter-raw-data-support-message
+            data_list = data.split(b",")
+            va, vb, vc = map(float, data_list[10 : 25 : 7])
+
+            # The DVL provides confidence values in each of the velocity axes
+            # Take the biggest (worst) confidence value as variance.
+            # Technically, values greater than 0.5 should be chucked out.
+            va_c, vb_c, vc_c = map(float, data_list[11 : 26 : 7])
+            max_var = max(va_c, vb_c, vc_c)
+            if max_var > 0.5:
+                return
+
+            vx, vy, vz = self.calculate_robot_velocity_from_beams(va, vb, vc)
+
+            # Create the message
+            msg = TwistWithCovarianceStamped()
+            msg.header.stamp = self.get_clock().now().to_msg()
+            msg.header.frame_id = "dvl_link"
+
+            # Write in the velocities
+            msg.twist.twist.linear.x = vx
+            msg.twist.twist.linear.y = vy
+            msg.twist.twist.linear.z = vz
+
+            if POS_UP:
+                msg.twist.twist.linear.z *= 1 # Flip from down-positive to up-positive
+
+            # The covariance matrix is a flattened 6x6 matrix. We only need to place the 0th, 7th, and 14th values.
+            cov = [0.0] * 36
+            cov[0]  = max_var
+            cov[7]  = max_var
+            cov[14] = max_var
+            msg.twist.covariance = cov
+
+            # Publish the msg
+            self.twist_pub.publish(msg)
+
+    def publish_local_pose(self, data):
+            # parse according to spec here https://docs.ceruleansonar.com/c/tracker-650/communicating-with-the-tracker-650/outgoing-message-formats-tracker-650-to-host/usddvpdl-and-usddvpdx-dvl-position-and-angle-deltas-messages
+            data_list = data.split(b",")
+            # tu=1, dtu=2, adr=3, adp=4, ady=5, pdx=6, pdy=7, pdz=8, c=9
+            try:
+                confidence = float(data_list[9])
+                if confidence <= 0: return # don't trust data with negative confidence
+
+                local_msg = Dvl()
+                local_msg.header.stamp = self.get_clock().now().to_msg()
+
+                local_msg.ad[0] = float(data_list[3]) # roll
+                local_msg.ad[1] = float(data_list[4]) # pitch
+                local_msg.ad[2] = float(data_list[5]) # yaw
+
+                local_msg.pd[0] = float(data_list[6]) # x
+                local_msg.pd[1] = float(data_list[7]) # y
+                local_msg.pd[2] = float(data_list[8]) # z
+
+                if POS_UP:
+                    local_msg.pd[2] *= -1 # Flip from down-positive to up-positive
+
+                local_msg.confidence = confidence
+
+                self.local_pose_pub.publish(local_msg) 
+            except (IndexError, ValueError):
+                 pass
+
+    def calculate_robot_velocity_from_beams(self, va, vb, vc):
+        '''
+        Returns the velocity of the robot in the Forward-Left-Down frame from the respective beam velocities 
+        '''
+        beams = np.array([va, vb, vc])
+        v_robot = self.T_beams_xyz @ beams
+        return v_robot
+
+    def construct_transformation_matrix(self):
+        # Since the Tracker 650 is a 3-beam DVL, this means that each beam is aligned at a 120deg offset
+        # from each other. Additionally, there is a 70def beam angle from the horizontal
+        # We need to apply a transformation matrix to the beam measurements to recover the robot frame velocity
+        alpha = np.radians(ALPHA)
+
+        s_a = np.sin(alpha)
+        c_a = np.cos(alpha)
+
+        self.T_beam_xyz = np.array([
+            [(2/3)/s_a,         (-1/3)/s_a,          (-1/3)/s_a], # Vx (Forward)
+            [        0, (1/np.sqrt(3))/s_a, (-1/np.sqrt(3))/s_a], # Vy (Left)
+            [(1/3)/c_a,          (1/3)/c_a,           (1/3)/c_a], # Vz (Down-positive)
+        ])
+
 def main():
     rclpy.init()
     node = DVLPublisher()

mrobosub_hal/mrobosub_hal/esp32_thruster.py

@@ -10,7 +10,7 @@
 from typing import Optional
 
 
-NUM_PINS = 12
+NUM_PINS = 8
 FREQUENCY = 50
 
 
@@ -22,7 +22,7 @@ class ESP32_Thruster(Node):
     def __init__(self):
         super().__init__("esp32_thruster")
         self.get_logger().info("Launched esp32_thruster node")
-        self.port = "/dev/thruster_esp" #IMPORTANT: DETERMINE ACTUAL PORT
+        self.port = "/dev/ttyACM2" #IMPORTANT: DETERMINE ACTUAL PORT
         self.thruster_outputs = [0] * NUM_PINS
         self.serial: Serial | None = None
         self.connect()
@@ -38,20 +38,22 @@ def __init__(self):
 
     def connect(self) -> bool:
         try:
-            self.serial = Serial(self.port, timeout=0.5, write_timeout=0.5)
-            enable_all()
+            self.serial = Serial(self.port, timeout=0, write_timeout=0.5)
+            self.enable_all()
         except SerialException as e:
             self.get_logger().info(f"Could not connect to esp32_thruster: {e}")
             return False
         return True
 
     def write(self, data: str) -> bool:
+        data = data + "\n"
         if self.serial is None:
             success = self.connect()
             if not success or self.serial is None:
+                self.get_logger().error("Serial is not connected. AHHHH!")
                 return False
         try:
-            self.serial.write(data)
+            self.serial.write(data.encode())
             return True
         except SerialException as e:
             self.get_logger().info(f"write error: {e}")
@@ -90,7 +92,8 @@ def send_power(self, pin: int, percent_raw: float) -> int:
 
 
     def send_enable_disable(self, pin: int, enable: bool) -> int:
-        if(enable == true):
+        self.get_logger().info(f"Send enable {enable} to pin {pin}")
+        if(enable):
             msg = "ENABLE:TRUE"
         else:
             msg = "ENABLE:FALSE"
@@ -106,7 +109,7 @@ def send_enable_disable(self, pin: int, enable: bool) -> int:
 
     def enable_all(self):
         for i in range(NUM_PINS):
-            send_enable_disable(i, true)
+            self.send_enable_disable(i, True)
 
 
     def send_estop(self):
@@ -118,19 +121,27 @@ def handle_emergency_stop(
         self, req: SetBool.Request, res: SetBool.Response
     ) -> SetBool.Response:
         if req.data:
-            send_estop
+            self.send_estop()
         res.success = True
         return res
 
     def eps32_thruster_commands_callback(self, msg: ThrusterCommands):
         for i in range(NUM_PINS):
             if msg.valid[i]:
                 self.thruster_outputs[i] = msg.pins[i]
+            if msg.pins[i] != 0:
+                self.get_logger().info(f"The motor at index {i} is running at value {msg.pins[i]}")
 
     def loop(self):
         for i in range(NUM_PINS):
             self.send_power(i, self.thruster_outputs[i])
 
+        if self.serial is not None:
+            data = self.serial.read_all()
+            if data is not None:
+                pass
+                # self.get_logger().info(f"data read from esp32: {str(data)}")
+
 
 def main():
     rclpy.init()

mrobosub_hal/mrobosub_hal/thruster_controller.py

@@ -40,9 +40,12 @@ def __init__(self):
         params = [Param('thruster_reverse', rclpy.Parameter.Type.BOOL_ARRAY, "List of booleans indicating whether each thruster is reversed"), 
                   Param('thruster_motor_id', rclpy.Parameter.Type.INTEGER_ARRAY, "List of motor IDs for each thruster on the thruster controller")
                   ]
-        
+
         self.declare_params(params)
 
+        for i in range(NUM_MOTORS):
+            self.get_logger().info(f"Motor index {i} is {self.thruster_motor_id[i]}")
+
 
     def handle_emergency_stop(
         self, req: SetBool.Request, res: SetBool.Response

mrobosub_hal/params/thruster_controller_params.yaml

@@ -1,4 +1,4 @@
 thruster_controller:
   ros__parameters:
     thruster_reverse: [true, true, true, true, false, false, true, true]
-    thruster_motor_id: [2, 10, 8, 1, 4, 0, 3, 5]
+    thruster_motor_id: [3, 4, 1, 7, 2, 6, 0, 5]

mrobosub_hal/setup.py

@@ -28,7 +28,7 @@
             "dvl_publisher = mrobosub_hal.dvl_publisher:main",
             "botcam = mrobosub_hal.botcam:main",
             "zed = mrobosub_hal.zed:main",
-            "esp32_thruster = mrobosub_hal.esp32_controller:main",
+            "esp32_thruster = mrobosub_hal.esp32_thruster:main",
             "arduino = mrobosub_hal.arduino:main"
         ],
     },