From ebf43b21d33e2aa2a706ac8106042a730e76bc79 Mon Sep 17 00:00:00 2001
From: cameron brown <52760912+cbrxyz@users.noreply.github.com>
Date: Sat, 15 Jun 2024 18:30:21 -0400
Subject: [PATCH] Dynamic pressure, move thrusters from Fall 2023 (#1205)

---
 .../nav_tube_driver/src/nav_tube_driver.cpp   | 26 ++++++++++++++++++-
 .../config/thruster_layout.yaml               |  8 +++---
 2 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp b/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp
index 5451370f3..a40080677 100644
--- a/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp
+++ b/SubjuGator/drivers/nav_tube_driver/src/nav_tube_driver.cpp
@@ -1,6 +1,8 @@
 #include <bits/stdint-uintn.h>
 #include <endian.h>
 #include <mil_msgs/DepthStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/callback_queue.h>
 #include <ros/ros.h>
 
 #include <boost/asio.hpp>
@@ -22,6 +24,8 @@ class NavTubeDriver
   ros::NodeHandle private_nh_;
 
   ros::Publisher pub_;
+  ros::Subscriber odom_sub_;
+  nav_msgs::Odometry recent_odom_msg_;
 
   std::string ip_;
   int port_;
@@ -59,11 +63,13 @@ class NavTubeDriver
   ~NavTubeDriver();
 
   void run();
+  void odom_callback(const nav_msgs::OdometryConstPtr& msg);
 };
 
 NavTubeDriver::NavTubeDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) : nh_(nh), private_nh_(private_nh)
 {
   pub_ = nh.advertise<mil_msgs::DepthStamped>("depth", 10);
+  odom_sub_ = nh.subscribe<nav_msgs::Odometry>("odom", 10, &NavTubeDriver::odom_callback, this);
   ip_ = private_nh.param<std::string>("ip", std::string("192.168.37.61"));
   port_ = private_nh.param<int>("port", 33056);
   frame_id_ = private_nh.param<std::string>("frame_id", "/depth");
@@ -85,6 +91,11 @@ NavTubeDriver::NavTubeDriver(ros::NodeHandle nh, ros::NodeHandle private_nh) : n
   reinterpret_cast<uint16_t*>(&heartbeat_packet[2])[0] = nw_ordering;
 }
 
+void NavTubeDriver::odom_callback(const nav_msgs::OdometryConstPtr& ptr)
+{
+  recent_odom_msg_ = *ptr;
+}
+
 NavTubeDriver::~NavTubeDriver()
 {
   {
@@ -194,7 +205,18 @@ void NavTubeDriver::read_messages(boost::shared_ptr<tcp::socket> socket)
 
         uint64_t bits = be64toh(*reinterpret_cast<uint64_t*>(&backing[2]));
         double pressure = *reinterpret_cast<double*>(&bits);
-        msg.depth = pressure;
+        if (recent_odom_msg_.header.seq)
+        {
+          // Accounts for the dynamic pressure applied to the pressure sensor
+          // when the sub is moving forwards or backwards
+          double velocity = recent_odom_msg_.twist.twist.linear.x;
+          double vel_effect = (abs(velocity) * velocity) / (1000 * 9.81);
+          msg.depth = pressure + vel_effect;
+        }
+        else
+        {
+          msg.depth = pressure;
+        }
 
         pub_.publish(msg);
         buffer = boost::asio::buffer(backing, sizeof(backing));
@@ -205,6 +227,8 @@ void NavTubeDriver::read_messages(boost::shared_ptr<tcp::socket> socket)
 
     buffer = boost::asio::buffer(buffer + bytes_read);
     prev = ros::Time::now();
+
+    ros::spinOnce();
   }
 }
 
diff --git a/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml b/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml
index 48992559a..b18e4ac0d 100644
--- a/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml
+++ b/SubjuGator/gnc/subjugator_thruster_mapper/config/thruster_layout.yaml
@@ -32,7 +32,7 @@ thrusters:
   FLH: {
     node_id: 4,
     position: [0.2678, 0.2795, 0.0],
-    direction: [0.866, -0.5, 0.0],
+    direction: [-0.866, 0.5, 0.0],
     thrust_bounds: [-81.85, 99.7],
     calib: {
       # yamllint disable-line rule:line-length
@@ -56,7 +56,7 @@ thrusters:
   FRH: {
     node_id: 0,
     position: [0.2678, -0.2795, 0.0],
-    direction: [0.866, 0.5, 0.0],
+    direction: [-0.866, -0.5, 0.0],
     thrust_bounds: [-81.85, 99.7],
     calib: {
       # yamllint disable-line rule:line-length
@@ -80,7 +80,7 @@ thrusters:
   BLH: {
     node_id: 3,
     position: [-0.2678, 0.2795, 0.0],
-    direction: [0.866, 0.5, 0.0],
+    direction: [-0.866, -0.5, 0.0],
     thrust_bounds: [-81.85, 99.7],
     calib: {
       # yamllint disable-line rule:line-length
@@ -104,7 +104,7 @@ thrusters:
   BRH: {
     node_id: 7,
     position: [-0.2678, -0.2795, 0.0],
-    direction: [0.866, -0.5, 0.0],
+    direction: [-0.866, 0.5, 0.0],
     thrust_bounds: [-81.85, 99.7],
     calib: {
       # yamllint disable-line rule:line-length