some updates from the spine mapper

jhughes50 · jhughes50 · commit e0e3c58d6cd8 · 2025-09-10T23:01:22.000-04:00
diff --git a/glider/config/example-params.yaml b/glider/config/example-params.yaml
@@ -3,8 +3,6 @@ gyroscope_covariance: 0.0001
 integration_covariance: 0.001
 bias_covariance: 0.001
 use_second_order: 0.0
-origin_x: 482990.7375634047
-origin_y: 4421250.443879729
 gravity: 9.81
 bias_num_measurements: 100
 gps_noise: 2.0
diff --git a/glider/config/ros-params.yaml b/glider/config/ros-params.yaml
@@ -1,7 +1,10 @@
 glider_node:
   ros__parameters:
     rate: 100.0
-    use_odom: true
+    use_odom: false
+    viz: true
     publish_nav_sat_fix: false
     utm_zone: "18S"
     declination: 11.82
+    origin_easting: 482933.2819920078
+    origin_northing: 4421345.135559781
diff --git a/glider/config/vectornav-vn100t.yaml b/glider/config/vectornav-vn100t.yaml
@@ -0,0 +1,17 @@
+accelerometer_covariance: 0.00001
+gyroscope_covariance: 0.00001
+integration_covariance: 0.001
+bias_covariance: 0.001
+use_second_order: 0.0
+gravity: 9.81
+bias_num_measurements: 100
+gps_noise: 1.5
+odom_noise: 0.5
+heading_noise: 0.1
+odom_orientation_noise: 0.1
+odom_translation_noise: 1.0
+odom_scale_noise: 50.0
+lag_time: 10.0
+imu_frame: "enu"
+scale_odom: true
+correct_imu: true
diff --git a/glider/include/glider/utils/parameters.hpp b/glider/include/glider/utils/parameters.hpp
@@ -22,8 +22,6 @@ struct Parameters
     double integration_cov;
     double bias_cov;
     double use_second_order;
-    double origin_x;
-    double origin_y;
     double gravity;
     double gps_noise;
     double heading_noise;
diff --git a/glider/include/ros/glider_node.hpp b/glider/include/ros/glider_node.hpp
@@ -58,6 +58,7 @@ class GliderNode : public rclcpp::Node
         
         // publishers 
         rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
+        rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_viz_pub_;
         rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_pub_;
 
         // timers
@@ -67,8 +68,11 @@ class GliderNode : public rclcpp::Node
         bool initialized_;
         bool use_sim_time_;
         bool publish_nsf_;
+        bool viz_;
         std::string utm_zone_;
         double declination_;
+        double origin_easting_;
+        double origin_northing_;
 
         // tracker
         Glider::State current_state_;
diff --git a/glider/launch/glider-node.launch.py b/glider/launch/glider-node.launch.py
@@ -31,13 +31,13 @@ def generate_launch_description():
     ros_params_file = PathJoinSubstitution([
         glider_share,
         'config',
-        'ros_params.yaml'
+        'ros-params.yaml'
     ])
     
     graph_params_file = PathJoinSubstitution([
         glider_share,
         'config',
-        'graph_params.yaml'
+        'vectornav-vn100t.yaml'
     ])
      
     # Create the glider node
@@ -53,9 +53,9 @@ def generate_launch_description():
              'use_odom': use_odom}
         ],
         remappings=[
-            ('/gps', '/ublox/fix'),
-            ('/imu', '/VN100T/imu'),
-            ('/mag', '/VN100T/mag'),
+            ('/gps', '/ublox_gps_node/fix'),
+            ('/imu', '/vectornav/imu'),
+            ('/mag', '/vectornav/magnetic'),
             ('/odom', '/Odometry'),
         ]
     )
diff --git a/glider/ros/conversions.cpp b/glider/ros/conversions.cpp
@@ -224,7 +224,7 @@ Output Conversions::odomToRos(Glider::Odometry& odom, const char* zone)
         msg.twist.twist.linear.x = v(0);
         msg.twist.twist.linear.y = v(1);
         msg.twist.twist.linear.z = v(2);
-        msg.header = getHeader(odom.getTimestamp(), "enu-utm");
+        msg.header = getHeader(odom.getTimestamp(), "enu");
 
         return msg;
     }
@@ -303,7 +303,7 @@ Output Conversions::stateToRos(Glider::State& state, const char* zone)
                 msg.twist.covariance[i * cov.rows() + j] = cov(i, j);
             }
         }
-        msg.header = getHeader(state.getTimestamp(), "enu-utm");
+        msg.header = getHeader(state.getTimestamp(), "enu");
 
     }
     else
diff --git a/glider/ros/glider_node.cpp b/glider/ros/glider_node.cpp
@@ -16,6 +16,9 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
     declare_parameter("publish_nav_sat_fix", false);
     declare_parameter("utm_zone", "18S");
     declare_parameter("declination", 12.0);
+    declare_parameter("origin_easting", 0.0);
+    declare_parameter("origin_northing", 0.0);
+    declare_parameter("viz", false);
 
     // Get parameters
     double freq = this->get_parameter("rate").as_double();
@@ -29,10 +32,18 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
 
     bool use_odom = this->get_parameter("use_odom").as_bool();
     RCLCPP_INFO_STREAM(this->get_logger(), "Fusing Odometry: " << std::boolalpha << use_odom);
+    
     publish_nsf_ = this->get_parameter("publish_nav_sat_fix").as_bool();
     utm_zone_ = this->get_parameter("utm_zone").as_string();
-    declination_ = this->get_parameter("declination").as_double() * M_PI / 180.0d;
+    RCLCPP_INFO_STREAM(this->get_logger(), "Using UTM Zone: " << utm_zone_); 
+
+    declination_ = this->get_parameter("declination").as_double() * M_PI / 180.0;
     RCLCPP_INFO_STREAM(this->get_logger(), "Using Magnetic Declination: " << declination_);
+    
+    viz_ = this->get_parameter("viz").as_bool();
+
+    origin_easting_ = this->get_parameter("origin_easting").as_double();
+    origin_northing_ = this->get_parameter("origin_northing").as_double();
 
     glider_ = std::make_unique<Glider::Glider>(path);
 
@@ -80,6 +91,12 @@ GliderNode::GliderNode(const rclcpp::NodeOptions& options) : rclcpp::Node("glide
         odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/glider/odom", 10);
     }
 
+    if(viz_)
+    {
+        RCLCPP_INFO(this->get_logger(), "Publishing Odometry Viz message on /glider/odom/viz");
+        odom_viz_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("/glider/odom/viz", 10);
+    }
+
     std::chrono::milliseconds d = GliderROS::Conversions::hzToDuration(freq);
     timer_ = this->create_wall_timer(d, std::bind(&GliderNode::interpolationCallback, this));
     RCLCPP_INFO(this->get_logger(), "GliderNode Initialized");
@@ -110,6 +127,17 @@ void GliderNode::interpolationCallback()
         nav_msgs::msg::Odometry msg = GliderROS::Conversions::odomToRos<nav_msgs::msg::Odometry>(odom);
         GliderROS::Conversions::addCovariance<nav_msgs::msg::Odometry>(current_state_, msg);
         odom_pub_->publish(msg);
+
+        if (viz_)
+        {
+            nav_msgs::msg::Odometry viz_msg = msg;
+            double x = viz_msg.pose.pose.position.x - origin_easting_;
+            double y = viz_msg.pose.pose.position.y - origin_northing_;
+            viz_msg.pose.pose.position.x = x;
+            viz_msg.pose.pose.position.y = y;
+
+            odom_viz_pub_->publish(viz_msg);
+        }
     }
 }
 
diff --git a/glider/scripts/heading_mag.py b/glider/scripts/heading_mag.py
@@ -13,7 +13,7 @@ def __init__(self):
         # Create subscriber for magnetometer data
         self.subscription = self.create_subscription(
             MagneticField,
-            '/VN100T/mag',  # Change this to your magnetometer topic name
+            '/vectornav/magnetic',  # Change this to your magnetometer topic name
             self.magnetometer_callback,
             10
         )
@@ -46,13 +46,9 @@ def magnetometer_callback(self, msg):
         magnitude = math.sqrt(mag_x**2 + mag_y**2 + mag_z**2)
         
         # Print the results
-        self.get_logger().info(
-            f'Magnetic Field - X: {mag_x:.3f}, Y: {mag_y:.3f}, Z: {mag_z:.3f} µT'
-        )
         self.get_logger().info(
             f'Heading: {heading_deg:.2f}° | Magnitude: {magnitude:.3f} µT'
         )
-        self.get_logger().info('---')
 
 
 def main(args=None):
diff --git a/glider/src/glider.cpp b/glider/src/glider.cpp
@@ -16,8 +16,6 @@ Glider::Glider(const std::string& path)
     Parameters params = Parameters::Load(path);
     factor_manager_ = FactorManager(params);
 
-    origin_x_ = params.origin_x;
-    origin_y_ = params.origin_y;
     frame_ = params.frame;
     correct_imu_ = params.correct_imu;
 
@@ -74,9 +72,6 @@ void Glider::addGPS(int64_t timestamp, Eigen::Vector3d& gps)
     char zone[4];
     geodetics::LLtoUTM(gps(0), gps(1), northing, easting, zone);
     
-    easting = easting - origin_x_;
-    northing = northing - origin_y_;
-    
     meas.head(2) << easting, northing;
     meas(2) = gps(2);
 
@@ -88,40 +83,41 @@ void Glider::addIMU(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d&
     if (frame_ == "ned")
     {
         // transfrom to enu
-        Eigen::Quaterniond quat_ned(quat(0), quat(1), quat(2), quat(3));
-        Eigen::Quaterniond quat_enu = ned_to_enu_quat_ * quat_ned;
-        Eigen::Vector4d vec_enu;
-        vec_enu << quat_enu.w(), quat_enu.x(), quat_enu.y(), quat_enu.z();
+        Eigen::Quaterniond eig_quat(quat(0), quat(1), quat(2), quat(3));
+        Eigen::Matrix3d imu_rot = eig_quat.toRotationMatrix();
+        // Vectornav gives BODY wrt NED, we need NED wrt BODY --> so take the inverse 
+        Eigen::Matrix3d imu_enu = ned_to_enu_rot_ * imu_rot.inverse();
+        Eigen::Quaterniond imu_quat(imu_enu);
+        Eigen::Vector4d imu_vec(imu_quat.w(), imu_quat.x(), imu_quat.y(), imu_quat.z());
 
         Eigen::Vector3d accel_enu = ned_to_enu_rot_ * accel;
         Eigen::Vector3d gyro_enu = ned_to_enu_rot_ * gyro;
-
+        
         if (correct_imu_)
         {
-            Eigen::Vector4d quat_corr = correctImuOrientation(vec_enu);
+            Eigen::Vector4d quat_corr = correctImuOrientation(imu_vec);
             factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, quat_corr);
         }
         else
         {
-            factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, vec_enu);
+            factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, imu_vec);
         }
-
     }
     else if (frame_ == "enu")
     {
         if (correct_imu_)
         {
             Eigen::Vector4d quat_corr = correctImuOrientation(quat);
             factor_manager_.addImuFactor(timestamp, accel, gyro, quat_corr);
-        }
+        }       
         else
         {
             factor_manager_.addImuFactor(timestamp, accel, gyro, quat);
         }
     }
     else
     {
-        throw std::runtime_error("IMU Frame, not supported use ned or enu");
+        throw std::runtime_error("IMU Frame, not supported use ENU or NED");
         return;
     }
 }  
@@ -151,9 +147,9 @@ void Glider::addOdom(int64_t timestamp, Eigen::Isometry3d& pose)
         // Convert orb pose to ENU
         Eigen::Matrix3d odom_to_enu;
         double h = initial_heading_;
-        odom_to_enu << std::cos(h), -std::sin(h), 0.0d, 
-                      std::sin(h), std::cos(h), 0.0d, 
-                      0.0d, 0.0d, 1.0d;
+        odom_to_enu << std::cos(h), -std::sin(h), 0.0, 
+                      std::sin(h), std::cos(h), 0.0, 
+                      0.0, 0.0, 1.0;
         Eigen::Isometry3d enu_pose = pose.rotate(odom_to_enu);
         
         // calculate the relative pose in ENU frame
diff --git a/glider/src/parameters.cpp b/glider/src/parameters.cpp
@@ -19,8 +19,6 @@ Glider::Parameters::Parameters(const std::string& path)
         integration_cov = config["integration_covariance"].as<double>();
         bias_cov = config["bias_covariance"].as<double>();
         use_second_order = config["use_second_order"].as<double>();
-        origin_x = config["origin_x"].as<double>();
-        origin_y = config["origin_y"].as<double>();
         gravity = config["gravity"].as<double>();
         gps_noise = config["gps_noise"].as<double>();
         heading_noise = config["heading_noise"].as<double>();

Original file line number	Diff line number	Diff line change
`@@ -224,7 +224,7 @@ Output Conversions::odomToRos(Glider::Odometry& odom, const char* zone)`
`224`	`224`	`msg.twist.twist.linear.x = v(0);`
`225`	`225`	`msg.twist.twist.linear.y = v(1);`
`226`	`226`	`msg.twist.twist.linear.z = v(2);`
`227`		`- msg.header = getHeader(odom.getTimestamp(), "enu-utm");`
	`227`	`+ msg.header = getHeader(odom.getTimestamp(), "enu");`
`228`	`228`
`229`	`229`	`return msg;`
`230`	`230`	`}`
`@@ -303,7 +303,7 @@ Output Conversions::stateToRos(Glider::State& state, const char* zone)`
`303`	`303`	`msg.twist.covariance[i * cov.rows() + j] = cov(i, j);`
`304`	`304`	`}`
`305`	`305`	`}`
`306`		`- msg.header = getHeader(state.getTimestamp(), "enu-utm");`
	`306`	`+ msg.header = getHeader(state.getTimestamp(), "enu");`
`307`	`307`
`308`	`308`	`}`
`309`	`309`	`else`
Original file line number	Diff line number	Diff line change
`@@ -13,7 +13,7 @@ def __init__(self):`
`13`	`13`	`# Create subscriber for magnetometer data`
`14`	`14`	`self.subscription = self.create_subscription(`
`15`	`15`	`MagneticField,`
`16`		`- '/VN100T/mag', # Change this to your magnetometer topic name`
	`16`	`+ '/vectornav/magnetic', # Change this to your magnetometer topic name`
`17`	`17`	`self.magnetometer_callback,`
`18`	`18`	`10`
`19`	`19`	`)`
`@@ -46,13 +46,9 @@ def magnetometer_callback(self, msg):`
`46`	`46`	`magnitude = math.sqrt(mag_x2 + mag_y2 + mag_z**2)`
`47`	`47`
`48`	`48`	`# Print the results`
`49`		`- self.get_logger().info(`
`50`		`- f'Magnetic Field - X: {mag_x:.3f}, Y: {mag_y:.3f}, Z: {mag_z:.3f} µT'`
`51`		`- )`
`52`	`49`	`self.get_logger().info(`
`53`	`50`	`f'Heading: {heading_deg:.2f}° \| Magnitude: {magnitude:.3f} µT'`
`54`	`51`	`)`
`55`		`- self.get_logger().info('---')`
`56`	`52`
`57`	`53`
`58`	`54`	`def main(args=None):`