add fused odometry

src/subsystems/navigation/README.md

@@ -1,40 +1,16 @@
-# athena_nav
+# Navigation
 
-This repository contains all packages for the navigation subteam for UMD Loop's 2024-25 rover, Athena.
+## How To Launch Navigation Simulation
 
------
+After building the relevant packages: 
 
-## Guidelines
-* Fork this repository for development, you can open a pull request to this repo to add your changes for everyone.
-* Do NOT force push.
-* Follow best practices (descriptive commit messages, well documented code, etc.)
 
-## Versions
-This code is written for an Ubuntu 22.04 system with ROS2 Humble Hawksbill and Gazebo Harmonic.
-
-## Building
-
-Note that to build this repository, you must have a [workspace](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html). You can clone this repository into the src/ folder.
-
-### Installing Dependencies
-
-Inside of your workspace, run:
 ```bash
-rosdep install --from-paths src -y --ignore-src
+ros2 launch simulation bringup.launch.py
 ```
-This will search through the `package.xml` files and install the necessary packages. Read more about rosdep and how to properly configure your packages [here](https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html).
-
-### Building
+With the following options:
+* `rviz:={true,false}`: Launch rviz visualization
+* `publish_ground_truth_tf:={true,false}`: Use the Gazebo odometry for the ground truth odom to base transform
+* `world:={empty.sdf, terrain_world.sdf}: Use specific SDF file
 
-Inside of your workspace, run:
-```bash
-colcon build --packages-select  <name-of-pkg>
-```
-
-You can also do ``colcon build`` to build all packages.
-
-After building, source your install with:
-```bash
-source install/setup.bash
-```
 

src/subsystems/navigation/localizer/config/localizer.yaml

@@ -1,4 +1,4 @@
-state_estimator:
+localizer_node:
   ros__parameters:
     imu_topic: "/imu"
     gnss_topic: "/gps/fix"

src/subsystems/navigation/localizer/include/localizer/state_estimator.h

@@ -250,7 +250,12 @@ class StateEstimator : public rclcpp::Node {
     // Cached sensor transforms
     std::optional<gtsam::Pose3> imu_to_base_;
     std::optional<gtsam::Point3> gnss_to_base_;
-    bool imu_extrinsic_set_;  // Flag to track if IMU extrinsic is initialized
+    bool imu_extrinsic_set_;
+
+    gtsam::NavState odom_state_;
+    gtsam::imuBias::ConstantBias odom_bias_;
+    std::unique_ptr<gtsam::PreintegratedCombinedMeasurements> odom_preintegrator_;
+    rclcpp::Time last_odom_propagation_time_;
 
     // EMA filter state (for accel x, y, z and gyro x, y, z)
     std::optional<Eigen::Vector3d> filtered_accel_;

src/subsystems/navigation/localizer/src/state_estimator.cpp

@@ -61,6 +61,8 @@ namespace localizer
     StateEstimator::StateEstimator()
         : Node("state_estimator"), current_key_index_(0), oldest_key_index_(0),
           enu_origin_set_(false), origin_lat_(0.0), origin_lon_(0.0), origin_alt_(0.0),
+          odom_state_(gtsam::Pose3::Identity(), gtsam::Vector3::Zero()),
+          last_odom_propagation_time_(0, 0, RCL_ROS_TIME),
           filter_initialized_(false)
     {
         init_state_.store(InitState::WAITING_FOR_IMU);
@@ -361,7 +363,11 @@ namespace localizer
         imu_to_base_.reset();
         gnss_to_base_.reset();
 
-        // Reset EMA filter state
+        odom_state_ = gtsam::NavState(gtsam::Pose3::Identity(), gtsam::Vector3::Zero());
+        odom_bias_ = gtsam::imuBias::ConstantBias();
+        odom_preintegrator_.reset();
+        last_odom_propagation_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
+
         filtered_accel_.reset();
         filtered_gyro_.reset();
         filter_initialized_ = false;
@@ -466,6 +472,16 @@ namespace localizer
             std::lock_guard<std::mutex> lock(state_mutex_);
             if (init_state_.load() == InitState::RUNNING)
             {
+                if (odom_preintegrator_ && last_odom_propagation_time_.seconds() > 0)
+                {
+                    double dt = (stamp - last_odom_propagation_time_).seconds();
+                    if (dt > 0.0 && dt < 1.0)
+                    {
+                        odom_preintegrator_->integrateMeasurement(accel, gyro, dt);
+                    }
+                }
+                last_odom_propagation_time_ = stamp;
+
                 double dt = (stamp - last_state_time_).seconds();
                 if (dt >= min_state_dt_)
                 {
@@ -582,6 +598,9 @@ namespace localizer
             pending_odom_.valid = true;
         }
 
+        gtsam::Pose3 new_pose = odom_state_.pose().compose(delta);
+        odom_state_ = gtsam::NavState(new_pose, odom_state_.velocity());
+
         pending_odom_.prev_pose = current_pose;
     }
 
@@ -631,6 +650,12 @@ namespace localizer
         reset_preintegration(initial_bias);
         last_state_time_ = timestamp;
 
+        odom_state_ = gtsam::NavState(gtsam::Pose3::Identity(), gtsam::Vector3::Zero());
+        odom_bias_ = initial_bias;
+        odom_preintegrator_ = std::make_unique<gtsam::PreintegratedCombinedMeasurements>(
+            imu_params_, odom_bias_);
+        last_odom_propagation_time_ = timestamp;
+
         {
             std::lock_guard<std::mutex> lock(imu_mutex_);
             prune_imu_buffer_locked(timestamp);
@@ -798,6 +823,12 @@ namespace localizer
             reset_preintegration(entry.bias);
             last_state_time_ = timestamp;
 
+            odom_bias_ = entry.bias;
+            if (odom_preintegrator_)
+            {
+                odom_preintegrator_->resetIntegrationAndSetBias(odom_bias_);
+            }
+
             {
                 std::lock_guard<std::mutex> lock(imu_mutex_);
                 prune_imu_buffer_locked(timestamp);
@@ -1015,7 +1046,7 @@ namespace localizer
             return;
         }
 
-        odom_pub_->publish(state->to_odometry(cached_frames_.map, cached_frames_.base));
+        odom_pub_->publish(state->to_odometry(cached_frames_.odom, cached_frames_.base));
     }
 
     void StateEstimator::publish_transforms()
@@ -1031,56 +1062,55 @@ namespace localizer
             return;
         }
 
-        geometry_msgs::msg::TransformStamped odom_to_base;
-        try
-        {
-            odom_to_base = tf_buffer_->lookupTransform(
-                cached_frames_.odom, cached_frames_.base, tf2::TimePointZero);
-        }
-        catch (const tf2::TransformException &ex)
+        std::lock_guard<std::mutex> lock(state_mutex_);
+
+        gtsam::NavState current_odom_state = odom_state_;
+        if (odom_preintegrator_ && odom_preintegrator_->deltaTij() > 1e-6)
         {
-            RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 1000,
-                                  "Cannot lookup odom->base: %s", ex.what());
-            return;
+            current_odom_state = odom_preintegrator_->predict(odom_state_, odom_bias_);
         }
 
-        gtsam::Pose3 T_odom_base(
-            gtsam::Rot3::Quaternion(
-                odom_to_base.transform.rotation.w,
-                odom_to_base.transform.rotation.x,
-                odom_to_base.transform.rotation.y,
-                odom_to_base.transform.rotation.z),
-            gtsam::Point3(
-                odom_to_base.transform.translation.x,
-                odom_to_base.transform.translation.y,
-                odom_to_base.transform.translation.z));
-
         gtsam::Pose3 T_map_base = state->nav_state.pose();
-        gtsam::Pose3 T_map_odom_raw = T_odom_base.between(T_map_base);
+        gtsam::Pose3 T_odom_base = current_odom_state.pose();
+        gtsam::Pose3 T_map_odom = T_odom_base.between(T_map_base);
 
-        // Apply 180° rotation around Z-axis to correct coordinate frame orientation
-        gtsam::Rot3 R_z_180 = gtsam::Rot3::Rz(M_PI);
+        /*gtsam::Rot3 R_z_180 = gtsam::Rot3::Rz(M_PI);
         gtsam::Point3 rotated_translation = R_z_180.rotate(T_map_odom_raw.translation());
         gtsam::Rot3 rotated_orientation = R_z_180.compose(T_map_odom_raw.rotation());
-        gtsam::Pose3 T_map_odom(rotated_orientation, rotated_translation);
-
-        // Publish map->odom transform
-        geometry_msgs::msg::TransformStamped tf_msg;
-        tf_msg.header.stamp = state->timestamp;
-        tf_msg.header.frame_id = cached_frames_.map;
-        tf_msg.child_frame_id = cached_frames_.odom;
-
-        tf_msg.transform.translation.x = T_map_odom.x();
-        tf_msg.transform.translation.y = T_map_odom.y();
-        tf_msg.transform.translation.z = T_map_odom.z();
-
-        auto q = T_map_odom.rotation().toQuaternion();
-        tf_msg.transform.rotation.w = q.w();
-        tf_msg.transform.rotation.x = q.x();
-        tf_msg.transform.rotation.y = q.y();
-        tf_msg.transform.rotation.z = q.z();
-
-        tf_broadcaster_->sendTransform(tf_msg);
+        gtsam::Pose3 T_map_odom(rotated_orientation, rotated_translation);*/
+
+        geometry_msgs::msg::TransformStamped map_to_odom_msg;
+        map_to_odom_msg.header.stamp = state->timestamp;
+        map_to_odom_msg.header.frame_id = cached_frames_.map;
+        map_to_odom_msg.child_frame_id = cached_frames_.odom;
+
+        map_to_odom_msg.transform.translation.x = T_map_odom.x();
+        map_to_odom_msg.transform.translation.y = T_map_odom.y();
+        map_to_odom_msg.transform.translation.z = T_map_odom.z();
+
+        auto q_map_odom = T_map_odom.rotation().toQuaternion();
+        map_to_odom_msg.transform.rotation.w = q_map_odom.w();
+        map_to_odom_msg.transform.rotation.x = q_map_odom.x();
+        map_to_odom_msg.transform.rotation.y = q_map_odom.y();
+        map_to_odom_msg.transform.rotation.z = q_map_odom.z();
+
+        geometry_msgs::msg::TransformStamped odom_to_base_msg;
+        odom_to_base_msg.header.stamp = state->timestamp;
+        odom_to_base_msg.header.frame_id = cached_frames_.odom;
+        odom_to_base_msg.child_frame_id = cached_frames_.base;
+
+        odom_to_base_msg.transform.translation.x = T_odom_base.x();
+        odom_to_base_msg.transform.translation.y = T_odom_base.y();
+        odom_to_base_msg.transform.translation.z = T_odom_base.z();
+
+        auto q_odom_base = T_odom_base.rotation().toQuaternion();
+        odom_to_base_msg.transform.rotation.w = q_odom_base.w();
+        odom_to_base_msg.transform.rotation.x = q_odom_base.x();
+        odom_to_base_msg.transform.rotation.y = q_odom_base.y();
+        odom_to_base_msg.transform.rotation.z = q_odom_base.z();
+
+        tf_broadcaster_->sendTransform(map_to_odom_msg);
+        tf_broadcaster_->sendTransform(odom_to_base_msg);
     }
 
     std::optional<EstimatedState> StateEstimator::get_latest_state() const