From e25129aebd99c994bd1e707cbddd53668e9a692b Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Wed, 28 Jan 2026 13:48:54 -0500 Subject: [PATCH 1/3] add fused odometry --- .../include/localizer/state_estimator.h | 7 +- .../localizer/src/state_estimator.cpp | 114 +++++++++++------- 2 files changed, 78 insertions(+), 43 deletions(-) diff --git a/src/subsystems/navigation/localizer/include/localizer/state_estimator.h b/src/subsystems/navigation/localizer/include/localizer/state_estimator.h index cf7239d..980872d 100644 --- a/src/subsystems/navigation/localizer/include/localizer/state_estimator.h +++ b/src/subsystems/navigation/localizer/include/localizer/state_estimator.h @@ -250,7 +250,12 @@ class StateEstimator : public rclcpp::Node { // Cached sensor transforms std::optional imu_to_base_; std::optional 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 odom_preintegrator_; + rclcpp::Time last_odom_propagation_time_; // EMA filter state (for accel x, y, z and gyro x, y, z) std::optional filtered_accel_; diff --git a/src/subsystems/navigation/localizer/src/state_estimator.cpp b/src/subsystems/navigation/localizer/src/state_estimator.cpp index 4e825f4..f3e6f9f 100644 --- a/src/subsystems/navigation/localizer/src/state_estimator.cpp +++ b/src/subsystems/navigation/localizer/src/state_estimator.cpp @@ -61,7 +61,9 @@ 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), - filter_initialized_(false) + filter_initialized_(false), + odom_state_(gtsam::Pose3::Identity(), gtsam::Vector3::Zero()), + last_odom_propagation_time_(0, 0, RCL_ROS_TIME) { 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 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( + imu_params_, odom_bias_); + last_odom_propagation_time_ = timestamp; + { std::lock_guard 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 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() @@ -1025,62 +1056,61 @@ namespace localizer return; } + std::lock_guard lock(state_mutex_); + auto state = get_latest_state(); if (!state) { return; } - geometry_msgs::msg::TransformStamped odom_to_base; - try + gtsam::NavState current_odom_state = odom_state_; + if (odom_preintegrator_ && odom_preintegrator_->deltaTij() > 1e-6) { - odom_to_base = tf_buffer_->lookupTransform( - cached_frames_.odom, cached_frames_.base, tf2::TimePointZero); + current_odom_state = odom_preintegrator_->predict(odom_state_, odom_bias_); } - catch (const tf2::TransformException &ex) - { - RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 1000, - "Cannot lookup odom->base: %s", ex.what()); - return; - } - - 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_odom_base = current_odom_state.pose(); gtsam::Pose3 T_map_odom_raw = 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::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); + 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 StateEstimator::get_latest_state() const From 980870bed2e2f56414ff553c24e50c71453522a7 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sun, 1 Feb 2026 12:28:26 -0500 Subject: [PATCH 2/3] when did this break... --- src/subsystems/navigation/README.md | 40 ++++--------------- .../localizer/config/localizer.yaml | 2 +- .../localizer/src/state_estimator.cpp | 4 +- 3 files changed, 11 insertions(+), 35 deletions(-) diff --git a/src/subsystems/navigation/README.md b/src/subsystems/navigation/README.md index d425dd9..25680cb 100644 --- a/src/subsystems/navigation/README.md +++ b/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 -``` - -You can also do ``colcon build`` to build all packages. - -After building, source your install with: -```bash -source install/setup.bash -``` diff --git a/src/subsystems/navigation/localizer/config/localizer.yaml b/src/subsystems/navigation/localizer/config/localizer.yaml index 4c5ed65..5399aaa 100644 --- a/src/subsystems/navigation/localizer/config/localizer.yaml +++ b/src/subsystems/navigation/localizer/config/localizer.yaml @@ -1,4 +1,4 @@ -state_estimator: +localizer_node: ros__parameters: imu_topic: "/imu" gnss_topic: "/gps/fix" diff --git a/src/subsystems/navigation/localizer/src/state_estimator.cpp b/src/subsystems/navigation/localizer/src/state_estimator.cpp index f3e6f9f..e1d43af 100644 --- a/src/subsystems/navigation/localizer/src/state_estimator.cpp +++ b/src/subsystems/navigation/localizer/src/state_estimator.cpp @@ -61,9 +61,9 @@ 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), - filter_initialized_(false), odom_state_(gtsam::Pose3::Identity(), gtsam::Vector3::Zero()), - last_odom_propagation_time_(0, 0, RCL_ROS_TIME) + last_odom_propagation_time_(0, 0, RCL_ROS_TIME), + filter_initialized_(false) { init_state_.store(InitState::WAITING_FOR_IMU); From adca8317a2951bf128faf3869dcde593573612f5 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sun, 1 Feb 2026 15:01:40 -0500 Subject: [PATCH 3/3] move mutex --- .../navigation/localizer/src/state_estimator.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/subsystems/navigation/localizer/src/state_estimator.cpp b/src/subsystems/navigation/localizer/src/state_estimator.cpp index e1d43af..5b52e66 100644 --- a/src/subsystems/navigation/localizer/src/state_estimator.cpp +++ b/src/subsystems/navigation/localizer/src/state_estimator.cpp @@ -1056,14 +1056,14 @@ namespace localizer return; } - std::lock_guard lock(state_mutex_); - auto state = get_latest_state(); if (!state) { return; } + std::lock_guard lock(state_mutex_); + gtsam::NavState current_odom_state = odom_state_; if (odom_preintegrator_ && odom_preintegrator_->deltaTij() > 1e-6) { @@ -1072,12 +1072,12 @@ namespace localizer gtsam::Pose3 T_map_base = state->nav_state.pose(); gtsam::Pose3 T_odom_base = current_odom_state.pose(); - gtsam::Pose3 T_map_odom_raw = T_odom_base.between(T_map_base); + gtsam::Pose3 T_map_odom = T_odom_base.between(T_map_base); - 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); + 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;