diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index b89a879c..4c29f9fd 100644 --- a/.devcontainer/devcontainer.json +++ b/.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" ] } } diff --git a/.vimrc b/.vimrc index cd6e24eb..17a8cc1b 100644 --- a/.vimrc +++ b/.vimrc @@ -2,5 +2,5 @@ syntax enable set tabstop=4 set shiftwidth=4 set expandtab -set visualbell +set novisualbell set number diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 27a611bb..c6811d87 100644 --- a/.vscode/c_cpp_properties.json +++ b/.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" } ], diff --git a/Dockerfile b/Dockerfile index 1c4f73f5..a017139f 100644 --- a/Dockerfile +++ b/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 && \ diff --git a/mrobosub_hal/mrobosub_hal/arduino.py b/mrobosub_hal/mrobosub_hal/arduino.py index ad528cf2..e45ba5ed 100755 --- a/mrobosub_hal/mrobosub_hal/arduino.py +++ b/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(): diff --git a/mrobosub_hal/mrobosub_hal/dvl_publisher.py b/mrobosub_hal/mrobosub_hal/dvl_publisher.py index 38456027..b68a576e 100755 --- a/mrobosub_hal/mrobosub_hal/dvl_publisher.py +++ b/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() diff --git a/mrobosub_hal/mrobosub_hal/esp32_thruster.py b/mrobosub_hal/mrobosub_hal/esp32_thruster.py index ccddab70..d76aae01 100644 --- a/mrobosub_hal/mrobosub_hal/esp32_thruster.py +++ b/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,7 +121,7 @@ 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 @@ -126,11 +129,19 @@ 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() diff --git a/mrobosub_hal/mrobosub_hal/thruster_controller.py b/mrobosub_hal/mrobosub_hal/thruster_controller.py index a97804c9..3812b5a1 100755 --- a/mrobosub_hal/mrobosub_hal/thruster_controller.py +++ b/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 diff --git a/mrobosub_hal/params/thruster_controller_params.yaml b/mrobosub_hal/params/thruster_controller_params.yaml index ec41bf60..6dc6f92d 100644 --- a/mrobosub_hal/params/thruster_controller_params.yaml +++ b/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] diff --git a/mrobosub_hal/setup.py b/mrobosub_hal/setup.py index 78b072d8..ebbdc03d 100644 --- a/mrobosub_hal/setup.py +++ b/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" ], }, diff --git a/mrobosub_hal_imu/CMakeLists.txt b/mrobosub_hal_imu/CMakeLists.txt index 78f46bda..c5b8b55f 100644 --- a/mrobosub_hal_imu/CMakeLists.txt +++ b/mrobosub_hal_imu/CMakeLists.txt @@ -1,48 +1,40 @@ cmake_minimum_required(VERSION 3.8) project(mrobosub_hal_imu) -add_subdirectory(inertial-sense-sdk) - -include_directories( - inertial-sense-sdk/src - inertial-sense-sdk/src/libusb/libusb - inertial-sense-sdk/src/libusb/linux - inertial-sense-sdk/src/yaml-cpp -) +find_package(PkgConfig REQUIRED) +pkg_check_modules(LIBUSB REQUIRED libusb-1.0) +find_package(Threads REQUIRED) -# find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. find_package(rclcpp REQUIRED) find_package(mrobosub_msgs REQUIRED) +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR}/inertial-sense-sdk/src + ${CMAKE_CURRENT_SOURCE_DIR}/inertial-sense-sdk/src/yaml-cpp + ${LIBUSB_INCLUDE_DIRS} +) + +add_subdirectory(inertial-sense-sdk) add_executable(imu_node src/imu_node.cpp) -ament_target_dependencies(imu_node rclcpp mrobosub_msgs) -target_link_libraries(imu_node InertialSenseSDK) -install(TARGETS - imu_node - DESTINATION lib/${PROJECT_NAME} -) +ament_target_dependencies(imu_node rclcpp mrobosub_msgs) -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME}/ +target_link_libraries(imu_node + InertialSenseSDK + ${LIBUSB_LIBRARIES} + Threads::Threads ) +install(TARGETS imu_node DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() -ament_package() +ament_package() \ No newline at end of file diff --git a/mrobosub_hal_imu/launch/imu_launch.xml b/mrobosub_hal_imu/launch/imu_launch.xml index a206092e..5604533f 100644 --- a/mrobosub_hal_imu/launch/imu_launch.xml +++ b/mrobosub_hal_imu/launch/imu_launch.xml @@ -1,5 +1,5 @@ - + diff --git a/mrobosub_hal_imu/package.xml b/mrobosub_hal_imu/package.xml index 8266cde2..7603f5dc 100644 --- a/mrobosub_hal_imu/package.xml +++ b/mrobosub_hal_imu/package.xml @@ -14,6 +14,7 @@ rclcpp mrobosub_msgs + sensor_msgs ament_cmake diff --git a/mrobosub_hal_imu/src/imu_node.cpp b/mrobosub_hal_imu/src/imu_node.cpp index 512ee6b5..514f0e29 100644 --- a/mrobosub_hal_imu/src/imu_node.cpp +++ b/mrobosub_hal_imu/src/imu_node.cpp @@ -6,8 +6,7 @@ #include "../inertial-sense-sdk/src/data_sets.h" #include -#include -#include +#include #include #include @@ -39,6 +38,20 @@ class TimeManager std::shared_ptr nh_; }; +struct PimuStorage { + pimu_t pimu; + bool has_pimu = false; + double tow_offset = -1; +}; + +PimuStorage pimu_storage; + +#define TIME_EPSILON 0.01 + +/** + * Publishes: + * /imu Imu + */ int main(int argc, char **argv) { rclcpp::init(argc, argv); @@ -55,64 +68,94 @@ int main(int argc, char **argv) return 1; } - auto pub_ins = node->create_publisher("/imu_INS", 1); - auto pub_pimu = node->create_publisher("/imu_PIMU", 1); - if (!pub_ins || !pub_pimu) - { + auto imu_pub = node->create_publisher("/imu", 1); + if (!imu_pub) { return -1; } - InertialSense is; + // We need to attach a global callback to all DID messages returned by the + // sensor so that we can sync the INS (orientation) and PIMU (linear acc and angular vel) measurements + InertialSense is([&](InertialSense *is_ptr, p_data_t *data, int port_handle) { + switch(data->hdr.id) { + case DID_GPS1_POS: { + auto gps = reinterpret_cast(data->ptr); + pimu_storage.tow_offset = gps->towOffset; + break; + } + + case DID_PIMU: { + pimu_storage.pimu = *reinterpret_cast(data->ptr); + pimu_storage.has_pimu = true; + break; + } + case DID_INS_2: { + if (!pimu_storage.has_pimu) return; // We want to sync the DID_PIMU and DID_INS_2 messages. + + auto ins = reinterpret_cast(data->ptr); + + double pimu_time_to_tow = pimu_storage.pimu.time + pimu_storage.tow_offset; + + if (std::abs(pimu_time_to_tow - ins->timeOfWeek) < TIME_EPSILON) { + auto time_stamp = timeManager.ros_time_from_start_time(ins->timeOfWeek); + const auto div = 1.0f/pimu_storage.pimu.dt; + + sensor_msgs::msg::Imu msg; + msg.header.stamp = time_stamp; + msg.header.frame_id = "imu_link"; + + msg.orientation.w = ins->qn2b[0]; + msg.orientation.x = ins->qn2b[1]; + msg.orientation.y = ins->qn2b[2]; + msg.orientation.z = ins->qn2b[3]; + + msg.angular_velocity.x = pimu_storage.pimu.theta[0] * div; + msg.angular_velocity.y = pimu_storage.pimu.theta[1] * div; + msg.angular_velocity.z = pimu_storage.pimu.theta[2] * div; + + msg.linear_acceleration.x = pimu_storage.pimu.vel[0] * div; + msg.linear_acceleration.y = pimu_storage.pimu.vel[1] * div; + msg.linear_acceleration.z = pimu_storage.pimu.vel[2] * div; + + double acc_var = 3.45e-7; // Acc noise density is 0.000588 -> var is noise^2 + double gyro_var = 7.61e-9; // Gyro noise density is 8.72e-5 -> var is noise^2 + + msg.linear_acceleration_covariance[0] = acc_var; + msg.linear_acceleration_covariance[4] = acc_var; + msg.linear_acceleration_covariance[8] = acc_var; + + msg.angular_velocity_covariance[0] = gyro_var; + msg.angular_velocity_covariance[4] = gyro_var; + msg.angular_velocity_covariance[8] = gyro_var; + + imu_pub->publish(msg); + } + pimu_storage.has_pimu = false; + break; + } + } + }); + is.Open(non_ros_args[1].c_str()); - auto pimu_registered = is.BroadcastBinaryData( - DID_PIMU, - 1, - [&](InertialSense *is, p_data_t *_data, int pHandle) - { - const auto data = reinterpret_cast(_data->ptr); - - mrobosub_msgs::msg::ImuPIMU msg; - const auto div = 1.0f/data->dt; - - msg.header.stamp = timeManager.ros_time_from_start_time(data->time); - msg.dt = data->dt; - msg.angular_velocity.x = data->theta[0] * div; - msg.angular_velocity.y = data->theta[1] * div; - msg.angular_velocity.z = data->theta[2] * div; - msg.linear_acceleration.x = data->vel[0] * div; - msg.linear_acceleration.y = data->vel[1] * div; - msg.linear_acceleration.z = data->vel[2] * div; - pub_pimu->publish(msg); - } - ); + auto pimu_registered = is.BroadcastBinaryData(DID_PIMU, 1); if (!pimu_registered) { return 1; } - auto ins_registered = is.BroadcastBinaryData( - DID_INS_1, - 1, - [&](InertialSense *is, p_data_t *_data, int pHandle) - { - const auto data = reinterpret_cast(_data->ptr); - - mrobosub_msgs::msg::ImuINS msg; - - msg.header.stamp = timeManager.ros_time_from_start_time(data->timeOfWeek); - msg.theta.x = data->theta[0]; - msg.theta.y = data->theta[1]; - msg.theta.z = data->theta[2]; - pub_ins->publish(msg); - } - ); - + auto ins_registered = is.BroadcastBinaryData(DID_INS_2, 1); if (!ins_registered) { return 1; } + + // Needed to get the offset to time of week + auto gps_registered = is.BroadcastBinaryData(DID_GPS1_POS, 1); + if (!gps_registered) + { + return 1; + } while (rclcpp::ok()) { @@ -122,6 +165,8 @@ int main(int argc, char **argv) return 1; } rclcpp::spin_some(node); - std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + // Try to get the publishing rate closer to the rate ascertained by the datasheet + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } } diff --git a/mrobosub_localization/mrobosub_localization/localization.py b/mrobosub_localization/mrobosub_localization/localization.py index 819e3a43..94614e98 100644 --- a/mrobosub_localization/mrobosub_localization/localization.py +++ b/mrobosub_localization/mrobosub_localization/localization.py @@ -1,20 +1,33 @@ +from dataclasses import dataclass import math import numpy as np import rclpy from std_msgs.msg import Float64, Float32 -from mrobosub_lib import Node +from mrobosub_lib.mrobosub_lib import Node from typing import Optional, Final from std_srvs.srv import Trigger -from geometry_msgs.msg import Quaternion -from mrobosub_msgs.msg import ImuINS +from mrobosub_msgs.msg import Imu +from geometry_msgs.msg import Quaternion as ROSQuaternion from math import degrees +@dataclass +class Quaternion: + x: float + y: float + z: float + w: float -def euler_from_quaternion(quaternion): +@dataclass +class Euler: + roll: float + pitch: float + yaw: float + +def euler_from_quaternion(quaternion: Quaternion) -> Euler: """ Converts quaternion (w in last place) to euler roll, pitch, yaw quaternion = [w, x, y, z] @@ -36,10 +49,10 @@ def euler_from_quaternion(quaternion): cosy_cosp = 1 - 2 * (y * y + z * z) yaw = np.arctan2(siny_cosp, cosy_cosp) - return roll, pitch, yaw + return Euler(roll, pitch, yaw) -def quaternion_from_euler(roll, pitch, yaw): +def quaternion_from_euler(roll: float, pitch: float, yaw: float) -> Quaternion: """ Converts euler roll, pitch, yaw to quaternion (w in last place) quat = [x, y, z, w] @@ -52,11 +65,11 @@ def quaternion_from_euler(roll, pitch, yaw): cr = math.cos(roll * 0.5) sr = math.sin(roll * 0.5) - q = [0.0] * 4 - q[0] = cy * cp * cr + sy * sp * sr - q[1] = cy * cp * sr - sy * sp * cr - q[2] = sy * cp * sr + cy * sp * cr - q[3] = sy * cp * cr - cy * sp * sr + q = Quaternion(0.0, 0.0, 0.0, 0.0) + q.x = cy * cp * cr + sy * sp * sr + q.y = cy * cp * sr - sy * sp * cr + q.z = sy * cp * sr + cy * sp * cr + q.w = sy * cp * cr - cy * sp * sr return q @@ -65,7 +78,7 @@ class StateEstimation(Node): """ Subscribers - /depth/raw_depth - - /imu_INS + - /imu """ """ @@ -93,7 +106,7 @@ def __init__(self): self.create_subscription( Float32, "/depth/raw_depth", self.raw_depth_callback, qos_profile=1 ) - self.create_subscription(ImuINS, "/imu_INS", self.imu_callback, qos_profile=1) + self.create_subscription(Imu, "/imu", self.imu_callback, qos_profile=1) self.create_service(Trigger, "localization/zero_state", self.handle_reset) def handle_reset( @@ -121,32 +134,28 @@ def raw_depth_callback(self, raw_depth: Float32): self.heave_offset = raw_depth.data self.heave_pub.publish(Float64(data=raw_depth.data - self.heave_offset)) - def imu_callback(self, msg: ImuINS): - - # orientation = msg.orientation - # - # quaternion = [ - # orientation.x, - # orientation.y, - # orientation.z, - # orientation.w - # ] - # euler = euler_from_quaternion(quaternion) - - euler = msg.theta + def imu_callback(self, msg: Imu): + orientation = msg.orientation + quaternion = Quaternion( + orientation.x, + orientation.y, + orientation.z, + orientation.w + ) + euler = euler_from_quaternion(quaternion) if ( self.yaw_offset is None or self.pitch_offset is None or self.roll_offset is None ): - self.yaw_offset = degrees(-euler.z) - self.pitch_offset = degrees(-euler.y) - self.roll_offset = degrees(euler.x) + self.yaw_offset = degrees(-euler.yaw) + self.pitch_offset = degrees(-euler.pitch) + self.roll_offset = degrees(euler.roll) - yaw = degrees(-euler.z) - self.yaw_offset - pitch = degrees(-euler.y) - self.pitch_offset - roll = degrees(euler.x) - self.roll_offset + yaw = degrees(-euler.yaw) - self.yaw_offset + pitch = degrees(-euler.pitch) - self.pitch_offset + roll = degrees(euler.roll) - self.roll_offset self.yaw_pub.publish(Float64(data=yaw)) self.pitch_pub.publish(Float64(data=pitch)) diff --git a/mrobosub_localization_cpp/CMakeLists.txt b/mrobosub_localization_cpp/CMakeLists.txt new file mode 100644 index 00000000..bcc52126 --- /dev/null +++ b/mrobosub_localization_cpp/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.8) +project(mrobosub_localization_cpp) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(GTSAM REQUIRED) +find_package(BOOST REQUIRED + COMPONENTS thread +) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + +# Add source files to executable +add_executable(localization + src/localization.cpp +) + +# Ament automates a lot of the CMake process. +# We can use ament to include+link rclcpp in one line +ament_target_dependencies(localization + rclcpp + tf2_ros + geometry_msgs + sensor_msgs + nav_msgs +) + +target_include_directories(localization PRIVATE + ${EIGEN3_INCLUDE_DIRS} +) + +# Expose the /include directory +target_include_directories(localization PUBLIC + $ + $ +) + +# Require C99 and C++17 +target_compile_features(localization PUBLIC + c_std_99 cxx_std_17 +) + +# Manually link with gtsam +target_link_libraries(localization + Eigen3::Eigen + gtsam + ${Boost_LIBRARIES} +) + + +# Copy to the `install/` folder. +install( + TARGETS localization + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# If other packages in the workspace need mrobosub_localization_cpp +# ament_export_dependencies(gtsam) +ament_package() diff --git a/mrobosub_localization_cpp/LICENSE b/mrobosub_localization_cpp/LICENSE new file mode 100644 index 00000000..8fa7a5de --- /dev/null +++ b/mrobosub_localization_cpp/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2026 University of Michigan Field Robotics + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. \ No newline at end of file diff --git a/mrobosub_localization_cpp/README.md b/mrobosub_localization_cpp/README.md new file mode 100644 index 00000000..833125eb --- /dev/null +++ b/mrobosub_localization_cpp/README.md @@ -0,0 +1,185 @@ +# mrobosub_localization_cpp + +For [Zayn Baig](https://github.com/imzaynb)'s Winter 2026 Multidisciplinary Design Project, he was tasked with migrating the [Field Robotics Group](https://fieldrobotics.engin.umich.edu/)'s seminal Autonomous Underwater Veheicle (AUV) localization paper [TURTLMap](https://umfieldrobotics.github.io/TURTLMap/) on the Sub. + +This document will serve as a development diary until I complete the entire project, in which case this will turn into more of a mix of rationale and documentation for the localization code. + +One thing I strongly believe is important to the longevity of the codebase is thorough, accurate documentation. I hope my efforts here will be a good first step towards accomplishing this. + +## TODOs +[] I should probably make a Github Project to keep me organized + +[] Figure out how to add downloading GTSAM to the Docker image + +[] Figure out how to add all of the other C++ requirements to the docker image + - could this be added to the CMakeLists.txt file? Not sure how ament works + +[] I have a feeling the state code listed in `PosegraphNode.h` could definitely be split into state classes for `KeyframeState`, `DvlState`, `ImuState`, and `BarometerState`. + +## Development Diary + +### [03/17] Cloning the repository +I realized pretty late that our codebase (as of October 2025) is fully in ROS2 while TURTLMap was written completely in ROS1. As such, this is going to be both a mix of making their code work on our submarine along with migrating their code to ROS2. + +I created this package using +```bash +ros2 pkg create --license Apache-2.0 --build-type ament-cmake mrobosub_localization_cpp +``` + +I then updated the `package.xml` to reflect relavent changes that needed to be made to the file. + +Across the codebase, we use the BSD-2.0 license, but since I am heavily using the [TURTLMap Github repository](https://github.com/umfieldrobotics/TURTLMap) which has this Apache 2.0 license, I thought it best to adhere to the one they use. + +### [03/17-03/18] Porting all of the Header files +I thought a good place to start would be porting over all of the headerfiles under `./include/mrobosub_localization_cpp` into this repository. + +I chose to have member variables for most classes have an `_` prefix instead of an `_` suffix. I the place where I chose not to adhere to this rule is for the structs in which all members are public anyways and there is really no more elaborate code than using the struct to hold related data together. + +I also found it fit to change the variable names and organize the code a little bit. I have a feeling that I will be majorly refactoring some of the code, especially the ROS specific code, later on, but I think gradual change is the best. I think my idea for the time being will be to keep it relatively similar to the source material for now until I get it running in which case I can majorly refactor it later. + +I also had the header files only define all of the functions, not actually implement them. They had a healthy mix of some getters/setters being implemented in the header file, but I prefer consistency, so all function implementations will be in the source files. + +### [03/18] Loading the GTSAM library onto this Docker container +The easiest way that I have seen to load GTSAM in for a ROS environment is installing the package via apt. + +```bash +sudo apt install ros-humble-gtsam +``` + +I was considering adding the GTSAM Github as a submodule, but this would drastically increase compilation times which is really not worth it given how frequently we build. + +This line was added to the Dockerfile. + +Additionally, you also need to reflect this dependency in the `package.xml` with the line `gtsam`. + +Also, you need to reflect these changes in the CMakeLists.txt. + +#### 1. Find the package + +We first need to have CMake find the package +``` +find_package(GTSAM required) +``` + +#### 2. Link with the library +``` +target_link_libraries(localization + gtsam +) +``` + +#### 3. (Optional) Expose GTSAM to other modules + +If you need other packages in the workspace need to use this package (`mrobosub_localization_cpp`), they will need GTSAM too. We can expose this to them using ament. +``` +ament_export_dependencies(gtsam) +``` + +After adding GTSAM, I rebuilt the container, which successfully worked. + +Additionally, I had to add a couple of VSCode extensions for C++ and CMake add better linting and intellisense. With this, I caught some small typos with my code. + +### [03/18] Fixing the variables to use the GTSAM library. + +Before, I was not using the `gtsam::` variables because I had not included `gtsam` into the project until after I finished all of the header files. I wanted to get a good bit of coding done, just as a productivity boost, before I get bogged down with downloading and debugging install GTSAM. + +After downloading the library, as I document above, I fully converted all of the commented out gtsam objects into the actual (uncommented out) versions. + +In the existing GTSAM code, there is a mix of smart pointers (e.g. `std::shared_ptr` or `std::unique_ptr`) between both the standard library `std` and the `boost` library. However, GTSAM heavily relies on the `boost` library. As such, for consistency sake, I will consistently use `boost`. + +In order to do so, I have to add `Boost` to the CMakeLists.txt in 2 ways: + +#### 1. Add the package +``` +find_package(Boost REQUIRED COMPONENTS thread) +``` + +#### 2. Link the libraries +``` +target_link_libraries(localization PRIVATE +${BOOST_LIBRARIES} +... +) +``` + +### [03/18] Migrating the Source Files +Before I get to ironing out the specific differences between ROS1 and ROS2 between the two C++ codebases, I want to migrate over all of the ROS-agnostic source code. + +So far, I have migrated `BluerovBarometerFactor`. + +### [03/24] Continuing to Migrate Source Files +I have worked through migrating `DvlOnlyFactor`, `PreintegratedVelocityHelpers`, and started `Posegraph`. + +As I began to work through `Posegraph`, one big thing I encountered was the loading of parameters. The existing solution used by the TURTLMap authors is to use YAML files loaded in by an external YAML C++ library. ROS2 has pretty robust parameter loading (not sure about ROS1 which may have motivated the authors to use the external YAML package). As such, I worked to convert the existing method of loading parameters to the ROS2 method. + +#### About Parameters +While refactoring the parameter code, I returned back to the ROS2 Parameter documentation. Here are the relevant pages. +- [Parameter Documentation](https://docs.ros.org/en/kilted/p/rclcpp/generated/classrclcpp_1_1Parameter.html) +- [Using Parameters in a C++ Class](https://docs.ros.org/en/kilted/Tutorials/Beginner-Client-Libraries/Using-Parameters-In-A-Class-CPP.html) +- [Understanding ROS2 Parameters](https://docs.ros.org/en/foxy/Tutorials/Beginner-CLI-Tools/Understanding-ROS2-Parameters/Understanding-ROS2-Parameters.html) +- [Using ROS2 Launch For Large Projects](https://docs.ros.org/en/kilted/Tutorials/Intermediate/Launch/Using-ROS2-Launch-For-Large-Projects.html#loadingparametersfromyamlfile) + +Here is the general format of a ROS2 parameter file: +```yaml +: + ros__parameters: + param_one: + parent_one: + child_one: + # ... +``` + +What I didn't understand until just recently is that in order to access _nested_ parameters, you can use `.` syntax. E.g. to access `child_one`, you would access it via `parent_one.child_one`. + +In order to access a node's parameters, you have to have a pointer to the node in question. For me, I wanted to separate the initial parameter loading logic into its own class (see `Parameter.h` and `Parameter.cpp`). Thus, in order to accomplish this, you need to pass a pointer to the node into the `Parameter` class. + +If you want to access a shared_ptr to `this` you can use `this->shared_from_this`. However, you can't pass a `shared_ptr` of a class into any other class or for any other use-case (as it does not exist!!!) until **AFTER THE CONSTRUCTOR RUNS**. I did not know this! As such, you must have a separate `initialize_parameters` method that you will run to populate the parameters after + +### [4/13] Determining Parameter Values +To determine the various parameter values, I started off with the datasheets for each of the sensors. + +#### Inertial Sense IMX-5 +The Inertial Sense IMX-5 (IMU)'s datasheet is [here](https://docs.inertialsense.com/datasheets/IMX-5_IMU_AHRS_GNSS-INS_Datasheet.pdf). From the datasheet, I was able to get the arr/gyro maximum values, noise density, and random walk values. + +According to the datasheet, apparently I am able to get readings at a rate of 1kHz which is really good (4x the rate that the TURTLMap IMU was able to get). + +Since I am having this node run in it's own package, I might be able to bump up the ROS publishing rate. Right now I am having the node sleep for 5ms before continuing --> this maxes out my maximum rate to 200kHz. + +I also changed the message type from our custom type to the `sensor_msgs::msg::Imu` type to be more in line with the traditional ROS2 sensor types. + +### Tracker 650 DVL +I primarily just need the extrinsic transformations from the CAD. Will ask Mechanical for those numbers ASAP. + +I needed to change the data published by the DVL because according to the datasheet [here](https://docs.ceruleansonar.com/c/tracker-650/communicating-with-the-tracker-650/outgoing-message-formats-tracker-650-to-host/usddvkfc-kalman-filter-raw-data-support-message) the DVL actually publishes it's confidence in each of it's axes. This can be used to provide a covariance value. + +Apparently the typical ROS method for publishing messages parsed from a DVL is the `TwistWithCovarianceStamped` (which makes sense since the DVL provides the sub an estimate of it's twist). + +Right now, we were extracting the raw beam velocities in the DVL publisher node as opposed to publishing the vx, vy, vz. I had to use a transformation matrix to turn beam velocities into robot frame velocities. + +The sensor is mounted via a NED format. Using right hand rule, point your index finger away from the data cable, your middle finger towards the right, and your thumb pointed down. + +See the following reference image. +![image](https://docs.ceruleansonar.com/c/~gitbook/image?url=https%3A%2F%2F977450193-files.gitbook.io%2F%7E%2Ffiles%2Fv0%2Fb%2Fgitbook-x-prod.appspot.com%2Fo%2Fspaces%252FldEroQctKFErSiZvuhJk%252Fuploads%252FxuL7u4W2BE68qUs2ovN7%252Fimage.png%3Falt%3Dmedia%26token%3D669440b3-49cd-4d2f-a5d5-f8bcedde80e0&width=768&dpr=3&quality=100&sign=d4de0e74&sv=2) + +Note that because of the beam orientations, the A beam is aligned with the x direction. + +## Citation + +### Acknowledgements +I give a huge thanks to [Dr. Katie Skinner](https://robotics.umich.edu/people/faculty/katie-skinner/) for guiding me with this project. She's such an awesome person! + +### TURTLMap Paper Citation + +```bibtex +@inproceedings{song2024turtlmap, + title={TURTLMap: Real-time Localization and Dense Mapping of Low-texture Underwater Environments with a Low-cost Unmanned Underwater Vehicle}, + author={Song, Jingyu and Bagoren, Onur and Andigani, Razan and Sethuraman, Advaith and Skinner, Katherine A}, + booktitle={2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + pages={1191--1198}, + year={2024}, + organization={IEEE} +} +``` + +### TURTLMap Github +You can see the source code provided by the authors for TURTLMap at the [TURTLMap Github repository](https://github.com/umfieldrobotics/TURTLMap). \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h new file mode 100644 index 00000000..fd33aa1e --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/BluerovBarometerFactor.h @@ -0,0 +1,29 @@ +#ifndef __BLUEROV_BAROMETER_FACTOR_H__ +#define __BLUEROV_BAROMETER_FACTOR_H__ + +#include // gtsam::Vector +#include // gtsam::Matrix +#include // gtsam::Pose3 +#include // gtsam::Key +#include // gtsam::NoiseModelFactor1 +#include // gtsam::SharedNoiseModel + +namespace localization { +class BluerovBarometerFactor : public gtsam::NoiseModelFactor1 { +private: + double _measured; + +public: + BluerovBarometerFactor(); + ~BluerovBarometerFactor(); + BluerovBarometerFactor(gtsam::Key key, double measured, const gtsam::SharedNoiseModel &model); + + gtsam::Vector evaluateError( + const gtsam::Pose3 &pose, + boost::optional H + ) const; +}; +} // namespace localization + + +#endif //__BLUEROV_BAROMETER_FACTOR_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h new file mode 100644 index 00000000..ca7d26da --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/DvlOnlyFactor.h @@ -0,0 +1,37 @@ +#ifndef __DVL_ONLY_FACTOR_H__ +#define __DVL_ONLY_FACTOR_H__ + +#include "PreintegratedVelocityHelpers.h" + +#include // Vector +#include // Matrix +#include // Pose3 +#include // Key +#include // imuBias::ConstantBias +#include // NoiseModelFactorN + +#include + +namespace localization { +class DvlOnlyFactor : public gtsam::NoiseModelFactorN { +public: // Methods + DvlOnlyFactor(); + DvlOnlyFactor(gtsam::Key pose_i, gtsam::Key pose_j, gtsam::Key vbias_i, + const PreintegratedVelocityMeasurementsDvlOnly &pvm); + virtual ~DvlOnlyFactor(); + + gtsam::Vector evaluateError( + const gtsam::Pose3 &pose_i, + const gtsam::Pose3 &pose_j, + const gtsam::imuBias::ConstantBias &vbias_i, + boost::optional H1, + boost::optional H2, + boost::optional H3 + ) const; + +private: // Members + PreintegratedVelocityMeasurementsDvlOnly _pvm; +}; +} // namespace localization + +#endif //__DVL_ONLY_FACTOR_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h new file mode 100644 index 00000000..b5578aa3 --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Parameters.h @@ -0,0 +1,119 @@ +#ifndef __PARAMETERS_H__ +#define __PARAMETERS_H__ + +#include // Matrix4d + +#include + +#include +#include + +namespace localization { +struct SensorUsage { + bool is_dvl_used; + bool is_imu_used; + bool is_barometer_used; + bool is_sonar_used; + bool are_cams_used; + + SensorUsage(const std::shared_ptr &node); +}; + +struct SensorTopics { + std::string imu_topic; + std::string dvl_topic; + std::string dvl_local_position_topic; + std::string barometer_topic; + std::string sonar_topic; + + SensorTopics(const std::shared_ptr &node); +}; + +struct ImuParameters { + double acc_max; + double gyro_max; + double gyro_noise_density; + double acc_noise_density; + double acc_random_walk; + double gyro_random_walk; + double acc_bias_prior; + double gyro_bias_prior; + double imu_rate; + double integration_covariance; + double g; + double dt_imu; + + ImuParameters(const std::shared_ptr &node); +}; + +struct ImuPreintegrationParameters { + double gap_time; + + ImuPreintegrationParameters(const std::shared_ptr &node); +}; + +struct DvlParameters { + double prior_bias; + double fom_threshold; + + DvlParameters(const std::shared_ptr &node); +}; + +struct BarometerParameters { + double atmospheric_pressure; + + BarometerParameters(const std::shared_ptr &node); +}; + +struct OptimizationParameters { + double lambda_upper_bound; + double lambda_lower_bound; + double lambda_initial; + int max_iterations; + double relative_error_tolerance; + double absolute_error_tolerance; + + OptimizationParameters(const std::shared_ptr &node); +}; + +struct Extrinsics { + Eigen::Matrix4d T_SD; // dvl to imu + Eigen::Matrix4d T_SSo; // sonar to imu + Eigen::Matrix4d T_BS; // imu to body + Eigen::Matrix4d T_SBa; // barometer to imu + Eigen::Matrix4d T_W_WD; // world to dvl world + + Extrinsics(const std::shared_ptr &node); +}; + +class Parameters { +public: + Parameters(const std::shared_ptr &node); + ~Parameters(); + + // About the sensors + SensorUsage _sensor_usage; + SensorTopics _sensor_topics; + + // Parameters + ImuParameters _imu_params; + ImuPreintegrationParameters _imu_preintegration_params; + DvlParameters _dvl_params; + BarometerParameters _barometer_params; + OptimizationParameters _optimization_params; + + Extrinsics _extrinsics; + + // Why is this not parameterized? + std::vector _rosbag_topics; + + // TODO: Can we remove this? + bool _using_orbslam; + int _num_iters; + bool _using_smoother; + double _keyframe_gap_time; + bool _using_pseudo_dvl; +}; +} // namespace localization + +#endif //__PARAMETERS_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h new file mode 100644 index 00000000..f7d2d4be --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/Posegraph.h @@ -0,0 +1,172 @@ +#ifndef __POSEGRAPH_H__ +#define __POSEGRAPH_H__ + +#include "PreintegratedVelocityHelpers.h" +#include "Parameters.h" +#include "PosegraphNode.h" + +#include // Matrix44 +#include // Vector +#include // Symbol +#include // Key +#include // Pose3 +#include // Rot3 +#include // NavState +#include // PreintegratedCombinedMeasurements +#include // imuBias::ConstantFactor +#include // ISAM2Params +#include // LevenbergMarquardtParams, LevenbergMarquardtOptimizer +#include // NonlinearFactorGraph +#include // Values +#include // BatchFixedLabSmoother +#include // FixedLagSmoother::KeyTimestampMap +#include // BetweenFactor + +#include +#include +#include +#include // std::unique_ptr + +namespace localization { +class PosegraphNode; // forward decl. +class Posegraph { +public: // Members + int _index; + std::mutex &_mtx; + + // Smoother + gtsam::ISAM2Params _smoother_parameters; + double _smoother_lag = 6.0; // TODO + gtsam::BatchFixedLagSmoother _smoother_ISAM2; + gtsam::FixedLagSmoother::KeyTimestampMap _smoother_timestamps; + + // GTSAM + boost::shared_ptr _graph; + boost::shared_ptr _initial; + boost::shared_ptr _result; + boost::shared_ptr _preintegrated_measurements; // used to be pim + boost::shared_ptr _preintegrated_velocity_measurements; // used to be pvm + boost::shared_ptr _preintegrated_measurement_params; // used to be pim_params + std::unique_ptr _pose_graph_params; // use to be params + + // Prior imuBias + gtsam::imuBias::ConstantBias _prior_imu_bias; + gtsam::imuBias::ConstantBias _prior_dvl_bias; + + // Transforms + gtsam::Matrix44 _T_SD; // sensor (IMU) to DVL affine transform + gtsam::Matrix44 _T_SB; // sensor (IMU) to robot center affine transform + gtsam::Matrix44 _T_W_WD; // world to DVL world transform + + // Previous + gtsam::Pose3 _prev_pose; + gtsam::Vector3 _prev_vel; + gtsam::NavState _prev_state; + double _prev_keyframe_time; // OLD TODO: used for saving the previous keyframe time + double _prev_dvl_odometry_time; + gtsam::Rot3 _prev_dvl_odometry_rot; + + // Current + gtsam::Pose3 _curr_pose; + gtsam::Vector3 _curr_vel; + gtsam::NavState _curr_state; + gtsam::imuBias::ConstantBias _curr_imu_bias; + double _curr_time; + + std::vector _curr_dvl_vels; + std::vector _curr_dvl_poses; + std::vector _curr_dvl_rotations; + std::vector _curr_dvl_foms; // TODO: What does FOM mean? + std::vector _imu_rot_list; + gtsam::Rot3 _imu_prev_rot; + + std::vector _curr_dvl_timestamps; + std::vector _curr_dvl_local_timestamps; + +private: // Members + std::mt19937 _rng; + std::normal_distribution<> _normal_distribution; + double _visual_gap_time; + + double _W_measurement_z; + gtsam::Vector3 _B_accelerometer_S; + gtsam::Vector3 _B_gyroscope_S; + gtsam::Vector3 _B_velocity_D; + gtsam::Vector3 _W_position_C; + +public: // Methods + // Ctors + Dtors + Posegraph(std::mutex &mtx); + ~Posegraph(); + + + // Barometer factors + void add_barometric_factor(double W_measurement_z, double measurement_noise, int baro_id); + + // IMU factors + void add_imu_factor(); + + // DVL Factors + void add_velocity_factor(); + void add_velocity_factor_with_rotation_interpolation(bool using_slerp); + void add_dvl_factor(bool using_slerp); + void add_dvl_factor_imu_rotation(); + void add_dvl_odometry_factor(double noise); + + // DVL Suppliers + void add_dvl_velocity(double time_stamp, gtsam::Vector3 velocity); + void add_dvl_pose(double time_stamp, gtsam::Pose3 pose); + void add_dvl_rotation(double time_stamp, gtsam::Rot3 rotation); + + // Other factors + void add_visual_constraint_factor(gtsam::Pose3 between_pose, double weight, int prev_idx, int curr_idx); + void add_prior_factor(gtsam::Pose3 initial_pose, gtsam::Vector initial_vel, double pose_noise); + + // Creating transforms + void define_transforms(); + + // Adding estimates + void add_simple_estimate(double dt, double noise); + void add_initial_estimate(gtsam::Pose3 initial_pose, gtsam::Vector3 intitial_vel); + void add_visual_estimate(const std::vector vertex); + + // Working with the pose graph + void initialize_pose_graph(); + void initialize_pose_graph_from_imu(gtsam::Rot3 initial_rotation); + void optimize_pose_graph(); + void optimize_pose_graph_smoother(); + + // Properties + gtsam::Rot3 find_current_pose_for_dvl_vel(double time_stamp) const; + + // Getters + double get_depth_measurement() const; + gtsam::Vector3 get_accelerometer_measurement() const; + gtsam::Vector3 get_gyroscope_measurement() const; + gtsam::Vector3 get_velocity_measurement() const; + gtsam::Vector3 get_position_measurement() const; + double get_visual_gap_time() const; + + // Setters + void set_depth_measurement(double W_measurement_z); + void set_accelerometer_measurement(gtsam::Vector3 B_accelerometer_S); + void set_gyroscope_measurement(gtsam::Vector3 B_gyroscope_S); + void set_velocity_measurement(gtsam::Vector3 B_velocity_D); + void set_position_measurement(gtsam::Vector3 W_position_C); + void set_visual_gap_time(double visual_gap_time); + +private: // Methods + void initialize_parameters(std::shared_ptr node); + void set_imu_parameters(); + void set_smoother_parameters(); + + // Helper methods used for creating the dvl factors + void calculate_interpolated_rotations(bool is_using_slerp, std::vector &interpolated_rotations); + void calculate_integrated_pose_rotation(gtsam::Rot3 &integrated_pose_rotation); + void reset_dvl_odometry(); + void reset_imu_rotation(); + void clear_dvl_timestamp_pose_vel(); +}; +} // namespace localization + +#endif //__POSEGRAPH_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h new file mode 100644 index 00000000..c0575dc0 --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PosegraphNode.h @@ -0,0 +1,129 @@ +#ifndef __POSEGRAPH_NODE_H__ +#define __POSEGRAPH_NODE_H__ + +#include "Posegraph.h" +#include "Parameters.h" +#include "PreintegratedVelocityHelpers.h" +#include "DvlOnlyFactor.h" +#include "BluerovBarometerFactor.h" + +#include // Vector +#include // Pose3 +#include // Rot3 +#include // PreintegratedCombinedMeasurements +#include // NavState + +#include // boost::mutex +#include // boost::condition_variable +#include // boost::shared_ptr + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include + +// Subscribers +#include +#include +#include +#include + +// TF +#include + +namespace localization { +class PosegraphNode : public rclcpp::Node { +public: + PosegraphNode(); + +private: // Members + std::unique_ptr _posegraph; + + // Define subscribers + rclcpp::Subscription::SharedPtr _imu_sub; + rclcpp::Subscription::SharedPtr _dvl_sub; + rclcpp::Subscription::SharedPtr _baro_sub; + rclcpp::Subscription::SharedPtr _dvl_local_sub; + + // Define publishers + rclcpp::Publisher::SharedPtr _pose_pub; + rclcpp::Publisher::SharedPtr _dvl_local_pose_pub; + rclcpp::Publisher::SharedPtr _path_pub; + + // Define callback groups + rclcpp::CallbackGroup::SharedPtr _imu_callback_group; + rclcpp::CallbackGroup::SharedPtr _low_freq_sensor_callback_group; + rclcpp::CallbackGroup::SharedPtr _posegraph_callback_group; + + // Define timers + rclcpp::TimerBase::SharedPtr _posegraph_timer; + + uint64_t _frame_count; + + // Barometer state variables + double _first_depth; + + // DVL state + std::unique_ptr _preintegrated_measurements_dvl; + // First + gtsam::Vector3 _first_dvl_vel; + // Previous + double _prev_dvl_time; + double _prev_dvl_local_time; + gtsam::Pose3 _prev_dvl_local_pose; + gtsam::Rot3 _prev_dvl_rot; + // Latest + gtsam::Vector3 _latest_dvl_vel; + gtsam::Pose3 _latest_dvl_pose; + + // Keyframe states + double _first_keyframe_time; + double _prev_keyframe_time; + double _current_keyframe_time; + bool _is_new_keyframe; // Used to be new_kf_flag + double _keyfram_gap_time; + gtsam::Pose3 _latest_keyframe_pose; + gtsam::Pose3 _latest_publish_pose; // what does this mean + + // ??? State + gtsam::Pose3 _T_w_wd; // what does this mean + + // IMU State + gtsam::Rot3 _imu_latest_rot; + gtsam::NavState _latest_imu_prop_state; + uint32_t _imu_init_count; + uint32_t _imu_count; + std::vector _imu_init_acc; + std::vector _imu_init_rot; + + // Transforms + std::unique_ptr _tf_broadcaster; + + std::mutex _mtx; + std::condition_variable _cond_var; + + // Get from config file. + bool _is_using_dvl_v2_factor; + bool _is_rot_initialized; + + /* TODO: What is save trajectory?? */ + // ros::ServiceServer _save_trajectory_service; + std::vector _keyframe_timestamps; + std::vector _trajectory_timestamps; + std::vector _trajectory_poses; + +private: // methods + /* TODO: What is save trajectory?? */ + // bool save_trajectory(turtlmap::save_trajectory::Request &req, turtlmap::save_trajectory::Response &res); + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg); + void dvl_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void baro_callback(const sensor_msgs::msg::FluidPressure::SharedPtr msg); + void dvl_local_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg); + + void main_loop(); + + void broadcast_imu_transform(); +}; +} // namespace localization + +#endif //__POSEGRAPH_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h new file mode 100644 index 00000000..82ba23d2 --- /dev/null +++ b/mrobosub_localization_cpp/include/mrobosub_localization_cpp/PreintegratedVelocityHelpers.h @@ -0,0 +1,111 @@ +#ifndef __PREINTEGRATED_VELOCITY_HELPERS_H__ +#define __PREINTEGRATED_VELOCITY_HELPERS_H__ + +#include // GTSAM_MAKE_ALIGNED_OPERATOR_NEW +#include // gtsam::Matrix3 +#include // gtsam::Vector +#include // gtsam::Rot3 +#include // gtsam::Pose3 +#include // gtsam::Point3 +#include // gtsam::imuBias::ConstantBias + +#include // Eigen::Matrix + +#include // boost::serialize::access, serialize +#include // boost::shared_ptr + +#include +#include + +namespace localization { +class PreintegratedVelocityParameters { +public: // Members + gtsam::Matrix3 _bias_velocity_covariance; // Continuous time "covariance" describing velocity measurement bias for random walk + gtsam::Matrix3 _bias_initial; // Covariance of bias used as an initial estimate + +public: // Methods + PreintegratedVelocityParameters(); + PreintegratedVelocityParameters( + const gtsam::Matrix3 &bias_velocity_covariance, + const gtsam::Matrix3 &bias_initial + ); + PreintegratedVelocityParameters( + const boost::shared_ptr &p + ); + + // TODO: Make as overloaded= and overloaded<< + void print(const std::string &s = "") const; + bool equals(const PreintegratedVelocityParameters &expected, double tolerance = 1e-9) const; + + void set_bias_velocity_covariance(const gtsam::Matrix3 &covariance); + void set_bias_initial(const gtsam::Matrix3 &covariance); + +private: // Methods + // Allows boost serialization to access private members. + friend class boost::serialization::access; + + // Needed to save data and load data + template + void PreintegratedVelocityParameters::serialize(ARCHIVE &ar, const unsigned int version) { + ar &BOOST_SERIALIZATION_NVP(_bias_velocity_covariance); + ar &BOOST_SERIALIZATION_NVP(_bias_initial); + } + +public: + // GTSAM uses advanced math with Eigen + // This is to ensure that the Matrices and other stuff + // is aligned how GTSAM wants it to be. + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; + +class PreintegratedVelocityMeasurementsDvlOnly { +public: // Members + Eigen::Matrix _preintegrated_measured_covariance; + std::vector _velocity_list; + std::vector _rotations_list; + std::vector _dt_list; + +protected: // Members + friend class DvlOnlyFactor; + +private: // Members + double delta_T_ij; + gtsam::Pose3 _accumulated_poses; + gtsam::Point3 _accumulated_positions; + gtsam::imuBias::ConstantBias _dvl_bias_for_imu; // Used for the dvl bias + gtsam::Matrix3 _delp_delbias_omega; + gtsam::Matrix3 _delp_delbias_dvl; // Default initialize as gtsam::Matrix3::Zero(); + +public: // Methods + PreintegratedVelocityMeasurementsDvlOnly(); + ~PreintegratedVelocityMeasurementsDvlOnly(); + + void reset_integration(); + void reset_integration_and_bias(const gtsam::imuBias::ConstantBias &bias); + void integrate_measurements( + const gtsam::Vector3 &linear_velocity, + const gtsam::Rot3 &interpolated_rotation, + double &dt // TODO: Is this an out param? Why is this passed by ref? + ); + + void integrate_measurements_noise( + const gtsam::Vector3 &linear_velocity, + const gtsam::Rot3 &interpolated_rotation, + double &dt, // TODO: Is this an out param? Why is this passed by ref? + const double &fom + ); + + gtsam::Pose3 get_accumulated_poses() const; + gtsam::Point3 get_accumulated_positions() const; + gtsam::Matrix get_delpij_delbias_omega() const; + gtsam::Matrix get_delpij_delbias_dvl() const; + gtsam::Matrix get_preintegrated_measured_covariance() const; + + void update_accumulated_stats(const gtsam::imuBias::ConstantBias &bias); + + gtsam::Point3 predict(const gtsam::Point3 &bias, gtsam::Matrix3 &H_bias) const; +}; + +} // namespace localization + +#endif //__PREINTEGRATED_VELOCITY_PARAMETERS_H__ \ No newline at end of file diff --git a/mrobosub_localization_cpp/package.xml b/mrobosub_localization_cpp/package.xml new file mode 100644 index 00000000..9a83c8d9 --- /dev/null +++ b/mrobosub_localization_cpp/package.xml @@ -0,0 +1,26 @@ + + + + mrobosub_localization_cpp + 1.0.0 + Factor graph localization using DVL+IMU+Barometer + Michigan Robotic Submarine + Apache-2.0 + + ament_cmake + + eigen + gtsam + geometry_msgs + sensor_msgs + nav_msgs + tf2_ros + gtsam + libeigen3-dev + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/mrobosub_localization_cpp/params/posegraph.yaml b/mrobosub_localization_cpp/params/posegraph.yaml new file mode 100644 index 00000000..d9f910d8 --- /dev/null +++ b/mrobosub_localization_cpp/params/posegraph.yaml @@ -0,0 +1,116 @@ +posegraph: + ros__parameters: + # IMX-5 TODO: Read the datasheet + imu_parameters: + g: 9.80665 + + acc_max: 156.9 # 16g = 16 * 9.80665 + acc_noise_density: 5.88e-4 # 60 ug/sqrt(hz) = 60e-6 * 9.80665 = [m/s^2/sqrt(Hz)] + acc_bias_prior: 1e-4 # from "in-run bias stability": < 1.5 deg/hr (is an upper bound) + acc_random_walk: 2.67e-3 # 0.16/60 = [m/s^2/sqrt(Hz)] + + gyro_max: 69.81 # 4000 deg/sec * pi/180 = + gyro_noise_density: 8.73e-5 # 0.005 deg/s/sqrt(Hz) * pi/180 = [rad/s/sqrt(Hz)] + gyro_bias_prior: 1e-5 # [rad/s] + gyro_random_walk: 4.65e-5 # 0.002667 * pi/180 [rad/s^2/sqrt(Hz)] + + integration_covariance: 0.0001 + imu_rate: 1000.0 + T_BS: + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + imu_preintegration_parameters: + gap_time: 1.0 + # What is the sonar? + sonar_parameters: + T_SSo: # What is this parameter? + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + dvl_parameters: + T_SD: # Transformation from Sensor to Drone/Body (measure this!!!) + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + T_W_WD: # Transformation from Water-Frame to World/Map (can be kept as I) + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + prior_bias: 0.0 + fom_threshold: 0.05 + barometer_parameters: + T_SBa: # Transformation from Sensor to Barometer. Z translation is important here. + [ + 1.0000, 0.0000, 0.0000, 0.0000, + 0.0000, 1.0000, 0.0000, 0.0000, + 0.0000, 0.0000, 1.0000, 0.0000, + 0.0000, 0.0000, 0.0000, 1.0000, + ] + atmospheric_pressure: 993.3999633789062 # fine. + optimization_parameters: + lambda_upper_bound: 1e10 + lambda_lower_bound: 1e-10 + lambda_initial: 1e-5 + max_iterations: 500 + relative_error_tolerance: 1e-4 + absolute_error_tolerance: 1e-4 + sensor_topics: + imu_topic: "/imu" + dvl_topic: "/dvl/twist" + dvl_local_position_topic: "/dvl/local" + barometer_topic: "/depth" + sonar_topic: "/unused" + # left_cam_topic: + # right_cam_topic: + sensor_usage: + is_dvl_used: true + is_imu_used: true + is_barometer_used: true + is_sonar_used: false + are_cams_used: false + using_orbslam: false + num_iters: 1000 + using_smoother: true + keyframe_gap_time: 1.0 + using_pseudo_dvl: true + +# [3/24]: As of right now, I have no plans on using the cameras. + # left_cam_params: + # T_SC: + # image_dimension: + # distortion_coefficients: + # distortion_type: + # focal_length: + # principal_point: + # right_cam_params: + # T_SC: + # image_dimension: + # distortion_coefficients: + # distortion_type: + # focal_length: + # principal_point: + # camera_parameters: + # camera_rate: 30 + # sigma_absolute_translation: 0.0 + # sigma_absolute_orientation: + # sigma_c_relative_translation: + # sigma_c_relative_orientation: + + + + + + diff --git a/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp b/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp new file mode 100644 index 00000000..2b7db057 --- /dev/null +++ b/mrobosub_localization_cpp/src/BluerovBarometerFactor.cpp @@ -0,0 +1,25 @@ +#include "BluerovBarometerFactor.h" + +namespace localization { +BluerovBarometerFactor::BluerovBarometerFactor() {} + +BluerovBarometerFactor::BluerovBarometerFactor(gtsam::Key key, double measured, const gtsam::SharedNoiseModel &model) + : NoiseModelFactor1(model, key), _measured(measured) {} + +BluerovBarometerFactor::~BluerovBarometerFactor() {} + +// Expected by the parent class to be evaluateError +gtsam::Vector BluerovBarometerFactor::evaluateError( + const gtsam::Pose3 &pose, + boost::optional H +) const { + // If the H is requested, get the Jacobian of the Z-translation + if (H) { + gtsam::Matrix16 H_z; + double z = pose.z(H_z); + *H = H_z; + } + + return (gtsam::Vector(1) << pose.z() - _measured).finished(); +} +} // namespace localization \ No newline at end of file diff --git a/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp b/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp new file mode 100644 index 00000000..9449d82c --- /dev/null +++ b/mrobosub_localization_cpp/src/DvlOnlyFactor.cpp @@ -0,0 +1,65 @@ +#include "DvlOnlyFactor.h" + +namespace localization { +DvlOnlyFactor::DvlOnlyFactor() {} + +DvlOnlyFactor::DvlOnlyFactor( + gtsam::Key pose_i, + gtsam::Key pose_j, + gtsam::Key vbias_i, + const PreintegratedVelocityMeasurementsDvlOnly &pvm) + : gtsam::NoiseModelFactorN( + gtsam::noiseModel::Gaussian::Covariance( + pvm._preintegrated_measured_covariance + ), + pose_i, + pose_j, + vbias_i + ) + , _pvm(pvm) {} + +DvlOnlyFactor::~DvlOnlyFactor() {} + +gtsam::Vector DvlOnlyFactor::evaluateError( + const gtsam::Pose3 &pose_i, + const gtsam::Pose3 &pose_j, + const gtsam::imuBias::ConstantBias &vbias_i, + boost::optional H1, + boost::optional H2, + boost::optional H3 +) const { + gtsam::Rot3 R_i = pose_i.rotation(); + gtsam::Point3 p_i = pose_i.translation(); + + gtsam::Rot3 R_j = pose_j.rotation(); + gtsam::Point3 p_j = pose_j.translation(); + + gtsam::Matrix3 H_vbias; + gtsam::Point3 vbias = vbias_i.accelerometer(); + gtsam::Point3 accumulated_translation = _pvm.predict(vbias, H_vbias); + + + // residual = R_i^T * (delta_p_world) - integrated_dvl_body + gtsam::Matrix3 RiT = R_i.transpose().matrix(); + gtsam::Vector3 residual = RiT * (p_j - p_i) - accumulated_translation; + + if (H1) { + H1->resize(3, 6); + H1->block<3, 3>(0, 0) = gtsam::Matrix3::Zero(); + H1->block<3, 3>(0, 3) = -RiT; // deriv w.r.t. p_i + } + if (H2) { + H2->resize(3, 6); + H2->block<3, 3>(0, 0) = gtsam::Matrix3::Zero(); + H2->block<3, 3>(0, 3) = RiT; // deriv w.r.t. p_j + } + if (H3) { + H3->resize(3, 6); + H3->block<3, 3>(0, 0) = -H_vbias; // acceleration bias affects prediction negatively + H3->block<3, 3>(0, 3) = gtsam::Matrix3::Zero(); + } + return residual; +} + + +} // namespace localization \ No newline at end of file diff --git a/mrobosub_localization_cpp/src/Parameters.cpp b/mrobosub_localization_cpp/src/Parameters.cpp new file mode 100644 index 00000000..24d18b2d --- /dev/null +++ b/mrobosub_localization_cpp/src/Parameters.cpp @@ -0,0 +1,103 @@ +#include "Parameters.h" + +#include + +namespace localization { + SensorUsage::SensorUsage(const std::shared_ptr &node) { + is_dvl_used = node->declare_parameter("sensor_usage.is_dvl_used", false); + is_imu_used = node->declare_parameter("sensor_usage.is_imu_used", false); + is_barometer_used = node->declare_parameter("sensor_usage.is_barometer_used", false); + is_sonar_used = node->declare_parameter("sensor_usage.is_sonar_used", false); + are_cams_used = node->declare_parameter("sensor_usage.are_cams_used", false); + } + + SensorTopics::SensorTopics(const std::shared_ptr &node) { + imu_topic = node->declare_parameter("sensor_topics.imu_topic", "/todo"); + dvl_topic = node->declare_parameter("sensor_topics.dvl_topic", "/todo"); + dvl_local_position_topic = node->declare_parameter("sensor_topics.dvl_local_position_topic", "/todo"); + barometer_topic = node->declare_parameter("sensor_topics.barometer_topic", "/todo"); + sonar_topic = node->declare_parameter("sensor_topics.sonar_topic", "/unused"); + } + + ImuParameters::ImuParameters(const std::shared_ptr &node) { + g = node->declare_parameter("imu_parameters.g", 0.0); + acc_max = node->declare_parameter("imu_parameters.acc_max", 0.0); + acc_noise_density = node->declare_parameter("imu_parameters.acc_noise_density", 0.0); + acc_bias_prior = node->declare_parameter("imu_parameters.acc_bias_prior", 0.0); + acc_random_walk = node->declare_parameter("imu_parameters.acc_random_walk", 0.0); + gyro_max = node->declare_parameter("imu_parameters.gyro_max", 0.0); + gyro_noise_density = node->declare_parameter("imu_parameters.gyro_noise_density", 0.0); + gyro_bias_prior = node->declare_parameter("imu_parameters.gyro_bias_prior", 0.0); + gyro_random_walk = node->declare_parameter("imu_parameters.gyro_random_walk", 0.0); + integration_covariance = node->declare_parameter("imu_parameters.integration_covariance", 0.0); + imu_rate = node->declare_parameter("imu_parameters.imu_rate", 0.0); + dt_imu = 1.0 / imu_rate; + } + + ImuPreintegrationParameters::ImuPreintegrationParameters(const std::shared_ptr &node) { + gap_time = node->declare_parameter("imu_preintegration_parameters.gap_time", 1.0); + } + + DvlParameters::DvlParameters(const std::shared_ptr &node) { + prior_bias = node->declare_parameter("dvl_parameters.prior_bias", 0.01); + fom_threshold = node->declare_parameter("dvl_parameters.fom_threshold", 0.01); + } + + BarometerParameters::BarometerParameters(const std::shared_ptr &node) { + atmospheric_pressure =node->declare_parameter("barometer_parameters.atmospheric_pressure", 993.3999633789062); + } + + OptimizationParameters::OptimizationParameters(const std::shared_ptr &node){ + lambda_upper_bound = node->declare_parameter("optimization_parameters.lambda_upper_bound", 1e10); + lambda_lower_bound = node->declare_parameter("optimization_parameters.lambda_lower_bound", 1e-10); + lambda_initial = node->declare_parameter("optimization_parameters.lambda_initial", 1e-5); + max_iterations = node->declare_parameter("optimization_parameters.max_iterations", 500); + relative_error_tolerance = node->declare_parameter("optimization_parameters.relative_error_tolerance", 1e-4); + absolute_error_tolerance = node->declare_parameter("optimization_parameters.absolute_error_tolerance", 1e-4); + } + + Extrinsics::Extrinsics(const std::shared_ptr &node) { + auto create_matrix = [](const std::vector &flat, Eigen::Matrix4d &mat) { + if (flat.size() == 16) { + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + mat(i, j) = flat[i * 4 + j]; + } + } + } else { + mat = Eigen::Matrix4d::Identity(); + } + }; + create_matrix(node->declare_parameter>("imu_parameters.T_BS", {}), T_BS); + create_matrix(node->declare_parameter>("sonar_parameters.T_SSo", {}), T_SSo); + create_matrix(node->declare_parameter>("dvl_parameters.T_SD", {}), T_SD); + create_matrix(node->declare_parameter>("dvl_parameters.T_W_WD", {}), T_W_WD); + create_matrix(node->declare_parameter>("barometer_parameters.T_SBa", {}), T_SBa); + } + + Parameters::Parameters(const std::shared_ptr &node) + : _sensor_usage(node) + , _sensor_topics(node) + , _imu_params(node) + , _imu_preintegration_params(node) + , _dvl_params(node) + , _barometer_params(node) + , _optimization_params(node) + , _extrinsics(node) { + _using_smoother = node->declare_parameter("using_smoother", true); + _using_pseudo_dvl = node->declare_parameter("using_pseudo_dvl", true); + _using_orbslam = node->declare_parameter("using_orbslam", false); + + _keyframe_gap_time = node->declare_parameter("keyframe_gap_time", 1.0); + + _num_iters = node->declare_parameter("num_iters", 1000); + + _rosbag_topics.push_back(_sensor_topics.imu_topic); + _rosbag_topics.push_back(_sensor_topics.dvl_topic); + _rosbag_topics.push_back(_sensor_topics.dvl_local_position_topic); + _rosbag_topics.push_back(_sensor_topics.barometer_topic); + // _rosbag_topics.push_back(_sensor_topics.sonar_topic); + } + + Parameters::~Parameters() {} +} // namespace localization \ No newline at end of file diff --git a/mrobosub_localization_cpp/src/Posegraph.cpp b/mrobosub_localization_cpp/src/Posegraph.cpp new file mode 100644 index 00000000..6ad6b947 --- /dev/null +++ b/mrobosub_localization_cpp/src/Posegraph.cpp @@ -0,0 +1,666 @@ +#include "Posegraph.h" + +#include "BluerovBarometerFactor.h" +#include "DvlOnlyFactor.h" + +#include +#include +#include + +namespace localization { +// Ctor + Dtor +Posegraph::Posegraph(std::mutex &mtx) + : _index(0) + , _mtx(mtx) + , _graph(new gtsam::NonlinearFactorGraph()) + , _initial(new gtsam::Values()) + , _result(new gtsam::Values()) + , _preintegrated_velocity_measurements(new PreintegratedVelocityMeasurementsDvlOnly()) + , _prior_imu_bias(gtsam::imuBias::ConstantBias(gtsam::Vector3(0.01, 0.01, 0.01), gtsam::Vector3(0, 0, 0))) + , _prev_dvl_odometry_time(0.0) + , _prev_dvl_odometry_rot(gtsam::Rot3()) + {} + +Posegraph::~Posegraph() {} + +void Posegraph::initialize_parameters(std::shared_ptr node) { + _pose_graph_params = std::make_unique(node); + set_smoother_parameters(); + set_imu_parameters(); + define_transforms(); +} + +void Posegraph::set_smoother_parameters() { + _smoother_parameters.relinearizeThreshold = 0.01; + _smoother_parameters.relinearizeSkip = 1; + _smoother_ISAM2 = gtsam::BatchFixedLagSmoother(_smoother_lag); +} + +void Posegraph::set_imu_parameters() { + _preintegrated_measurement_params = boost::make_shared( + gtsam::Vector3(0, 0, -_pose_graph_params->_imu_params.g) // Z-UP! + ); + _preintegrated_measurement_params->setAccelerometerCovariance( + gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.acc_noise_density, 2) + ); + _preintegrated_measurement_params->setGyroscopeCovariance( + gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.gyro_noise_density, 2) + ); + _preintegrated_measurement_params->setIntegrationCovariance( + gtsam::I_3x3 * _pose_graph_params->_imu_params.integration_covariance + ); + _preintegrated_measurement_params->setBiasAccCovariance( + gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.acc_random_walk, 2) + ); + _preintegrated_measurement_params->setBiasOmegaCovariance( + gtsam::I_3x3 * std::pow(_pose_graph_params->_imu_params.gyro_random_walk, 2) + ); + + double prior_acc_bias = _pose_graph_params->_imu_params.acc_bias_prior; + double prior_gyro_bias = _pose_graph_params->_imu_params.gyro_bias_prior; + double prior_dvl_bias = _pose_graph_params->_dvl_params.prior_bias; + + gtsam::Vector3 prior_acc_bias_vec = (gtsam::Vector(3) << prior_acc_bias, prior_acc_bias, prior_acc_bias).finished(); + gtsam::Vector3 prior_gyro_bias_vec = (gtsam::Vector(3) << prior_gyro_bias, prior_gyro_bias, prior_gyro_bias).finished(); + gtsam::Vector3 prior_dvl_bias_vec = (gtsam::Vector(3) << prior_dvl_bias, prior_dvl_bias, prior_dvl_bias).finished(); + gtsam::Vector3 prior_zeros_vec = (gtsam::Vector(3) << 0, 0, 0).finished(); + + _prior_imu_bias = gtsam::imuBias::ConstantBias(prior_acc_bias_vec, prior_gyro_bias_vec); + _prior_dvl_bias = gtsam::imuBias::ConstantBias(prior_dvl_bias_vec, prior_zeros_vec); + + _preintegrated_measurements = boost::make_shared( + _preintegrated_measurement_params, + _prior_imu_bias + ); +} + +void Posegraph::add_barometric_factor(double W_measurement_z, double measurement_noise, int baro_id) { + _graph->emplace_shared( + gtsam::Symbol('x', baro_id), + W_measurement_z, + gtsam::noiseModel::Isotropic::Sigma(1, measurement_noise) + ); +} + +void Posegraph::add_imu_factor() { + _graph->emplace_shared( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('v', _index - 1), + gtsam::Symbol('x', _index), + gtsam::Symbol('v', _index), + gtsam::Symbol('b', _index - 1), + gtsam::Symbol('b', _index), + _preintegrated_measurement_params.get() + ); + + // Must reset integration for next interval + _preintegrated_measurements->resetIntegrationAndSetBias(_curr_imu_bias); +} + +void Posegraph::add_velocity_factor_with_rotation_interpolation(bool using_slerp) { + // Get the size of the current dvl measurements + size_t dvl_size = _curr_dvl_timestamps.size(); + size_t dvl_local_size = _curr_dvl_local_timestamps.size(); + + // If any of the size is zero, assert error + if (dvl_size == 0 && dvl_local_size == 0) { + assert(false); + } + + std::vector interpolated_rotations; + calculate_interpolated_rotations(using_slerp, interpolated_rotations); + + // Calculate the integrated pose translation by accumulating dvl velocities with the right rotation for each + // timestamp we have + gtsam::Point3 integrated_pose_translation = gtsam::Point3(0.0, 0.0, 0.0); + double dt_dvl = -1; + gtsam::Matrix3 integrated_rot_matrix = gtsam::Matrix3::Identity(); + for (size_t i = 0; i < dvl_size; ++i) { + dt_dvl = (dt_dvl == -1) + ? _curr_dvl_timestamps[i] - _prev_keyframe_time + : _curr_dvl_timestamps[i] - _curr_dvl_timestamps[i-1]; + integrated_rot_matrix = integrated_rot_matrix * interpolated_rotations[i].matrix(); + integrated_pose_translation += integrated_rot_matrix * _curr_dvl_vels[i] * dt_dvl; + } + + // Calculate the integrated pose rotation by accumulating rotations + gtsam::Rot3 integrated_pose_rotation; + calculate_integrated_pose_rotation(integrated_pose_rotation); + + gtsam::Pose3 integrated_pose(integrated_pose_rotation, integrated_pose_translation); + + // Insert the integrated velocity pose into the graph + if (_index > 0) { + _initial->insert(gtsam::Symbol('x', _index), _prev_pose * integrated_pose); + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + integrated_pose, + gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector6::Constant(0.01) + ) + ); + } + + // clear variables used above + reset_dvl_odometry(); + clear_dvl_timestamp_pose_vel(); +} + +void Posegraph::add_dvl_factor(bool using_slerp) { + // Get the size of the current dvl measurements + size_t dvl_size = _curr_dvl_timestamps.size(); + size_t dvl_local_size = _curr_dvl_local_timestamps.size(); + + // If any of the size is zero, assert error + if (dvl_size == 0 && dvl_local_size == 0) { + assert(false); + } + + std::vector interpolated_rotations; + calculate_interpolated_rotations(using_slerp, interpolated_rotations); + + gtsam::Point3 integrated_pose_translation = gtsam::Point3(0.0, 0.0, 0.0); + double dt_dvl = -1; + gtsam::Matrix3 integrated_rot_matrix = gtsam::Matrix3::Identity(); + for (size_t i = 0; i < dvl_size; ++i) { + dt_dvl = (dt_dvl == -1) + ? _curr_dvl_timestamps[i] - _prev_keyframe_time + : _curr_dvl_timestamps[i] - _curr_dvl_timestamps[i-1]; + integrated_rot_matrix = integrated_rot_matrix * interpolated_rotations[i].matrix(); + _preintegrated_velocity_measurements->integrate_measurements( + _curr_dvl_vels[i], gtsam::Rot3(integrated_rot_matrix), dt_dvl + ); + integrated_pose_translation += integrated_rot_matrix * _curr_dvl_vels[i] * dt_dvl; + std::cout << "[add_dvl_factor] dt_dvl: " << dt_dvl << "\n"; + std::cout << "[add_dvl_factor] _curr_dvl_vels[i]: " << _curr_dvl_vels[i] << "\n"; + std::cout << "[add_dvl_factor] integrated_pose_translation: " << integrated_pose_translation << "\n"; + } + + gtsam::Rot3 integrated_pose_rotation; + calculate_integrated_pose_rotation(integrated_pose_rotation); + + gtsam::Pose3 integrated_pose(integrated_pose_rotation, integrated_pose_translation); + + if (_index > 0) { + _initial->insert(gtsam::Symbol('x', _index), _prev_pose * integrated_pose); + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + integrated_pose, + gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector6::Constant(0.01) + ) + ); + _graph->emplace_shared( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + gtsam::Symbol('d', _index), + *_preintegrated_velocity_measurements + ); + _graph->emplace_shared>( + gtsam::Symbol('d', _index - 1), + gtsam::Symbol('d', _index), + gtsam::imuBias::ConstantBias(gtsam::Vector3(0, 0, 0), gtsam::Vector(0, 0, 0)), + gtsam::noiseModel::Isotropic::Sigma(6, 1e-3) + ); + } + + reset_dvl_odometry(); + clear_dvl_timestamp_pose_vel(); + _preintegrated_velocity_measurements->reset_integration(); +} + +void Posegraph::add_dvl_factor_imu_rotation() { + int dvl_size = _curr_dvl_timestamps.size(); + assert(_curr_dvl_vels.size() == _imu_rot_list.size()); + + double dt_dvl = -1; + gtsam::Point3 integrated_pose_translation = gtsam::Point3(0.0, 0.0, 0.0); + gtsam::Matrix3 integrated_rot_matrix = gtsam::Matrix3::Identity(); + + for (size_t i = 0; i < dvl_size; ++i) { + dt_dvl = (dt_dvl == -1) + ? _curr_dvl_timestamps[i] - _prev_keyframe_time + : _curr_dvl_timestamps[i] - _curr_dvl_timestamps[i-1]; + integrated_rot_matrix = _imu_prev_rot.matrix().inverse() * _imu_rot_list[i].matrix(); + _preintegrated_velocity_measurements->integrate_measurements_noise( + _curr_dvl_vels[i], + gtsam::Rot3(integrated_rot_matrix), + dt_dvl, + _curr_dvl_foms[i] + ); + integrated_pose_translation += integrated_rot_matrix.matrix() * _curr_dvl_vels[i] * dt_dvl; + } + gtsam::Rot3 integrated_pose_rotation = _imu_prev_rot.inverse() * _imu_rot_list.back(); + gtsam::Pose3 integrated_pose(integrated_pose_rotation, integrated_pose_translation); + + if (_index > 0) { + _initial->insert(gtsam::Symbol('x', _index), _prev_pose * integrated_pose); + if (_index < 10000) { // TODO REMOVE + _graph->emplace_shared( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + gtsam::Symbol('d', _index - 1), + *_preintegrated_velocity_measurements + ); + + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + integrated_pose, + gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(6) << 0.01, 0.01, 0.01, 0.1, 0.1, 0.1).finished() + ) + ); + } else { + std::cout << "[add_dvl_factor_imu_rotation] no dvl factor added as index greater than 10,000\n"; + } + + _graph->emplace_shared>( + gtsam::Symbol('d', _index - 1), + gtsam::Symbol('d', _index), + gtsam::imuBias::ConstantBias(gtsam::Vector3(0, 0, 0), gtsam::Vector(0, 0, 0)), + gtsam::noiseModel::Isotropic::Sigma(6, 1e-1) + ); + } + + clear_dvl_timestamp_pose_vel(); + _curr_dvl_foms.clear(); + reset_imu_rotation(); + _preintegrated_velocity_measurements->reset_integration(); +} + +void Posegraph::add_velocity_factor() { + int dvl_size = _curr_dvl_timestamps.size(); + int dvl_local_size = _curr_dvl_local_timestamps.size(); + + assert(dvl_size > 0); + + gtsam::Point3 integrated_pose_translation = gtsam::Point3(0.0, 0.0, 0.0); + double dt; + for (size_t i = 0; i < dvl_size; ++i) { + dt = (i == 0) + ? _curr_dvl_timestamps[i] - _prev_keyframe_time + : _curr_dvl_timestamps[i] - _curr_dvl_timestamps[i - 1]; + + gtsam::Rot3 curr_rotation = find_current_pose_for_dvl_vel(_curr_dvl_timestamps[i]); + gtsam::Vector3 curr_velocity = _curr_dvl_vels[i]; + gtsam::Vector3 curr_velocity_world = curr_rotation.matrix() * curr_velocity; + integrated_pose_translation += curr_velocity_world * dt; + } + + gtsam::Rot3 integrated_pose_rotation; + calculate_integrated_pose_rotation(integrated_pose_rotation); + + gtsam::Pose3 integrated_pose(integrated_pose_rotation, integrated_pose_translation); + + if (_index > 0) { + _initial->insert(gtsam::Symbol('x', _index), _prev_pose * integrated_pose); + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + integrated_pose, + gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector6::Constant(0.01) + ) + ); + } + + // clear variables used above + clear_dvl_timestamp_pose_vel(); +} + +void Posegraph::add_dvl_odometry_factor(double noise) { + gtsam::Pose3 between_pose = std::accumulate( + std::begin(_curr_dvl_poses), + std::end(_curr_dvl_poses), + gtsam::Pose3(), + [] (gtsam::Pose3 total, gtsam::Pose3 pose) { + return total * pose; + } + ); + if (_index > 0) { + _graph->emplace_shared>( + gtsam::Symbol('x', _index - 1), + gtsam::Symbol('x', _index), + between_pose, + gtsam::noiseModel::Diagonal::Sigmas( + gtsam::Vector6::Constant(noise) + ) + ); + } +} + +void Posegraph::add_dvl_velocity(double time_stamp, gtsam::Vector3 velocity) { + _curr_dvl_timestamps.push_back(time_stamp); + _curr_dvl_vels.push_back(velocity); +} +void Posegraph::add_dvl_pose(double time_stamp, gtsam::Pose3 pose) { + _curr_dvl_local_timestamps.push_back(time_stamp); + _curr_dvl_poses.push_back(pose); +} +void Posegraph::add_dvl_rotation(double time_stamp, gtsam::Rot3 rotation) { + _curr_dvl_local_timestamps.push_back(time_stamp); + _curr_dvl_rotations.push_back(rotation); +} + + +void Posegraph::add_visual_constraint_factor(gtsam::Pose3 between_pose, double weight, int prev_idx, int curr_idx) { + _graph->emplace_shared>( + gtsam::Symbol('x', prev_idx), + gtsam::Symbol('x', curr_idx), + between_pose, + gtsam::noiseModel::Gaussian::Information( + gtsam::Matrix66::Identity() * weight * 1e-6 + ) + ); +} + + +void Posegraph::add_prior_factor(gtsam::Pose3 initial_pose, gtsam::Vector initial_vel, double pose_noise) { + _graph->emplace_shared>( + gtsam::Symbol('x', 0), + initial_pose, + gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(6) << pose_noise, pose_noise, pose_noise, pose_noise, pose_noise, pose_noise).finished() + ) + ); + _graph->emplace_shared>( + gtsam::Symbol('v', 0), + initial_vel, + gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(3) << pose_noise, pose_noise, pose_noise).finished() + ) + ); +} + +// Creating transforms +void Posegraph::define_transforms() { + _T_SD = _pose_graph_params->_extrinsics.T_SD; + _T_SB = _pose_graph_params->_extrinsics.T_BS; + _T_W_WD = _pose_graph_params->_extrinsics.T_W_WD; +} + +// Adding estimates +void Posegraph::add_simple_estimate(double dt, double noise) { + // Create a random vector of dim 3 + gtsam::Vector3 noisy = gtsam::Vector3(_normal_distribution(_rng), _normal_distribution(_rng), _normal_distribution(_rng)) * noise; + if (_pose_graph_params->_sensor_usage.is_imu_used) { + gtsam::Pose3 pose_i = _initial->at(gtsam::Symbol('x', _index - 1)); + gtsam::Vector3 vel_i = _initial->at(gtsam::Symbol('v', _index - 1)); + gtsam::imuBias::ConstantBias bias_i = _initial->at(gtsam::Symbol('b', _index - 1)); + + // Compute translation update + gtsam::Vector3 W_dp_S = pose_i.rotation().matrix() * (vel_i * dt + 0.5 * std::pow(dt, 2) * noisy); + gtsam::Vector3 W_p_S = pose_i.translation() + W_dp_S; + + // Compute rotation update + // Perturb the rotation with a small angle about a random axis + gtsam::Vector3 axis = gtsam::Vector3(_normal_distribution(_rng), _normal_distribution(_rng), _normal_distribution(_rng)); + axis.normalize(); + double angle = _normal_distribution(_rng); + gtsam::Rot3 R_W_S = gtsam::Rot3::Rodrigues(axis * angle) * pose_i.rotation(); + + // Compute pose + gtsam::Pose3 pred_pose(R_W_S, W_p_S); + + // Compute velocity update + gtsam::Vector3 B_v_S = vel_i + dt * noisy; + gtsam::Vector3 &pred_vel = B_v_S; + + // Create a bias by perturbing the prior imu bias + gtsam::imuBias::ConstantBias pred_bias( + _prior_imu_bias.gyroscope() + gtsam::Vector3(_normal_distribution(_rng), _normal_distribution(_rng), _normal_distribution(_rng)), + _prior_imu_bias.accelerometer() + gtsam::Vector3(_normal_distribution(_rng), _normal_distribution(_rng), _normal_distribution(_rng)) + ); + + // Push to initial + _initial->insert(gtsam::Symbol('x', _index), pred_pose); + _initial->insert(gtsam::Symbol('v', _index), pred_vel); + _initial->insert(gtsam::Symbol('b', _index), pred_bias); + _initial->insert(gtsam::Symbol('d', _index), _prior_dvl_bias); + } +} + +void Posegraph::add_initial_estimate(gtsam::Pose3 initial_pose, gtsam::Vector3 intitial_vel) { + _initial->insert(gtsam::Symbol('x', _index), initial_pose); + if (_pose_graph_params->_sensor_usage.is_imu_used) { + _initial->insert(gtsam::Symbol('b', _index), _prior_imu_bias); + } + if (_pose_graph_params->_sensor_usage.is_dvl_used) { + _initial->insert(gtsam::Symbol('d', _index), _prior_dvl_bias); + } +} + +void Posegraph::add_visual_estimate(const std::vector vertex) { + Eigen::Quaterniond quaternion(vertex[8], vertex[5], vertex[5], vertex[7]); + Eigen::Vector3d translation(vertex[2], vertex[3], vertex[4]); + Eigen::Vector3d velocity(vertex[9], vertex[10], vertex[11]); + Eigen::Vector3d bias_acc(vertex[12], vertex[13], vertex[14]); + Eigen::Vector3d bias_gyro(vertex[15], vertex[16], vertex[17]); + + // Convert quaternion to Rot3 + gtsam::Rot3 R(quaternion.normalized().toRotationMatrix()); + + // Convert translation to Point3 + gtsam::Point3 p(translation); + + // Create Pose 3 + gtsam::Pose3 W_visual_estimated_pose(R, p); + gtsam::imuBias::ConstantBias visual_estimated_bias(bias_acc, bias_gyro); + gtsam::Point3 B_visual_esimate_velocity(velocity); + + // Set previous pose to be the estimated one + _prev_pose = W_visual_estimated_pose; + + // Push to initial + _initial->insert(gtsam::Symbol('x', _index), W_visual_estimated_pose); + if (_pose_graph_params->_sensor_usage.is_imu_used) { + _initial->insert(gtsam::Symbol('v', _index), B_visual_esimate_velocity); + _initial->insert(gtsam::Symbol('b', _index), visual_estimated_bias); + } + if (_pose_graph_params->_sensor_usage.is_dvl_used) { + _initial->insert(gtsam::Symbol('d', _index), _prior_dvl_bias); + } +} + +// Working with the pose graph +void Posegraph::initialize_pose_graph() { + // If we are not using orbslam, it is okay to keep pose from identity + gtsam::Pose3 prior_pose = gtsam::Pose3(gtsam::Rot3(), gtsam::Point3(0, 0, 0)); + if (_pose_graph_params->_using_orbslam) { + assert(false); // TODO + } + + add_initial_estimate(prior_pose, gtsam::Vector3()); + _prev_pose = prior_pose; + add_prior_factor(prior_pose, gtsam::Vector3(), 0.001); +} + +void Posegraph::initialize_pose_graph_from_imu(gtsam::Rot3 initial_rotation) { + gtsam::Pose3 prior_pose = gtsam::Pose3(initial_rotation, gtsam::Point3(0, 0, 0)); + add_initial_estimate(prior_pose, gtsam::Vector3()); + _prev_pose = prior_pose; + add_prior_factor(prior_pose, gtsam::Vector3(), 0.001); +} + +void Posegraph::optimize_pose_graph() { + gtsam::LevenbergMarquardtParams lm_params; + gtsam::LevenbergMarquardtOptimizer optimizer(*_graph, *_initial, lm_params); + + *_result = optimizer.optimize(); + std::cout << "[optimize_pose_graph] Initial error = " << _graph->error(*_initial) << "\n"; + *_initial = *_result; + std::cout << "[optimize_pose_graph] Final error = " << _graph->error(*_result) << "\n"; +} + +void Posegraph::optimize_pose_graph_smoother() { + gtsam::FactorIndices delete_slots; + _smoother_ISAM2.update(*_graph, *_initial, _smoother_timestamps, delete_slots); + *_result = _smoother_ISAM2.calculateEstimate(); + + // Calculate error + double error = _smoother_ISAM2.getFactors().error(*_result); + if (std::isnan(error)) { + std::cout << "[optimize_pose_graph_smoother] Error is NaN \n"; + assert(false); + } + std::cout << "[optimize_pose_graph_smoother] Final error = " << _smoother_ISAM2.getFactors().error(*_result) << "\n"; + + _smoother_timestamps.clear(); + _initial->clear(); + _graph->resize(0); +} + +gtsam::Rot3 Posegraph::find_current_pose_for_dvl_vel(double time_stamp) const { + // find the difference between previous time and current time + gtsam::Pose3 current_keyframe_pose = _prev_pose; + double current_time_diff = time_stamp - _prev_keyframe_time; + double min_diff = std::numeric_limits::max(); + + auto minimum = std::accumulate( + std::begin(_curr_dvl_local_timestamps), + std::end(_curr_dvl_local_timestamps), + std::make_tuple(0, std::numeric_limits::max(), -1), + [&time_stamp] (std::tuple mins, double next_time_stamp) { + auto [index, min_diff, min_index] = mins; + double difference = abs(time_stamp - next_time_stamp); + bool less = difference < min_diff; + return std::make_tuple( + index + 1, + (less ? difference : min_diff), + (less ? index : min_index) + ); + } + ); + + double min_time_diff; + int min_index; + std::tie(std::ignore, min_time_diff, min_index) = minimum; + + gtsam::Rot3 accumulated_rotation = gtsam::Rot3(); + if (current_time_diff < min_time_diff) { + return accumulated_rotation; + } else { + return std::accumulate( + std::begin(_curr_dvl_poses), + std::end(_curr_dvl_poses), + accumulated_rotation, + [] (gtsam::Rot3 accumulated_rotation, gtsam::Pose3 dvl_pose) { + return accumulated_rotation * dvl_pose.rotation(); + } + ); + } +} + +void Posegraph::calculate_interpolated_rotations(bool is_using_slerp, std::vector &interpolated_rotations) { + if (_curr_dvl_poses.empty() || _curr_dvl_timestamps.empty()) return; + + gtsam::Rot3 prev_rotation = _prev_dvl_odometry_rot; + + gtsam::Rot3 curr_rotation = _curr_dvl_poses[0].rotation(); + double last_local_time = _prev_dvl_odometry_time; + + int next_local_idx = 0; + + for (const double &ts : _curr_dvl_timestamps) { + while (ts > _curr_dvl_local_timestamps[next_local_idx] && + next_local_idx < _curr_dvl_local_timestamps.size() - 1) { + + next_local_idx++; + prev_rotation = curr_rotation; + curr_rotation = _curr_dvl_poses[next_local_idx].rotation(); + last_local_time = _curr_dvl_local_timestamps[next_local_idx - 1]; + } + + double denom = _curr_dvl_local_timestamps[next_local_idx] - last_local_time; + double dt_interp = (denom > 1e-6) ? (ts - last_local_time) / denom : 0.0; + + // Clamp to [0, 1] for safety + dt_interp = std::max(0.0, std::min(1.0, dt_interp)); + interpolated_rotations.push_back(prev_rotation.slerp(dt_interp, curr_rotation)); + } +} + +void Posegraph::calculate_integrated_pose_rotation(gtsam::Rot3 &integrated_pose_rotation) { + // Calculate the integrated pose rotation by accumulating rotations + integrated_pose_rotation = std::accumulate( + std::begin(_curr_dvl_poses), + std::end(_curr_dvl_poses), + gtsam::Rot3(), + [](gtsam::Rot3 total, gtsam::Pose3 pose) { + return total * pose.rotation(); + } + ); +} + +void Posegraph::reset_dvl_odometry() { + _prev_dvl_odometry_time = _curr_dvl_local_timestamps.back(); + _prev_dvl_odometry_rot = _curr_dvl_poses.back().rotation(); +} + +void Posegraph::clear_dvl_timestamp_pose_vel() { + _curr_dvl_timestamps.clear(); + _curr_dvl_vels.clear(); + _curr_dvl_poses.clear(); + _curr_dvl_local_timestamps.clear(); +} + + +void Posegraph::reset_imu_rotation() { + _imu_prev_rot = _imu_rot_list.back(); + _imu_rot_list.clear(); +} + +// Getters +double Posegraph::get_depth_measurement() const { + return _W_measurement_z; +} + +gtsam::Vector3 Posegraph::get_accelerometer_measurement() const { + return _B_accelerometer_S; +} + +gtsam::Vector3 Posegraph::get_gyroscope_measurement() const { + return _B_gyroscope_S; +} + +gtsam::Vector3 Posegraph::get_velocity_measurement() const{ + return _B_velocity_D; +} + +gtsam::Vector3 Posegraph::get_position_measurement() const { + return _W_position_C; +} + +double Posegraph::get_visual_gap_time() const { + return _visual_gap_time; +} + +// Setters +void Posegraph::set_depth_measurement(double W_measurement_z) { + _W_measurement_z = W_measurement_z; +} + +void Posegraph::set_accelerometer_measurement(gtsam::Vector3 B_accelerometer_S) { + _B_accelerometer_S = B_accelerometer_S; +} + +void Posegraph::set_gyroscope_measurement(gtsam::Vector3 B_gyroscope_S) { + _B_gyroscope_S = B_gyroscope_S; +} + +void Posegraph::set_velocity_measurement(gtsam::Vector3 B_velocity_D) { + _B_velocity_D = B_velocity_D; +} + +void Posegraph::set_position_measurement(gtsam::Vector3 W_position_C) { + _W_position_C = W_position_C; +} + +void Posegraph::set_visual_gap_time(double visual_gap_time) { + _visual_gap_time = visual_gap_time; +} + +} // namespace localization \ No newline at end of file diff --git a/mrobosub_localization_cpp/src/PosegraphNode.cpp b/mrobosub_localization_cpp/src/PosegraphNode.cpp new file mode 100644 index 00000000..29c27158 --- /dev/null +++ b/mrobosub_localization_cpp/src/PosegraphNode.cpp @@ -0,0 +1,504 @@ +#include "PosegraphNode.h" +#include "mrobosub_msgs/msg/Imu.hpp" +#include "mrobosub_msgs/msg/Dvl.hpp" +#include "std_msgs/msg/float64.hpp" + +namespace localization { + +PosegraphNode::PosegraphNode() + : Node("posegraph") + , _first_depth(0.0) + , _prev_dvl_time(0.0) + , _prev_dvl_local_time(0.0) + , _prev_keyframe_time(0.0) + , _current_keyframe_time(0.0) + , _is_new_keyframe(false) + , _imu_init_count(0) + , _imu_count(0) + , _is_using_dvl_v2_factor(true) + , _is_rot_initialized(false) { + + RCLCPP_INFO(this->get_logger(), "'posegraph' node has started"); + + // Create posegraph variables + _posegraph = std::make_unique(); + _preintegrated_measurements_dvl = std::make_unique( + boost::make_shared( + _posegraph->_preintegrated_measurement_params + ), + _posegraph->_prior_imu_bias + ); + _keyframe_gap_time = _posegraph->_pose_graph_params->_keyframe_gap_time; + + // Set up the transform + _tf_broadcaster = std::make_unique(*this); + + // Set up callback groups + _imu_callback_group = this->create_callback_group(rclcpp::CallbackGroup::MutuallyExclusive); + _low_freq_sensor_callback_group = this->create_callback_group(rclcpp::CallbackGroup::MutuallyExclusive); + _posegraph_callback_group = this->create_callback_group(rclcpp::CallbackGroup::MutuallyExclusive); + _keyframe_callback_group = this->create_callback_group(rclcpp::CallbackGroup::MutuallyExclusive); + + // Create ROS subscriptions + auto imu_options = rclcpp::SubscriptionOptions(); + imu_options.callback_group = _imu_callback_group; + + auto low_freq_options = rclcpp::SubscriptionOptions(); + low_freq_options.callback_group = _low_freq_sensor_callback_group; + + _imu_sub = this->create_subscription( + "/imu", 10, + std::bind(&PosegraphNode::imu_callback, this, std::placeholders::_1), + imu_options + ); + + _dvl_sub = this->create_subscription( + "/dvl/twist", 10, + std::bind(&PosegraphNode::dvl_callback, this, std::placeholders::_1), + low_freq_options + ); + + _baro_sub = this->create_subscription( + "/depth", 10, + std::bind(&PosegraphNode::baro_callback, this, std::placeholders::_1), + low_freq_options + ); + + _dvl_local_sub = this->create_subscription( + "/dvl_local", 10, + std::bind(&PosegraphNode::dvl_local_callback, this, std::placeholders::_1), + low_freq_options + ); + + // Create ROS publishers + _pose_pub = this->create_publisher("/localization/pose", 1000); + _dvl_local_pose_pub = this->create_publisher("/localization/dvl_local_pose", 1000); + _path_pub = this->create_publisher("/localization/path", 1000); + + // Create timers + _posegraph_timer = this->create_wall_timer( + std::chrono::milliseconds(50), + std::bind(&PosegraphNode::main_loop, this), + _posegraph_callback_group + ); + + // TODO: Should this be on a separate keyframe_callback_group? + _keyframe_timer = this->create_wall_timer( + std::chrono::milliseconds(50), + std::bind(&PosegraphNode::kf_loop, this), + _keyframe_callback_group + ); +} + +void PosegraphNode::main_loop() { + if (_is_new_keyframe) { + _is_new_keyframe = false; + _posegraph->add_barometric_factor(_posegraph->get_depth_measurement(), 0.1, _posegraph->_index); + if (_is_using_dvl_v2_factor) { + _posegraph->add_dvl_factor(true); // using slerp true + } else { + throw std::runtime_error("Not implemented yet!"); + } + + if (_posegraph->_index > 0) { + _posegraph->optimize_pose_graph(); + _latest_keyframe_pose = _posegraph->_result->at(gtsam::Symbol('x', _posegraph->_index)); + _latest_publish_pose = _latest_keyframe_pose; + _posegraph->_prev_pose = _latest_keyframe_pose; + if (_is_using_dvl_v2_factor) { + _posegraph->_prior_dvl_bias = _posegraph->_result->at(gtsam::Symbol('d', _posegraph->_index)); + _posegraph->_preintegrated_velocity_measurements->reset_integration_and_bias(_posegraph->_prior_dvl_bias); + } else { + throw std::runtime_error("Not implemented yet!"); + } + } + + _posegraph->_prev_keyframe_time = _current_keyframe_time; + _posegraph->_index++; + _posegraph->_initial->insert(gtsam::('d', _posegraph->_index), _posegraph->_prior_dvl_bias); + } +} + +void PosegraphNode::kf_loop() { + _posegraph->_index++; + + _keyframe_timestamps.push_back(_current_keyframe_time); + double time = _current_keyframe_time - _first_keyframe_time; + _posegraph->_initial->insert(gtsam::Symbol('d', _posegraph->_index), _posegraph->_prior_dvl_bias); + _posegraph->_initial->insert(gtsam::Symbol('b', _posegraph->_index), _posegraph->_prior_imu_bias); + + // Add smoothing timestamps + _posegraph->_smoother_timestamps[gtsam::Symbol('d', _posegraph->_index)] = time; + _posegraph->_smoother_timestamps[gtsam::Symbol('b', _posegraph->_index)] = time; + _posegraph->_smoother_timestamps[gtsam::Symbol('x', _posegraph->_index)] = time; + + // Start to process new kf + if (_posegraph->_index < 10000) { + _posegraph->add_barometric_factor(_posegraph->get_depth_measurement(), 0.005, _posegraph->_index); + } + + if (_posegraph->_index > 1) { + _posegraph->_initial->insert( + gtsam::Symbol('v', _posegraph->_index), + _posegraph->_result->at( + gtsam::Symbol('v', _posegraph->_index-1), + ) + ); + } else { + _posegraph->_initial->insert( + gtsam::Symbol('v', _posegraph->_index), + gtsam::Vector3(0, 0, 0) + ); + } + _posegraph->_smoother_timestamps[gtsam::Symbol('v', _posegraph->_index)] = time; + + _posegraph->add_imu_factor(); + if (_is_using_dvl_v2_factor) { + _posegraph->add_dvl_factor_imu_rotation(); + } else { + throw std::runtime_error("Not implemented yet!"); + } + + if (_posegraph->_index > 0) { + if (!_posegraph->_pose_graph_params->_using_smoother) { + _posegraph->optimize_pose_graph(); + } else { + _posegraph->optimize_pose_graph_smoother(); + } + + _posegraph->_prior_imu_bias = _posegraph->_result->at(gtsam::Symbol('b', _posegraph->_index)); + _posegraph->_preintegrated_measurements->resetIntegrationAndSetBias(_posegraph->_prior_imu_bias); + _preintegrated_measurements_dvl->resetIntegrationAndSetBias(_posegraph->_prior_imu_bias); + + _latest_keyframe_pose = _posegraph->_result->at(gtsam::Symbol('x', _posegraph->_index)); + _latest_publish_pose = _latest_keyframe_pose; + _latest_dvl_pose = _latest_keyframe_pose; + _latest_dvl_vel = _posegraph->_result->at(gtsam::Symbol('v', _posegraph->_index)); + _posegraph->_prev_pose = _latest_keyframe_pose; + + if (_is_using_dvl_v2_factor) { + _posegraph->_prior_dvl_bias = _posegraph->_result->at(gtsam::Symbol('d', _posegraph->_index)) + gtsam::Point3 dvl_bias_point3 = _posegraph->_prior_dvl_bias.accelerometer(); + dvl_bias_point3 = _posegraph->_prev_pose.rotation() * dvl_bias_point3; + _posegraph->_preintegrated_velocity_measurements->reset_integration_and_bias(); + } else { + throw std::runtime_error("Not implemented yet"); + } + } + + _posegraph->_prev_keyframe_time = _current_keyframe_time; + _is_new_keyframe = false; +} + +void PosegraphNode::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { + // This function is guaranteed to be mutually exclusive to all other + // functions since it is on a separate timer. + + if (!_is_rot_initialized) { + _imu_init_count++; + // get the rotation from the first imu message + gtsam::Rot3 imu_rot = gtsam::Rot3(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z); + _imu_init_rot.push_back(imu_rot.xyz()); + } else { + gtsam::Vector3 imu_acc = gtsam::Vector3(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z); + gtsam::Vector3 imu_gyro = gtsam::Vector3(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); + _imu_latest_rot = gtsam::Rot3(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z); + _posegraph->_preintegrated_measurements->integrateMeasurement(imu_acc, imu_gyro, _posegraph->_pose_graph_params->_imu_params.dt_imu); + _preintegrated_measurements_dvl->integrateMeasurement(imu_acc, imu_gyro, _posegraph->_pose_graph_params->_imu_params.dt_imu); + _imu_count++; + + if (_imu_count >= 5) { + _latest_imu_prop_state = _preintegrated_measurements_dvl->predict(gtsam::NavState( + _latest_dvl_pose, + _latest_dvl_pose.rotation() * _latest_dvl_vel, + _posegraph->_prior_imu_bias + )) + + gtsam::Pose3 latest_pose = _latest_imu_prop_state.pose();; + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.stamp = imu_msg->header.stamp; + pose_msg.header.frame_id = "NED_imu"; + pose_msg.pose.position.x = latest_pose.translation().x(); + pose_msg.pose.position.y = latest_pose.translation().y(); + pose_msg.pose.position.z = latest_pose.translation().z(); + pose_msg.pose.orientation.w = latest_pose.rotation().toQuaternion().w(); + pose_msg.pose.orientation.x = latest_pose.rotation().toQuaternion().x(); + pose_msg.pose.orientation.y = latest_pose.rotation().toQuaternion().y(); + pose_msg.pose.orientation.z = latest_pose.rotation().toQuaternion().z(); + _pose_pub.publish(pose_msg); + + // Transform + broadcast_imu_transform(); + _imu_count = 0; + } + } +} + +void PosegraphNode::dvl_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg) { + if(!_is_rot_initialized) return; + + const std::unique_lock lock(_mtx); + double dt_dvl; + double dvl_current_time = msg->header.stamp.toSec(); + gtsam::Vector3 dvl_vel = _posegraph->_T_SD.block(0, 0, 3, 3) * + gtsam::Vector3(msg->velocity.x, msg->velocity.y, msg->velocity.z); + double fom = msg->fom; + bool is_valid = msg->velocity_valid; + + // Handle initial measurement + if (_prev_dvl_time == 0.0) { + _prev_dvl_time = dvl_current_time; + dt_dvl = dvl_current_time - _current_keyframe_time; + _prev_dvl_rot = _imu_latest_rot; + _first_dvl_vel = dvl_vel; + + _posegraph->_initial->insert( + gtsam::Symbol('v', _posegraph->_index), + _first_dvl_vel + ); + _posegraph->_graph->add(gtsam::PriorFactor( + gtsam::Symbol('v', 0), + _first_dvl_vel, + gtsam::noiseModel::Diagonal::shared_ptr prior_vel_noise = gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector(3) << 0.1, 0.1, 0.1).finished() + ) + )) + + // Update for preintegrated_measurements_dvl + _latest_dvl_vel = _first_dvl_vel; + _latest_dvl_pose = _latest_publish_pose; + } else { + dt_dvl = dvl_current_time - _prev_dvl_time; + } + + if (fom >= _posegraph->_pose_graph_params->_dvl_params.fom_threshold && !is_valid) { + gtsam::NavState imu_prop_state = _preintegrated_measurements_dvl->predict( + gtsam::NavState(_latest_dvl_pose), + _latest_dvl_pose.rotation() * _latest_dvl_vel, + _posegraph->_prior_imu_bias + ) + + gtsam::Vector3 old_dvl_vel = dvl_vel; + dvl_vel = imu_prop_state.pose().rotation().inverse() * imu_prop_state.velocity(); + + // Compute difference vector + gtsam::Vector3 dvl_vel_diff = dvl_vel - old_dvl_vel; + + RCLCPP_WARN(this->get_logger(), "Invalid DVL Measurement with diff", dvl_vel_diff); + } + + // Communicate current dvl to posegraph + _posegraph->add_dvl_velocity(dvl_current_time, dvl_vel); + + gtsam::Rot3 d_rot = _prev_dvl_rot.inverse() * _imu_latest_rot; + gtsam::Point3 d_position = d_rot.matrix() * gtsam::Point3( + dvl_vel.x() * dt_dvl, + dvl_vel.y() * dt_dvl, + dvl_vel.z() * dt_dvl, + ); + gtsam::Pose3 d_pose(d_rot, d_position); + _latest_publish_pose = _latest_publish_pose * d_pose; + + // Update latest + _latest_dvl_vel = dvl_vel; + _latest_dvl_pose = _latest_publish_pose; + _preintegrated_measurements_dvl->resetIntegration(); + + _posegraph->_curr_dvl_foms.push_back(fom * 4); // TDOO: multiply fom by 4 to get actual? + _posegraph->_imu_rot_list.push_back(_imu_latest_rot); + _prev_dvl_time = dvl_current_time; + + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.stamp = msg->header.stamp; + pose_msg.header.frome_id = "NED_imu"; + pose_msg.pose.position.x = _latest_publish_pose.translation().x(); + pose_msg.pose.position.y = _latest_publish_pose.translation().y(); + pose_msg.pose.position.z = _latest_publish_pose.translation().z(); + pose_msg.pose.orientation.w = _latest_publish_pose.rotation().toQuaternion().w(); + pose_msg.pose.orientation.x = _latest_publish_pose.rotation().toQuaternion().x(); + pose_msg.pose.orientation.y = _latest_publish_pose.rotation().toQuaternion().y(); + pose_msg.pose.orientation.z = _latest_publish_pose.rotation().toQuaternion().z(); + _pose_pub.publish(pose_msg); + + broadcast_imu_transform(); + + geometry_msgs::msg::TransformStamped world_tf_msg; + world_tf_msg.header.stamp = msg->header.stamp; + world_tf_msg.header.frame_id = "world"; + world_tf_msg.child_frame_id = "NED_imu"; + world_tf_msg.transform.translation.x = 0.0; + world_tf_msg.transform.translation.y = 0.0; + world_tf_msg.transform.translation.z = 0.0; + world_tf_msg.transform.rotation.w = 0.0; + world_tf_msg.transform.rotation.x = 1.0; + world_tf_msg.transform.rotation.y = 0.0; + world_tf_msg.transform.rotation.z = 0.0; + br.sendTransform(world_tf_msg); + + _prev_dvl_rot = _imu_latest_rot; +} + +void PosegraphNode::baro_callback(const sensor_msgs::msg::FluidPressure::SharedPtr msg) { + std::unique_lock lock(_mtx); + if (!_is_rot_initialized) { + _cond_var.wait(lock, [_imu_init_rot] { return _imu_init_rot.size() < 5; }); + gtsam::Vector3 imu_rot_mean(0.0, 0.0, 0.0); + int latest_imu_rot_number = 5; + for (int i = _imu_init_rot.size() - latest_imu_rot_number; i < _imu_init_rot.size(); ++i) { + imu_init_mean += _imu_init_rot[i]; + } + imu_rot_mean /= latest_imu_rot_number; + + gtsam::Rot3 imu_rot = gtsam::Rot3::RzRyRx(imu_rot_mean[0], imu_rot_mean[1], imu_rot_mean[2]); + _imu_latest_rot = imu_rot; + _posegraph->_imu_prev_rot = imu_rot; + _imu_init_rot.clear(); + + _posegraph->initialize_pose_graph_from_imu(imu_rot); + _current_keyframe_time = msg->header.stamp.toSec(); + if (_posegraph->_pose_graph_params->_using_smoother) { + _posegraph->_smoother_timestamps[gtsam::Symbol('x', _posegraph->_index)] = 0.0; + _posegraph->_smoother_timestamps[gtsam::Symbol('v', _posegraph->_index)] = 0.0; + _posegraph->_smoother_timestamps[gtsam::Symbol('b', _posegraph->_index)] = 0.0; + _posegraph->_smoother_timestamps[gtsam::Symbol('d', _posegraph->_index)] = 0.0; + } + + _first_keyframe_time = _current_keyframe_time; + _posegraph->_prev_keyframe_time = _current_keyframe_time; + _keyframe_timestamps.push_back(_current_keyframe_time); + _latest_keyframe_pose = _posegraph->_initial->at(gtsam::Symbol('x', 0)); + _latest_publish_pose = _posegraph->_initial->at(gtsam::Symbol('x', 0)); + + _is_rot_initialized = true; + + // TODO: what is this calculation??? + double depth = (msg->fluid_pressure - _posegraph->_pose_graph_params->_barometer_params.atmospheric_pressure) * 100 / 9.81 * 997.0; + _first_depth = depth; + return; + } + + double depth = (msg->fluid_pressure - _posegraph->_pose_graph_params->_barometer_params.atmospheric_pressure) * 100 / 9.81 * 997.0; + if (_first_depth == 0.0) { + _first_depth = depth; + } + _posegraph->set_depth_measurement(depth - _first_depth); + + double baro_current_time = msg->header.stamp.toSec(); + if (baro_current_time - _current_keyframe_time > _keyframe_gap_time) { + if (_posegraph->_pose_graph_params->_using_pseudo_dvl) { + gtsam::NavState pseudo_state = _preintegrated_measurements_dvl->predict( + gtsam::NavState( + _latest_dvl_pose, + _latest_dvl_pose.rotation() * _latest_dvl_vel + ), + _posegraph->_prior_imu_bias + ); + + gtsam::Vector3 pseudo_dvl_vel = pseudo_state.pose().rotation().inverse() * pseudo_state.velocity(); + + _posegraph->add_dvl_velocity(baro_current_time, pseudo_dvl_vel); + _posegraph->_imu_rot_list.push_back(_imu_latest_rot); + _posegraph->_curr_dvl_foms.push_back(0.02); + _prev_dvl_time = baro_current_time; + } + _current_keyframe_time = baro_current_time; + _new_keyframe_flag = true; + _cond_var.notify_one(); + } +} + +void PosegraphNode::dvl_local_callback(const mrobosub_msgs::msg::Dvl::SharedPtr msg) { + std::unique_lock lock(_mtx); + double dt_dvl_local; + double dvl_local_current_time = msg->header.stamp.toSec(); + gtsam::Pose3 dvl_local_pose = gtsam::Pose3( + gtsam::Rot3( + msg->pose.pose.orientation.w, + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z + ), + gtsam::Point3( + msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z + ) + ); + + if (_prev_dvl_local_time == 0.0) { + _prev_dvl_local_time = dvl_local_current_time; + dt_dvl_local = dvl_local_current_time - _current_keyframe_time; + _prev_dvl_local_pose = dvl_local_pose; + _T_w_wd = _latest_publish_pose * gtsam::Pose3(_posegraph->_T_SD) * _prev_dvl_local_pose.inverse(); + } else { + dt_dvl_local = dvl_local_current_time - _prev_dvl_local_time; + } + + Eigen::Quaterniond q( + msg->pose.pose.orientation.w, + msg->pose.pose.orientation.x, + msg->pose.pose.orientation.y, + msg->pose.pose.orientation.z + ); + gtsam:Rot3 dvl_pose = gtsam::Pose3( + gtsam::Rot3(q.normalized().toRotationMatrix()) + gtsam::Point3( + msg->pose.pose.position.x, + msg->pose.pose.position.y, + msg->pose.pose.position.z + ) + ); + + gtsam::Pose3 d_pose = _prev_dvl_local_pose.inverse() * dvl_pose; + _posegraph->add_dvl_pose(dvl_local_current_time, d_pose); + dvl_local_pose = _T_w_wd * _dvl_local_pose * gtsam::Pose3(_posegraph->_T_SD).inverse(); + + gtsam::Pose3 T_sensor_zed = gtsam::Pose3( + gtsam::Rot3(0, 0, 0, 0), + gtsam::Point3(0, 0, 0) + ) + dvl_local_pose = dvl_local_pose * T_sensor_zed.inverse(); + + geometry_msgs::msg::TransformStamped dvl_local_pose_msg; + dvl_local_pose_msg.header.stamp = msg->header.stamp; + dvl_local_pose_msg.header.frame_id = "NED_imu"; + dvl_local_pose_msg.pose.position.x = dvl_local_pose.translation().x(); + dvl_local_pose_msg.pose.position.y = dvl_local_pose.translation().y(); + dvl_local_pose_msg.pose.position.z = dvl_local_pose.translation().z(); + dvl_local_pose_msg.pose.orientation.w = dvl_local_pose.rotation().toQuaternion().w(); + dvl_local_pose_msg.pose.orientation.x = dvl_local_pose.rotation().toQuaternion().x(); + dvl_local_pose_msg.pose.orientation.y = dvl_local_pose.rotation().toQuaternion().y(); + dvl_local_pose_msg.pose.orientation.z = dvl_local_pose.rotation().toQuaternion().z(); + _dvl_local_pose_publisher.publish(dvl_local_pose_msg); + + _prev_dvl_local_time = dvl_local_current_time; + _prev_dvl_local_pose = dvl_pose; +} + +void PosegraphNode::broadcast_imu_transform() { + // Back calculate where the base_link must be based off of where the sensor says it is. + // T_sensor_zed is the static offset of the sensor relative to the robot's origin + // TODO! + gtsam::Pose3 T_sensor_zed = gtsam::Pose3( + gtsam::Rot3(0, 0, 0, 0), + gtsam::Point3(0, 0, 0) + ) + gtsam::Pose3 latest_publish_pose_base_link = latest_pose * T_sensor_zed.inverse(); + + geometry_msgs::msg::TransformStamped pose_tf_msg; + pose_tf_msg.header.stamp = imu_msg->header.stamp; + pose_tf_msg.header.frame_id = "NED_imu"; + pose_tf_msg.child_frame_id = "base_link"; + pose_tf_msg.pose.position.x = latest_pose.translation().x(); + pose_tf_msg.pose.position.y = latest_pose.translation().y(); + pose_tf_msg.pose.position.z = latest_pose.translation().z(); + pose_tf_msg.pose.orientation.w = latest_pose.rotation().toQuaternion().w(); + pose_tf_msg.pose.orientation.x = latest_pose.rotation().toQuaternion().x(); + pose_tf_msg.pose.orientation.y = latest_pose.rotation().toQuaternion().y(); + pose_tf_msg.pose.orientation.z = latest_pose.rotation().toQuaternion().z(); + _tf_broadcaster.sendTransform(pose_tf_msg); +} + + + +} // namespace localization \ No newline at end of file diff --git a/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp b/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp new file mode 100644 index 00000000..e95456d1 --- /dev/null +++ b/mrobosub_localization_cpp/src/PreintegratedVelocityHelpers.cpp @@ -0,0 +1,132 @@ +#include "PreintegratedVelocityHelpers.h" + +namespace localization { + PreintegratedVelocityParameters::PreintegratedVelocityParameters() + : _bias_velocity_covariance(gtsam::Matrix3::Zero()) + , _bias_initial(gtsam::Matrix3::Zero()) {} + + PreintegratedVelocityParameters::PreintegratedVelocityParameters( + const gtsam::Matrix3 &bias_velocity_covariance, + const gtsam::Matrix3 &bias_initial + ) : _bias_velocity_covariance(bias_velocity_covariance) + , _bias_initial(bias_initial) {} + + PreintegratedVelocityParameters::PreintegratedVelocityParameters( + const boost::shared_ptr &p + ) { + _bias_velocity_covariance = p->_bias_velocity_covariance; + _bias_initial = p->_bias_initial; + } + + // TODO: Make as overloaded= and overloaded<< + // void PreintegratedVelocityParameters::print(const std::string &s = "") const; + // bool PreintegratedVelocityParameters::equals(const PreintegratedVelocityParameters &expected, double tolerance = 1e-9) const; + + void PreintegratedVelocityParameters::set_bias_velocity_covariance(const gtsam::Matrix3 &covariance) { + _bias_velocity_covariance = covariance; + } + void PreintegratedVelocityParameters::set_bias_initial(const gtsam::Matrix3 &covariance) { + _bias_initial = covariance; + } + + + PreintegratedVelocityMeasurementsDvlOnly::PreintegratedVelocityMeasurementsDvlOnly() + : _preintegrated_measured_covariance(gtsam::Matrix3::Zero()) {} + + PreintegratedVelocityMeasurementsDvlOnly::~PreintegratedVelocityMeasurementsDvlOnly() {} + + void PreintegratedVelocityMeasurementsDvlOnly::reset_integration() { + _accumulated_poses = gtsam::Pose3(); + _accumulated_positions = gtsam::Point3(0, 0, 0); + _delp_delbias_dvl = gtsam::Matrix3::Zero(); + _preintegrated_measured_covariance = gtsam::Matrix3::Zero(); + _velocity_list.clear(); + _rotations_list.clear(); + _dt_list.clear(); + } + + void PreintegratedVelocityMeasurementsDvlOnly::reset_integration_and_bias(const gtsam::imuBias::ConstantBias &bias) { + reset_integration(); + _dvl_bias_for_imu = bias; + } + + void PreintegratedVelocityMeasurementsDvlOnly::integrate_measurements( + const gtsam::Vector3 &linear_velocity, + const gtsam::Rot3 &interpolated_rotation, + double &dt // TODO: Is this an out param? Why is this passed by ref? + ) { + if (dt <= 0) { + std::cout << "Warning: dt <= 0\n"; + dt = 0.0001; + } + + _velocity_list.push_back(linear_velocity); + _rotations_list.push_back(interpolated_rotation); + _dt_list.push_back(dt); + } + + void PreintegratedVelocityMeasurementsDvlOnly::integrate_measurements_noise( + const gtsam::Vector3 &linear_velocity, + const gtsam::Rot3 &interpolated_rotation, + double &dt, // TODO: Is this an out param? Why is this passed by ref? + const double &fom + ) { + if (dt <= 0) { + std::cout << "Warning: dt <= 0\n"; + dt = 0.0001; + } + + // TODO: Investigate this. + gtsam::Matrix3 B = interpolated_rotation.matrix() * dt; + gtsam::Matrix3 dvl_covariance = fom * fom * gtsam::I_3x3; // Figure of Merit should be SQUARED + _preintegrated_measured_covariance += B * dvl_covariance * B.transpose(); + + _velocity_list.push_back(linear_velocity); + _rotations_list.push_back(interpolated_rotation); + _dt_list.push_back(dt); + } + + gtsam::Point3 PreintegratedVelocityMeasurementsDvlOnly::predict(const gtsam::Point3 &bias, gtsam::Matrix3 &H_bias) const { + gtsam::Point3 predicted_position; + + // Resets the H_bias + H_bias = gtsam::Matrix3::Zero(); + + // integrate using _velocity_list and _rotations_list + for (int i = 0; i < _velocity_list.size(); ++i) { + gtsam::Vector3 velocity = _velocity_list[i] - bias; + gtsam::Vector3 delta_pos = _rotations_list[i].matrix() * velocity * _dt_list[i]; // Looks like pretty bog-standard integration to me. + predicted_position += delta_pos; + H_bias += _rotations_list[i].matrix() * _dt_list[i]; // What is this bias? + } + + return predicted_position; + } + + gtsam::Pose3 PreintegratedVelocityMeasurementsDvlOnly::get_accumulated_poses() const { + return _accumulated_poses; + } + + gtsam::Point3 PreintegratedVelocityMeasurementsDvlOnly::get_accumulated_positions() const { + return _accumulated_positions; + } + + gtsam::Matrix PreintegratedVelocityMeasurementsDvlOnly::get_delpij_delbias_omega() const { + return _delp_delbias_omega; + } + + gtsam::Matrix PreintegratedVelocityMeasurementsDvlOnly::get_delpij_delbias_dvl() const { + return _delp_delbias_dvl; + } + + gtsam::Matrix PreintegratedVelocityMeasurementsDvlOnly::get_preintegrated_measured_covariance() const { + return _preintegrated_measured_covariance; + } + + void PreintegratedVelocityMeasurementsDvlOnly::update_accumulated_stats(const gtsam::imuBias::ConstantBias &bias) { + gtsam::Matrix3 H; + _accumulated_positions = predict(bias.accelerometer(), H); + _delp_delbias_dvl = H; + } + +} // namespace localization \ No newline at end of file diff --git a/mrobosub_localization_cpp/src/localization.cpp b/mrobosub_localization_cpp/src/localization.cpp new file mode 100644 index 00000000..5da1e625 --- /dev/null +++ b/mrobosub_localization_cpp/src/localization.cpp @@ -0,0 +1,16 @@ +#include +#include "PosegraphNode.h" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + + executor.spin(); + + rclcpp::shutdown(); + return 0; +} diff --git a/mrobosub_msgs/CMakeLists.txt b/mrobosub_msgs/CMakeLists.txt index c788f208..b4e9e81f 100644 --- a/mrobosub_msgs/CMakeLists.txt +++ b/mrobosub_msgs/CMakeLists.txt @@ -16,6 +16,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Detection.msg" "msg/Detections.msg" "msg/Dvl.msg" + "msg/Imu.msg" "msg/ImuINS.msg" "msg/ImuPIMU.msg" "msg/MotorState.msg" diff --git a/mrobosub_msgs/msg/Dvl.msg b/mrobosub_msgs/msg/Dvl.msg index 3a6b6b89..ea28aac0 100644 --- a/mrobosub_msgs/msg/Dvl.msg +++ b/mrobosub_msgs/msg/Dvl.msg @@ -1,2 +1,4 @@ std_msgs/Header header -float32[3] velocity \ No newline at end of file +float32[3] pd +float32[3] ad +float32 confidence diff --git a/mrobosub_msgs/msg/Imu.msg b/mrobosub_msgs/msg/Imu.msg new file mode 100644 index 00000000..18722567 --- /dev/null +++ b/mrobosub_msgs/msg/Imu.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +geometry_msgs/Vector3 angular_velocity +geometry_msgs/Vector3 linear_acceleration +geometry_msgs/Quaternion orientation +float64 dt # Time difference since last measurement in seconds + # (Used to calculate velocity and accel from diffs) \ No newline at end of file diff --git a/mrobosub_perception/README.md b/mrobosub_perception/README.md new file mode 100644 index 00000000..1ede8e50 --- /dev/null +++ b/mrobosub_perception/README.md @@ -0,0 +1,43 @@ +# How to install torch, torchvision on fresh jetson + +Updated Apr 2026 by Muskaan Mittal (muskaan@umich.edu) + +Sorry I don't have more detailed instructions, installing this drove me insane. + +Basically, you have to install cuda (if cuda wasn't already installed when the jetpack was), torch, and torchvision + +## STEP 1. Find out Jetpack version +At time of writing doc, we are using jetpack 6.1; which works well with CUDA 12.6 + +## STEP 2: Install appropriate CUDA + +## STEP 3: Install appropriate torch. You CANNOT do pip install torch +I used the instructions here: https://docs.nvidia.com/deeplearning/frameworks/install-pytorch-jetson-platform/index.html +and found out the appropriate version of torch using some hunting + +the export TORCH_INSTALL url came out to https://developer.download.nvidia.com/compute/redist/jp/v61/pytorch/ (... insert appropriate version) + +## STEP 4: Install other dependencies and test torch installation +Test installation by running this: +python3 -c "import torch; print(torch.__version__)" +python3 -c "import torch; print(torch.cuda.is_available())" + +You'll probably run into a whole host of python packages you don't have. Just sudo apt/ pip install them. + +## STEP 5: Install torchvision +There are pre-built wheels but there were none that matched CUDA 12.6, so I built from source which took about 30 min + +These are the instructions Claude gave me; some iteration of them worked successfully: + +### Install dependencies +sudo apt-get install -y libjpeg-dev libpng-dev libtiff-dev libavcodec-dev libavformat-dev libswscale-dev + +### Clone torchvision +git clone https://github.com/pytorch/vision.git +cd vision +git checkout tags/v0.20.0 + +### Build and install +sudo python3 setup.py install + +You can test whether this worked by opening up a python shell and making sure "import torchvision" runs successfully. \ No newline at end of file diff --git a/mrobosub_perception/_CMakeLists.txt b/mrobosub_perception/_CMakeLists.txt deleted file mode 100644 index 0e3a0428..00000000 --- a/mrobosub_perception/_CMakeLists.txt +++ /dev/null @@ -1,208 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(mrobosub_perception) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS mrobosub_msgs dynamic_reconfigure) -generate_dynamic_reconfigure_options( - cfg/hsv_params.cfg - cfg/pathmarker_params.cfg - cfg/rectify_params.cfg - cfg/dummy_botcam_params.cfg -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html - -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( - # FILES - # ObjectPosition.srv - # ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS mrobosub_msgs dynamic_reconfigure - DEPENDS python-numpy python-cv2 python-cv_bridge -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include -# ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/mrobosub_perception.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/mrobosub_perception_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_mrobosub_perception.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/mrobosub_perception/launch/ml_executor_launch.xml b/mrobosub_perception/launch/ml_executor_launch.xml index f0ff4038..7dcdd1c8 100644 --- a/mrobosub_perception/launch/ml_executor_launch.xml +++ b/mrobosub_perception/launch/ml_executor_launch.xml @@ -1,4 +1,5 @@ - - - + + + + \ No newline at end of file diff --git a/mrobosub_perception/mrobosub_perception/ml_executor.py b/mrobosub_perception/mrobosub_perception/ml_executor.py index 3b518fe6..b06b43b8 100755 --- a/mrobosub_perception/mrobosub_perception/ml_executor.py +++ b/mrobosub_perception/mrobosub_perception/ml_executor.py @@ -15,10 +15,10 @@ def load_yolo(): # load model - path = os.path.abspath(os.path.join(os.path.dirname(__file__), "../..")) - yolo_path = os.path.join(path, "yolov5") + path = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../../")) + yolo_path = os.path.join(path, "src/mrobosub_perception/yolov5") - model_path = os.path.join(path, "models/mar2026_best.pt") + model_path = os.path.expanduser('~/ros2_ws/src/mrobosub_perception/models/mar2026_best.pt') print(yolo_path) print(model_path) model = torch.hub.load( @@ -27,6 +27,15 @@ def load_yolo(): model.conf = 0.1 # NMS confidence threshold return model +def make_detection(d) -> Detection: + det = Detection() + det.left = float(d[0]) + det.top = float(d[1]) + det.right = float(d[2]) + det.bottom = float(d[3]) + det.confidence = float(d[4]) + det.classification = int(d[5]) + return det class MlExecutor(Node): def __init__(self): @@ -57,9 +66,9 @@ def zed_callback(self, image: Image): print(f"TIME: {time.time() - start}") message = Detections( - detections=(Detection(*d) for d in detections), - width=width, - height=height, + detections=[make_detection(d) for d in detections], + width=float(width), + height=float(height), ) self.detection_pub.publish(message) diff --git a/mrobosub_perception/mrobosub_perception/ml_srv.py b/mrobosub_perception/mrobosub_perception/ml_srv.py index d5060594..5ddbe2c2 100755 --- a/mrobosub_perception/mrobosub_perception/ml_srv.py +++ b/mrobosub_perception/mrobosub_perception/ml_srv.py @@ -40,7 +40,7 @@ def __init__(self): name = target.name.lower() setattr(self, f"{name}_srv", self.create_service(ObjectPosition, f"object_position/{name}", - lambda _, response: self.handle_obj_request(idx, response), # the _ is the request which is unused + lambda _, response, i=idx: self.handle_obj_request(i, response), )) # eg: will define self.gate_red_srv = a service "object_position/gate_red" whose handler is handle_obj_request(0, resp) # where 0 is the index of GATE_RED in Targets @@ -96,11 +96,11 @@ def detections_callback(self, detections: Detections): d_y = y_pos - (height / 2) object_pos.found = True - object_pos.x_position = x_pos - object_pos.y_position = y_pos - object_pos.x_theta = (d_x * fov_x) / width - object_pos.y_theta = (d_y * fov_y) / height - object_pos.confidence = d.confidence + object_pos.x_position = float(x_pos) + object_pos.y_position = float(y_pos) + object_pos.x_theta = float((d_x * fov_x)) / width + object_pos.y_theta = float((d_y * fov_y)) / height + object_pos.confidence = float(d.confidence) idx: int = d.classification diff --git a/mrobosub_perception/setup.py b/mrobosub_perception/setup.py index 6d681421..73853481 100644 --- a/mrobosub_perception/setup.py +++ b/mrobosub_perception/setup.py @@ -1,8 +1,20 @@ from setuptools import find_packages, setup import glob +import os package_name = 'mrobosub_perception' +def get_data_files(src_dir, dest_prefix): + result = [] + for dirpath, dirnames, filenames in os.walk(src_dir): + dirnames[:] = [d for d in dirnames if d != '__pycache__'] + if not filenames: + continue + files = [os.path.join(dirpath, f) for f in filenames if not f.endswith('.pyc')] + dest = os.path.join(dest_prefix, dirpath) + result.append((dest, files)) + return result + setup( name=package_name, version='0.0.0', @@ -13,6 +25,7 @@ ('share/' + package_name, ['package.xml']), (f"share/{package_name}/launch", glob.glob("launch/*")), (f"share/{package_name}/params", glob.glob("params/*")), + *get_data_files('yolov5', 'share/' + package_name), ], install_requires=['setuptools'], zip_safe=True,