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/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..5b52e66 100644 --- a/src/subsystems/navigation/localizer/src/state_estimator.cpp +++ b/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 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() @@ -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 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 StateEstimator::get_latest_state() const