From 3092635fa813857208f9f69fe9fc460825dd366c Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 23 Dec 2025 17:17:29 -0500 Subject: [PATCH 1/9] added dependencies script and requirements.txt --- .github/workflows/main.yml | 24 +++++++++++++++++------- dependencies.sh | 11 +++++++++++ requirements.txt | 5 +++++ 3 files changed, 33 insertions(+), 7 deletions(-) create mode 100755 dependencies.sh create mode 100644 requirements.txt diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 270a6ff..ddbfcdd 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -2,27 +2,37 @@ name: ROS2 Build on: push: - branches: [ main ] + branches: [main] pull_request: - branches: [ main ] + branches: [main] workflow_dispatch: + jobs: build: runs-on: ubuntu-22.04 container: image: ros:humble-ros-base - + steps: - name: Checkout repository - uses: actions/checkout@v4 - - name: Install dependencies + uses: actions/checkout@v4 + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.10' + + - name: Install Python dependencies + run: python3 -m pip install -r requirements.txt + + - name: Install system dependencies run: | apt-get update - apt-get install -y python3-colcon-common-extensions + bash dependencies.sh rosdep update rosdep install --from-paths . --ignore-src -r -y shell: bash - + - name: Build workspace run: | source /opt/ros/humble/setup.bash diff --git a/dependencies.sh b/dependencies.sh new file mode 100755 index 0000000..a341778 --- /dev/null +++ b/dependencies.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +set -euo pipefail + +pkgs=( + python3-colcon-common-extensions +) + +for pkg in "${pkgs[@]}"; do + apt-get install -y "$pkg" +done \ No newline at end of file diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..2d920ce --- /dev/null +++ b/requirements.txt @@ -0,0 +1,5 @@ +pygame +catkin_pkg +empy==3.3.4 +lark +numpy \ No newline at end of file From fdf6d2b1c30a7e9be8c790bebace6a5bee481d67 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Sun, 7 Dec 2025 15:35:15 -0500 Subject: [PATCH 2/9] add localizer package back in and add base-link in properly just rename chassis base link no need for new link --- .gitignore | 2 + src/description/rviz/athena_drive.rviz | 8 +- src/description/rviz/new_athena_drive.rviz | 2 +- src/description/urdf/athena_drive.urdf.xacro | 9 + .../urdf/athena_drive_description.urdf.xacro | 6 +- .../athena_localizer/CMakeLists.txt | 92 +++ .../athena_localizer/state_estimator.h | 147 +++++ .../navigation/athena_localizer/package.xml | 37 ++ .../athena_localizer/src/state_estimator.cpp | 571 ++++++++++++++++++ .../athena_localizer_test/CMakeLists.txt | 61 ++ .../athena_localizer_test/data/metadata.yaml | 134 ++++ .../athena_localizer_test/package.xml | 23 + .../src/athena_localizer_test_node.cpp | 104 ++++ .../src/localizer_evaluation_node.cpp | 145 +++++ .../src/run_evaluation.sh | 17 + 15 files changed, 1350 insertions(+), 8 deletions(-) create mode 100644 src/subsystems/navigation/athena_localizer/CMakeLists.txt create mode 100644 src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h create mode 100644 src/subsystems/navigation/athena_localizer/package.xml create mode 100644 src/subsystems/navigation/athena_localizer/src/state_estimator.cpp create mode 100644 src/subsystems/navigation/athena_localizer_test/CMakeLists.txt create mode 100644 src/subsystems/navigation/athena_localizer_test/data/metadata.yaml create mode 100644 src/subsystems/navigation/athena_localizer_test/package.xml create mode 100644 src/subsystems/navigation/athena_localizer_test/src/athena_localizer_test_node.cpp create mode 100644 src/subsystems/navigation/athena_localizer_test/src/localizer_evaluation_node.cpp create mode 100755 src/subsystems/navigation/athena_localizer_test/src/run_evaluation.sh diff --git a/.gitignore b/.gitignore index c788cb9..f68042f 100644 --- a/.gitignore +++ b/.gitignore @@ -207,3 +207,5 @@ cython_debug/ *.out *.app +.DS_Store +*.mcap \ No newline at end of file diff --git a/src/description/rviz/athena_drive.rviz b/src/description/rviz/athena_drive.rviz index 376f691..257bb00 100644 --- a/src/description/rviz/athena_drive.rviz +++ b/src/description/rviz/athena_drive.rviz @@ -62,7 +62,7 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - chassis_base_link: + base_link: Alpha: 1 Show Axes: false Show Trail: false @@ -170,7 +170,7 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - chassis_base_link: + base_link: Value: true propulsion_1_link: Value: true @@ -214,7 +214,7 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - chassis_base_link: + base_link: suspension_front_left_link: swerve_mount_1_link: steer_1_link: @@ -242,7 +242,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: chassis_base_link + Fixed Frame: base_link Frame Rate: 30 Name: root Tools: diff --git a/src/description/rviz/new_athena_drive.rviz b/src/description/rviz/new_athena_drive.rviz index b0197ef..d8e5739 100644 --- a/src/description/rviz/new_athena_drive.rviz +++ b/src/description/rviz/new_athena_drive.rviz @@ -86,7 +86,7 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - chassis_base_link: + base_link: Alpha: 1 Show Axes: false Show Trail: false diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index dbce67b..e361ecd 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -37,6 +37,15 @@ + + + + + + + + + - + @@ -52,7 +52,7 @@ - + @@ -325,7 +325,7 @@ - + diff --git a/src/subsystems/navigation/athena_localizer/CMakeLists.txt b/src/subsystems/navigation/athena_localizer/CMakeLists.txt new file mode 100644 index 0000000..7631dc1 --- /dev/null +++ b/src/subsystems/navigation/athena_localizer/CMakeLists.txt @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 3.8) +project(athena_localizer) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(GTSAM REQUIRED) +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") +find_package(GeographicLib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) + +# Create library for state estimator +add_library(${PROJECT_NAME} SHARED + src/state_estimator.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${GTSAM_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} + gtsam + ${GeographicLib_LIBRARIES}) +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +# Add ROS2 dependencies +ament_target_dependencies(${PROJECT_NAME} + rclcpp + sensor_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + Eigen3) + +# Install library and headers +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include) + +install(DIRECTORY include/ + DESTINATION include/) + +# Export library for other packages +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + sensor_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + Eigen3 + GTSAM) + +# Generate and install CMake config files +install(EXPORT ${PROJECT_NAME}Targets + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION lib/cmake/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + +endif() + +ament_package() diff --git a/src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h b/src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h new file mode 100644 index 0000000..7df7956 --- /dev/null +++ b/src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h @@ -0,0 +1,147 @@ +/** + * @file state_estimator.h + * @brief Simplified state estimation for IMU and GNSS fusion using ROS2 messages + * @author Modified from MOLA StateEstimationSmoother for athena_localizer + * @date 2025 + */ +#pragma once + +// ROS2 +#include +#include +#include +#include +#include +#include +#include +#include + +// GTSAM +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// std +#include +#include +#include +#include + +namespace athena_localizer +{ + +struct FrameParams +{ + std::string tf_prefix = ""; + std::string map_frame = "map"; + std::string odom_frame = "odom"; + std::string base_frame = "base_link"; + std::string imu_frame = "imu_link"; + std::string gnss_frame = "gnss_link"; +}; + +struct StateEstimatorParams +{ + // Noise parameters + double imu_accel_noise = 0.1; // m/s^2 + double imu_gyro_noise = 0.005; // rad/s + double imu_bias_noise = 0.0001; // bias random walk + double gnss_noise = 1.0; // meters + double odom_noise = 0.1; // meters + + // Window parameters + double max_time_window = 10.0; // seconds + size_t max_states = 100; + + // Frame names + FrameParams frames; +}; + +struct EstimatedState +{ + rclcpp::Time timestamp; + gtsam::NavState nav_state; + gtsam::imuBias::ConstantBias imu_bias; + Eigen::Matrix covariance; + + nav_msgs::msg::Odometry to_odometry(const std::string& frame_id, const std::string& child_frame_id) const; + geometry_msgs::msg::PoseWithCovarianceStamped to_pose(const std::string& frame_id) const; +}; + +class StateEstimator +{ +public: + StateEstimator(); + ~StateEstimator(); + + void initialize(const StateEstimatorParams& params, rclcpp::Node::SharedPtr node); + void reset(); + + void fuse_imu(const sensor_msgs::msg::Imu::SharedPtr& imu_msg); + void fuse_gnss(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg); + void fuse_odometry(const nav_msgs::msg::Odometry::SharedPtr& odom_msg); + + std::optional get_latest_state() const; + std::optional get_state_at_time(const rclcpp::Time& timestamp) const; + + void publish_transforms(); + + bool is_initialized() const { return initialized_; } + +private: + StateEstimatorParams params_; + rclcpp::Node::SharedPtr node_; + std::unique_ptr tf_broadcaster_; + + // GTSAM components + std::unique_ptr isam_; + std::unique_ptr new_factors_; + std::unique_ptr new_values_; + std::unique_ptr imu_preintegrated_; + + // State management + std::deque state_history_; + std::deque imu_buffer_; + + // State tracking + gtsam::Pose3 odom_to_base_; + gtsam::Pose3 map_to_odom_; + gtsam::Key current_pose_key_, current_vel_key_, current_bias_key_; + size_t key_index_; + bool initialized_; + rclcpp::Time last_optimization_time_; + std::optional prev_odom_pose_; + + mutable std::mutex state_mutex_; + + void optimize_graph(); + void add_imu_factor(); + void add_gnss_factor(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg); + void add_odom_factor(const nav_msgs::msg::Odometry::SharedPtr& odom_msg); + void createNewState(); + void marginalize_old_states(); + void initialize_with_gnss(const gtsam::Point3& gnss_pos, const rclcpp::Time& timestamp); + + gtsam::Point3 lla_to_enu(double lat, double lon, double alt) const; + void set_enu_origin(double lat, double lon, double alt); + bool enu_origin_set_; + double origin_lat_, origin_lon_, origin_alt_; + + gtsam::SharedNoiseModel imu_noise_model_; + gtsam::SharedNoiseModel gnss_noise_model_; + gtsam::SharedNoiseModel odom_noise_model_; + + // Key generation + gtsam::Key pose_key(size_t i) const { return gtsam::Symbol('x', i); } + gtsam::Key vel_key(size_t i) const { return gtsam::Symbol('v', i); } + gtsam::Key bias_key(size_t i) const { return gtsam::Symbol('b', i); } +}; + + +} // namespace athena_localizer \ No newline at end of file diff --git a/src/subsystems/navigation/athena_localizer/package.xml b/src/subsystems/navigation/athena_localizer/package.xml new file mode 100644 index 0000000..736012c --- /dev/null +++ b/src/subsystems/navigation/athena_localizer/package.xml @@ -0,0 +1,37 @@ + + + + athena_localizer + 0.0.0 + TODO: Package description + ros + TODO: License declaration + + gtsam + libgeographic-dev + rclcpp + sensor_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + eigen3_cmake_module + libboost-serialization-dev + libboost-system-dev + libboost-filesystem-dev + libboost-thread-dev + libboost-program-options-dev + libboost-date-time-dev + libboost-timer-dev + libboost-chrono-dev + libboost-regex-dev + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/subsystems/navigation/athena_localizer/src/state_estimator.cpp b/src/subsystems/navigation/athena_localizer/src/state_estimator.cpp new file mode 100644 index 0000000..6d52cbf --- /dev/null +++ b/src/subsystems/navigation/athena_localizer/src/state_estimator.cpp @@ -0,0 +1,571 @@ +#include "athena_localizer/state_estimator.h" +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +namespace athena_localizer +{ + +nav_msgs::msg::Odometry EstimatedState::to_odometry(const std::string& frame_id, const std::string& child_frame_id) const +{ + nav_msgs::msg::Odometry odom; + odom.header.stamp = timestamp; + odom.header.frame_id = frame_id; + odom.child_frame_id = child_frame_id; + + auto pose = nav_state.pose(); + auto vel = nav_state.velocity(); + + odom.pose.pose.position.x = pose.x(); + odom.pose.pose.position.y = pose.y(); + odom.pose.pose.position.z = pose.z(); + + auto quat = pose.rotation().toQuaternion(); + odom.pose.pose.orientation.w = quat.w(); + odom.pose.pose.orientation.x = quat.x(); + odom.pose.pose.orientation.y = quat.y(); + odom.pose.pose.orientation.z = quat.z(); + + odom.twist.twist.linear.x = vel.x(); + odom.twist.twist.linear.y = vel.y(); + odom.twist.twist.linear.z = vel.z(); + + for (int i = 0; i < 36; ++i) { + odom.pose.covariance[i] = 0.0; + odom.twist.covariance[i] = 0.0; + } + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 6; ++j) { + if (i < 3 && j < 3) { + odom.pose.covariance[i*6 + j] = covariance(i, j); + } + } + } + + return odom; +} + +geometry_msgs::msg::PoseWithCovarianceStamped EstimatedState::to_pose(const std::string& frame_id) const +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; + pose_msg.header.stamp = timestamp; + pose_msg.header.frame_id = frame_id; + + auto pose = nav_state.pose(); + pose_msg.pose.pose.position.x = pose.x(); + pose_msg.pose.pose.position.y = pose.y(); + pose_msg.pose.pose.position.z = pose.z(); + + auto quat = pose.rotation().toQuaternion(); + pose_msg.pose.pose.orientation.w = quat.w(); + pose_msg.pose.pose.orientation.x = quat.x(); + pose_msg.pose.pose.orientation.y = quat.y(); + pose_msg.pose.pose.orientation.z = quat.z(); + + for (int i = 0; i < 36; ++i) { + pose_msg.pose.covariance[i] = 0.0; + } + for (int i = 0; i < 6; ++i) { + for (int j = 0; j < 6; ++j) { + if (i < 3 && j < 3) { + pose_msg.pose.covariance[i*6 + j] = covariance(i, j); + } + } + } + + return pose_msg; +} + +// StateEstimator constructor +StateEstimator::StateEstimator() + : odom_to_base_(Pose3()) + , map_to_odom_(Pose3()) + , key_index_(0) + , initialized_(false) + , enu_origin_set_(false) + , origin_lat_(0.0) + , origin_lon_(0.0) + , origin_alt_(0.0) +{ + ISAM2Params isam_params; + isam_params.factorization = ISAM2Params::CHOLESKY; + isam_params.relinearizeSkip = 10; + + isam_ = std::make_unique(isam_params); + new_factors_ = std::make_unique(); + new_values_ = std::make_unique(); +} + +// StateEstimator destructor +StateEstimator::~StateEstimator() = default; + +void StateEstimator::initialize(const StateEstimatorParams& params, rclcpp::Node::SharedPtr node) +{ + params_ = params; + node_ = node; + + tf_broadcaster_ = std::make_unique(node_); + + // Create noise models + Matrix33 imu_cov = Matrix33::Identity() * params_.imu_accel_noise * params_.imu_accel_noise; + Matrix33 gyro_cov = Matrix33::Identity() * params_.imu_gyro_noise * params_.imu_gyro_noise; + Matrix33 bias_cov = Matrix33::Identity() * params_.imu_bias_noise * params_.imu_bias_noise; + + auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(9.81); + imu_params->accelerometerCovariance = imu_cov; + imu_params->gyroscopeCovariance = gyro_cov; + imu_params->integrationCovariance = bias_cov; + + imu_preintegrated_ = std::make_unique(imu_params, imuBias::ConstantBias()); + + gnss_noise_model_ = noiseModel::Diagonal::Sigmas((Vector3() << params_.gnss_noise, params_.gnss_noise, params_.gnss_noise).finished()); + odom_noise_model_ = noiseModel::Diagonal::Sigmas((Vector6() << params_.odom_noise, params_.odom_noise, params_.odom_noise, params_.odom_noise, params_.odom_noise, params_.odom_noise).finished()); + + RCLCPP_INFO(node_->get_logger(), "StateEstimator initialized"); +} + +void StateEstimator::reset() +{ + std::lock_guard lock(state_mutex_); + + ISAM2Params isam_params; + isam_params.factorization = ISAM2Params::CHOLESKY; + isam_params.relinearizeSkip = 10; + + isam_ = std::make_unique(isam_params); + new_factors_->resize(0); + new_values_->clear(); + state_history_.clear(); + imu_buffer_.clear(); + + odom_to_base_ = Pose3(); + map_to_odom_ = Pose3(); + key_index_ = 0; + initialized_ = false; + enu_origin_set_ = false; + prev_odom_pose_ = std::nullopt; + + if (imu_preintegrated_) { + imu_preintegrated_->resetIntegration(); + } + + RCLCPP_INFO(node_->get_logger(), "StateEstimator reset"); +} + +void StateEstimator::fuse_imu(const sensor_msgs::msg::Imu::SharedPtr& imu_msg) +{ + std::lock_guard lock(state_mutex_); + + if (!initialized_) { + imu_buffer_.push_back(*imu_msg); + return; + } + + double dt = 0.01; + if (!imu_buffer_.empty()) { + auto prev_time = rclcpp::Time(imu_buffer_.back().header.stamp); + auto curr_time = rclcpp::Time(imu_msg->header.stamp); + double computed_dt = (curr_time - prev_time).seconds(); + + dt = std::max(0.001, std::min(0.1, computed_dt)); + + if (computed_dt <= 0.0) { + RCLCPP_WARN(node_->get_logger(), "Non-positive dt detected: %.6f, using default", computed_dt); + dt = 0.01; + } + } + + Vector3 accel(imu_msg->linear_acceleration.x, + imu_msg->linear_acceleration.y, + imu_msg->linear_acceleration.z); + Vector3 gyro(imu_msg->angular_velocity.x, + imu_msg->angular_velocity.y, + imu_msg->angular_velocity.z); + + if (imu_preintegrated_) { + imu_preintegrated_->integrateMeasurement(accel, gyro, dt); + } + + imu_buffer_.push_back(*imu_msg); + + if (imu_buffer_.size() > 1000) { + imu_buffer_.pop_front(); + } +} + +void StateEstimator::fuse_gnss(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg) +{ + std::lock_guard lock(state_mutex_); + + + // within the Kitti dataset, the GPS status is -2, but this + /*if (gnss_msg->status.status < 0) { + RCLCPP_INFO( + node_->get_logger(), "GNSS Status is bad: status=%d", + gnss_msg->status.status); + return; + }*/ + + if (!enu_origin_set_) { + set_enu_origin(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude); + } + + auto gnss_pos = lla_to_enu(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude); + + if (!initialized_) { + initialize_with_gnss(gnss_pos, rclcpp::Time(gnss_msg->header.stamp)); + return; + } + + add_gnss_factor(gnss_msg); + optimize_graph(); + marginalize_old_states(); +} + +void StateEstimator::fuse_odometry(const nav_msgs::msg::Odometry::SharedPtr& odom_msg) +{ + std::lock_guard lock(state_mutex_); + + if (!initialized_) { + return; + } + + add_odom_factor(odom_msg); + optimize_graph(); + marginalize_old_states(); +} + +void StateEstimator::initialize_with_gnss(const Point3& gnss_pos, const rclcpp::Time& timestamp) +{ + // Use first IMU measurement for initial orientation if available + Rot3 initial_rotation = Rot3(); + if (!imu_buffer_.empty()) { + const auto& first_imu = imu_buffer_.front(); + tf2::Quaternion q( + first_imu.orientation.x, + first_imu.orientation.y, + first_imu.orientation.z, + first_imu.orientation.w); + + // Convert to GTSAM rotation if orientation is valid (not all zeros) + if (q.length2() > 0.01) { + q.normalize(); + initial_rotation = Rot3::Quaternion(q.w(), q.x(), q.y(), q.z()); + RCLCPP_INFO(node_->get_logger(), "Initializing with IMU orientation"); + } else { + RCLCPP_WARN(node_->get_logger(), "IMU orientation invalid, using identity rotation"); + } + } else { + RCLCPP_WARN(node_->get_logger(), "No IMU data available, using identity rotation"); + } + + auto initial_pose = Pose3(initial_rotation, gnss_pos); + auto initial_velocity = Vector3(0.0, 0.0, 0.0); + auto initial_bias = imuBias::ConstantBias(); + + odom_to_base_ = initial_pose; + map_to_odom_ = Pose3(); + + current_pose_key_ = pose_key(key_index_); + current_vel_key_ = vel_key(key_index_); + current_bias_key_ = bias_key(key_index_); + + new_values_->insert(current_pose_key_, initial_pose); + new_values_->insert(current_vel_key_, initial_velocity); + new_values_->insert(current_bias_key_, initial_bias); + + // Tighter rotation prior if we have IMU orientation + double rot_prior_noise = (!imu_buffer_.empty() && initial_rotation.matrix() != Rot3().matrix()) ? 0.02 : 0.1; + + auto pose_prior = noiseModel::Diagonal::Sigmas( + (Vector6() << rot_prior_noise, rot_prior_noise, rot_prior_noise, 0.1, 0.1, 0.1).finished()); + auto vel_prior = noiseModel::Diagonal::Sigmas( + (Vector3() << 0.5, 0.5, 0.5).finished()); // Tighter velocity prior + auto bias_prior = noiseModel::Diagonal::Sigmas( + (Vector6() << 0.05, 0.05, 0.05, 0.005, 0.005, 0.005).finished()); // Tighter bias prior + + new_factors_->addPrior(current_pose_key_, initial_pose, pose_prior); + new_factors_->addPrior(current_vel_key_, initial_velocity, vel_prior); + new_factors_->addPrior(current_bias_key_, initial_bias, bias_prior); + + optimize_graph(); + + EstimatedState state; + state.timestamp = timestamp; + state.nav_state = NavState(initial_pose, initial_velocity); + state.imu_bias = initial_bias; + state.covariance = Eigen::Matrix::Identity() * 0.1; + + state_history_.push_back(state); + + initialized_ = true; + last_optimization_time_ = timestamp; + + RCLCPP_INFO(node_->get_logger(), "StateEstimator initialized with GNSS at (%.2f, %.2f, %.2f)", + gnss_pos.x(), gnss_pos.y(), gnss_pos.z()); +} + +std::optional StateEstimator::get_latest_state() const +{ + std::lock_guard lock(state_mutex_); + + if (state_history_.empty()) { + return std::nullopt; + } + + return state_history_.back(); +} + +std::optional StateEstimator::get_state_at_time(const rclcpp::Time& timestamp) const +{ + std::lock_guard lock(state_mutex_); + + if (state_history_.empty()) { + return std::nullopt; + } + + auto it = std::lower_bound(state_history_.begin(), state_history_.end(), timestamp, + [](const EstimatedState& state, const rclcpp::Time& time) { + return state.timestamp < time; + }); + + if (it != state_history_.end()) { + return *it; + } + + return state_history_.back(); +} + +void StateEstimator::publish_transforms() +{ + auto latest_state = get_latest_state(); + if (!latest_state) { + return; + } + + // Publish odom → base_link + geometry_msgs::msg::TransformStamped odom_to_base_transform; + odom_to_base_transform.header.stamp = latest_state->timestamp; + odom_to_base_transform.header.frame_id = params_.frames.tf_prefix.empty() ? + params_.frames.odom_frame : params_.frames.tf_prefix + "/" + params_.frames.odom_frame; + odom_to_base_transform.child_frame_id = params_.frames.tf_prefix.empty() ? + params_.frames.base_frame : params_.frames.tf_prefix + "/" + params_.frames.base_frame; + + odom_to_base_transform.transform.translation.x = odom_to_base_.x(); + odom_to_base_transform.transform.translation.y = odom_to_base_.y(); + odom_to_base_transform.transform.translation.z = odom_to_base_.z(); + + auto odom_quat = odom_to_base_.rotation().toQuaternion(); + odom_to_base_transform.transform.rotation.w = odom_quat.w(); + odom_to_base_transform.transform.rotation.x = odom_quat.x(); + odom_to_base_transform.transform.rotation.y = odom_quat.y(); + odom_to_base_transform.transform.rotation.z = odom_quat.z(); + + tf_broadcaster_->sendTransform(odom_to_base_transform); + + // Publish map → odom + geometry_msgs::msg::TransformStamped map_to_odom_transform; + map_to_odom_transform.header.stamp = latest_state->timestamp; + map_to_odom_transform.header.frame_id = params_.frames.tf_prefix.empty() ? + params_.frames.map_frame : params_.frames.tf_prefix + "/" + params_.frames.map_frame; + map_to_odom_transform.child_frame_id = params_.frames.tf_prefix.empty() ? + params_.frames.odom_frame : params_.frames.tf_prefix + "/" + params_.frames.odom_frame; + + map_to_odom_transform.transform.translation.x = map_to_odom_.x(); + map_to_odom_transform.transform.translation.y = map_to_odom_.y(); + map_to_odom_transform.transform.translation.z = map_to_odom_.z(); + + auto map_quat = map_to_odom_.rotation().toQuaternion(); + map_to_odom_transform.transform.rotation.w = map_quat.w(); + map_to_odom_transform.transform.rotation.x = map_quat.x(); + map_to_odom_transform.transform.rotation.y = map_quat.y(); + map_to_odom_transform.transform.rotation.z = map_quat.z(); + + tf_broadcaster_->sendTransform(map_to_odom_transform); +} + +void StateEstimator::optimize_graph() +{ + if (new_factors_->empty()) { + return; + } + + try { + isam_->update(*new_factors_, *new_values_); + + new_factors_->resize(0); + new_values_->clear(); + + auto result = isam_->calculateEstimate(); + + if (!result.empty() && result.exists(current_pose_key_) && result.exists(current_vel_key_)) { + EstimatedState state; + state.timestamp = node_->get_clock()->now(); + + auto map_to_base = result.at(current_pose_key_); + auto vel = result.at(current_vel_key_); + state.nav_state = NavState(map_to_base, vel); + + if (result.exists(current_bias_key_)) { + state.imu_bias = result.at(current_bias_key_); + } + + // Update odom_to_base_ to the optimized pose + odom_to_base_ = map_to_base; + + // Update map_to_odom_ transform + // Relationship: map_to_base = map_to_odom * odom_to_base + // Therefore: map_to_odom = map_to_base * odom_to_base.inverse() + map_to_odom_ = map_to_base * odom_to_base_.inverse(); + + // Get covariance for pose only + try { + state.covariance = Eigen::Matrix::Identity() * 0.1; + auto pose_cov = isam_->marginalCovariance(current_pose_key_); + if (pose_cov.rows() == 6 && pose_cov.cols() == 6) { + state.covariance.block<6, 6>(0, 0) = pose_cov; + } + } catch (const std::exception& e) { + RCLCPP_WARN(node_->get_logger(), "Failed to get covariance: %s", e.what()); + state.covariance = Eigen::Matrix::Identity() * 0.1; + } + + state_history_.push_back(state); + } + } + catch (const std::exception& e) { + RCLCPP_WARN(node_->get_logger(), "ISAM2 update failed: %s", e.what()); + } +} + +void StateEstimator::add_imu_factor() +{ + if (!imu_preintegrated_ || key_index_ == 0) { + return; + } + + auto prev_pose_key = pose_key(key_index_ - 1); + auto prev_vel_key = vel_key(key_index_ - 1); + auto prev_bias_key = bias_key(key_index_ - 1); + + new_factors_->emplace_shared( + prev_pose_key, prev_vel_key, current_pose_key_, + current_vel_key_, prev_bias_key, *imu_preintegrated_); + + auto bias_noise = noiseModel::Diagonal::Sigmas( + (Vector6() << params_.imu_bias_noise, params_.imu_bias_noise, params_.imu_bias_noise, + params_.imu_bias_noise, params_.imu_bias_noise, params_.imu_bias_noise).finished()); + + new_factors_->emplace_shared>( + prev_bias_key, current_bias_key_, imuBias::ConstantBias(), bias_noise); +} + +void StateEstimator::add_odom_factor(const nav_msgs::msg::Odometry::SharedPtr& odom_msg) +{ + if (!prev_odom_pose_) { + prev_odom_pose_ = Pose3(Rot3(odom_msg->pose.pose.orientation.w, odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, odom_msg->pose.pose.orientation.z), + Point3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z)); + return; + } + + auto current_odom_pose = Pose3(Rot3(odom_msg->pose.pose.orientation.w, odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, odom_msg->pose.pose.orientation.z), + Point3(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z)); + + auto odom_delta = prev_odom_pose_->between(current_odom_pose); + + createNewState(); + + new_factors_->emplace_shared>(pose_key(key_index_ - 1), current_pose_key_, odom_delta, odom_noise_model_); + + prev_odom_pose_ = current_odom_pose; +} + +void StateEstimator::add_gnss_factor(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg) +{ + auto gnss_pos = lla_to_enu(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude); + + createNewState(); + + double base_noise = params_.gnss_noise; + double adaptive_noise = base_noise; + + if (gnss_msg->position_covariance_type != sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN) { + double cov_noise = std::sqrt(gnss_msg->position_covariance[0]); + adaptive_noise = std::max(base_noise, cov_noise); + } + + // Note: KITTI dataset uses status=-2 but has good GPS quality + // Only apply noise penalty for status values indicating actual poor quality + // Do not penalize negative status codes from KITTI + // Removed: if (gnss_msg->status.status < 0) adaptive_noise *= 10.0; + if (gnss_msg->status.status == 0) adaptive_noise *= 1.5; // Reduced from 2.0 + + auto adaptive_gnss_noise = gtsam::noiseModel::Diagonal::Sigmas( + (gtsam::Vector3() << adaptive_noise, adaptive_noise, adaptive_noise).finished()); + + new_factors_->emplace_shared(current_pose_key_, gnss_pos, adaptive_gnss_noise); +} + +void StateEstimator::createNewState() +{ + key_index_++; + current_pose_key_ = pose_key(key_index_); + current_vel_key_ = vel_key(key_index_); + current_bias_key_ = bias_key(key_index_); + + if (key_index_ > 0 && imu_preintegrated_) { + auto prev_state = state_history_.back(); + auto predicted = imu_preintegrated_->predict(prev_state.nav_state, prev_state.imu_bias); + + new_values_->insert(current_pose_key_, predicted.pose()); + new_values_->insert(current_vel_key_, predicted.velocity()); + new_values_->insert(current_bias_key_, prev_state.imu_bias); + + add_imu_factor(); + imu_preintegrated_->resetIntegration(); + } +} + +void StateEstimator::marginalize_old_states() +{ + if (state_history_.size() <= params_.max_states) { + return; + } + + auto time_threshold = node_->get_clock()->now() - rclcpp::Duration::from_seconds(params_.max_time_window); + + while (!state_history_.empty() && state_history_.front().timestamp < time_threshold) { + state_history_.pop_front(); + } +} + +gtsam::Point3 StateEstimator::lla_to_enu(double lat, double lon, double alt) const +{ + if (!enu_origin_set_) { + return Point3(); + } + + GeographicLib::LocalCartesian proj(origin_lat_, origin_lon_, origin_alt_); + double x, y, z; + proj.Forward(lat, lon, alt, x, y, z); + + return Point3(x, y, z); +} + +void StateEstimator::set_enu_origin(double lat, double lon, double alt) +{ + origin_lat_ = lat; + origin_lon_ = lon; + origin_alt_ = alt; + enu_origin_set_ = true; + + RCLCPP_INFO(node_->get_logger(), "ENU origin set to (%.6f, %.6f, %.2f)", lat, lon, alt); +} + +} // namespace athena_localizer diff --git a/src/subsystems/navigation/athena_localizer_test/CMakeLists.txt b/src/subsystems/navigation/athena_localizer_test/CMakeLists.txt new file mode 100644 index 0000000..7a3227c --- /dev/null +++ b/src/subsystems/navigation/athena_localizer_test/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(athena_localizer_test) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(athena_localizer REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +add_executable(athena_localizer_test_node src/athena_localizer_test_node.cpp) +target_include_directories(athena_localizer_test_node PUBLIC + $ + $) +target_compile_features(athena_localizer_test_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +ament_target_dependencies(athena_localizer_test_node + athena_localizer + rclcpp + sensor_msgs + geometry_msgs + nav_msgs) + +install(TARGETS athena_localizer_test_node + DESTINATION lib/${PROJECT_NAME}) + +add_executable(localizer_evaluation_node src/localizer_evaluation_node.cpp) +target_include_directories(localizer_evaluation_node PUBLIC + $ + $) +target_compile_features(localizer_evaluation_node PUBLIC c_std_99 cxx_std_17) + +ament_target_dependencies(localizer_evaluation_node + rclcpp + geometry_msgs + tf2 + tf2_ros) + +install(TARGETS localizer_evaluation_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/subsystems/navigation/athena_localizer_test/data/metadata.yaml b/src/subsystems/navigation/athena_localizer_test/data/metadata.yaml new file mode 100644 index 0000000..f2a21aa --- /dev/null +++ b/src/subsystems/navigation/athena_localizer_test/data/metadata.yaml @@ -0,0 +1,134 @@ +rosbag2_bagfile_information: + version: 9 + storage_identifier: mcap + duration: + nanoseconds: 68172873020 + starting_time: + nanoseconds_since_epoch: 1317051643426995038 + message_count: 9240 + topics_with_message_count: + - topic_metadata: + name: /tf_static + type: tf2_msgs/msg/TFMessage + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/camera_gray_left/camera_info + type: sensor_msgs/msg/CameraInfo + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/camera_gray_right/image_raw + type: sensor_msgs/msg/Image + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/camera_color_left/camera_info + type: sensor_msgs/msg/CameraInfo + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /tf + type: tf2_msgs/msg/TFMessage + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/camera_gray_left/image_raw + type: sensor_msgs/msg/Image + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/oxts/imu + type: sensor_msgs/msg/Imu + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/oxts/gps/fix + type: sensor_msgs/msg/NavSatFix + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/velo/pointcloud + type: sensor_msgs/msg/PointCloud2 + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/camera_gray_right/camera_info + type: sensor_msgs/msg/CameraInfo + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/camera_color_left/image_raw + type: sensor_msgs/msg/Image + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/oxts/gps/vel + type: geometry_msgs/msg/TwistStamped + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/camera_color_right/image_raw + type: sensor_msgs/msg/Image + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + - topic_metadata: + name: /kitti/camera_color_right/camera_info + type: sensor_msgs/msg/CameraInfo + serialization_format: cdr + offered_qos_profiles: + [] + type_description_hash: "" + message_count: 660 + compression_format: "" + compression_mode: "" + relative_file_paths: + - kitti_2011_09_26_drive_0117_synced_0.mcap + files: + - path: kitti_2011_09_26_drive_0117_synced_0.mcap + starting_time: + nanoseconds_since_epoch: 1317051643426995038 + duration: + nanoseconds: 68172873020 + message_count: 9240 + custom_data: ~ + ros_distro: jazzy \ No newline at end of file diff --git a/src/subsystems/navigation/athena_localizer_test/package.xml b/src/subsystems/navigation/athena_localizer_test/package.xml new file mode 100644 index 0000000..05e7c26 --- /dev/null +++ b/src/subsystems/navigation/athena_localizer_test/package.xml @@ -0,0 +1,23 @@ + + + + athena_localizer_test + 0.0.0 + TODO: Package description + ros + TODO: License declaration + + ament_cmake + + athena_localizer + rclcpp + sensor_msgs + geometry_msgs + nav_msgs + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/subsystems/navigation/athena_localizer_test/src/athena_localizer_test_node.cpp b/src/subsystems/navigation/athena_localizer_test/src/athena_localizer_test_node.cpp new file mode 100644 index 0000000..0eb47e7 --- /dev/null +++ b/src/subsystems/navigation/athena_localizer_test/src/athena_localizer_test_node.cpp @@ -0,0 +1,104 @@ +#include +#include +#include +#include + +class AthenaLocalizerTestNode : public rclcpp::Node +{ +public: + AthenaLocalizerTestNode() + : Node("athena_localizer_test_node") + { + RCLCPP_INFO(this->get_logger(), "Starting Athena Localizer Test Node"); + + imu_sub_ = this->create_subscription( + "/kitti/oxts/imu", 10, + std::bind(&AthenaLocalizerTestNode::imu_callback, this, std::placeholders::_1)); + + gnss_sub_ = this->create_subscription( + "/kitti/oxts/gps/fix", 10, + std::bind(&AthenaLocalizerTestNode::gnss_callback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "/odom", 10, + std::bind(&AthenaLocalizerTestNode::odom_callback, this, std::placeholders::_1)); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + std::bind(&AthenaLocalizerTestNode::timer_callback, this)); + + RCLCPP_INFO(this->get_logger(), "Subscribers created"); + } + + void initialize_state_estimator() + { + // Initialize state estimator + athena_localizer::StateEstimatorParams params; + params.imu_accel_noise = 0.1; + params.imu_gyro_noise = 0.01; + params.imu_bias_noise = 0.001; + params.gnss_noise = 1.0; + params.odom_noise = 0.1; + params.max_time_window = 10.0; + params.max_states = 100; + + // Get tf_prefix parameter + params.frames.tf_prefix = this->declare_parameter("tf_prefix", std::string("test_jawn")); + + state_estimator_.initialize(params, shared_from_this()); + RCLCPP_INFO(this->get_logger(), "State estimator initialized"); + } + +private: + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) + { + RCLCPP_DEBUG(this->get_logger(), "Received IMU data"); + state_estimator_.fuse_imu(msg); + } + + void gnss_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) + { + RCLCPP_INFO( + this->get_logger(), "Received GNSS data: lat=%.6f, lon=%.6f", + msg->latitude, msg->longitude); + state_estimator_.fuse_gnss(msg); + } + + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) + { + RCLCPP_DEBUG(this->get_logger(), "Received odom data"); + state_estimator_.fuse_odometry(msg); + } + + void timer_callback() + { auto latest_state = state_estimator_.get_latest_state(); + if (latest_state.has_value()) { + RCLCPP_DEBUG(this->get_logger(), "State estimator has valid state"); + state_estimator_.publish_transforms(); + } else { + RCLCPP_DEBUG(this->get_logger(), "State estimator not yet initialized"); + } + } + + athena_localizer::StateEstimator state_estimator_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr gnss_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr debug_timer_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + node->initialize_state_estimator(); + + RCLCPP_INFO(rclcpp::get_logger("main"), "Spinning Athena Localizer Test Node"); + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/navigation/athena_localizer_test/src/localizer_evaluation_node.cpp b/src/subsystems/navigation/athena_localizer_test/src/localizer_evaluation_node.cpp new file mode 100644 index 0000000..d3ced5a --- /dev/null +++ b/src/subsystems/navigation/athena_localizer_test/src/localizer_evaluation_node.cpp @@ -0,0 +1,145 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class LocalizerEvaluationNode : public rclcpp::Node +{ +public: + LocalizerEvaluationNode() : Node("localizer_evaluation_node"), alignment_set_(false) + { + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + timer_ = this->create_wall_timer(std::chrono::milliseconds(100), + std::bind(&LocalizerEvaluationNode::evaluate, this)); + + output_.open("evaluation.csv"); + output_ << "timestamp,trans_error,rot_error,x,y,z,roll,pitch,yaw\n"; + } + + ~LocalizerEvaluationNode() + { + if (output_.is_open()) output_.close(); + print_stats(); + } + +private: + void evaluate() + { + geometry_msgs::msg::TransformStamped gt, est; + + try { + gt = tf_buffer_->lookupTransform("world", "base_link", tf2::TimePointZero); + est = tf_buffer_->lookupTransform("test_jawn/map", "test_jawn/base_link", tf2::TimePointZero); + } catch (tf2::TransformException &ex) { + return; + } + + if (!alignment_set_) { + offset_x_ = gt.transform.translation.x - est.transform.translation.x; + offset_y_ = gt.transform.translation.y - est.transform.translation.y; + offset_z_ = gt.transform.translation.z - est.transform.translation.z; + + tf2::Quaternion gt_q(gt.transform.rotation.x, gt.transform.rotation.y, + gt.transform.rotation.z, gt.transform.rotation.w); + tf2::Quaternion est_q(est.transform.rotation.x, est.transform.rotation.y, + est.transform.rotation.z, est.transform.rotation.w); + + offset_rot_ = gt_q * est_q.inverse(); + alignment_set_ = true; + } + + tf2::Vector3 est_pos(est.transform.translation.x + offset_x_, + est.transform.translation.y + offset_y_, + est.transform.translation.z + offset_z_); + + tf2::Quaternion est_q(est.transform.rotation.x, est.transform.rotation.y, + est.transform.rotation.z, est.transform.rotation.w); + tf2::Quaternion est_rot = offset_rot_ * est_q; + + double dx = gt.transform.translation.x - est_pos.x(); + double dy = gt.transform.translation.y - est_pos.y(); + double dz = gt.transform.translation.z - est_pos.z(); + double trans_error = std::sqrt(dx*dx + dy*dy + dz*dz); + + tf2::Quaternion gt_q(gt.transform.rotation.x, gt.transform.rotation.y, + gt.transform.rotation.z, gt.transform.rotation.w); + + double gt_r, gt_p, gt_y, est_r, est_p, est_y; + tf2::Matrix3x3(gt_q).getRPY(gt_r, gt_p, gt_y); + tf2::Matrix3x3(est_rot).getRPY(est_r, est_p, est_y); + + double dr = normalize(gt_r - est_r) * 180.0 / M_PI; + double dp = normalize(gt_p - est_p) * 180.0 / M_PI; + double dy_angle = normalize(gt_y - est_y) * 180.0 / M_PI; + + tf2::Quaternion error_q = gt_q * est_rot.inverse(); + double rot_error = 2.0 * std::acos(std::min(1.0, std::abs(error_q.w()))) * 180.0 / M_PI; + + double timestamp = gt.header.stamp.sec + gt.header.stamp.nanosec * 1e-9; + + output_ << timestamp << "," << trans_error << "," << rot_error << "," + << dx << "," << dy << "," << dz << "," + << dr << "," << dp << "," << dy_angle << "\n"; + + trans_errors_.push_back(trans_error); + rot_errors_.push_back(rot_error); + } + + double normalize(double angle) + { + while (angle > M_PI) angle -= 2.0 * M_PI; + while (angle < -M_PI) angle += 2.0 * M_PI; + return angle; + } + + void print_stats() + { + if (trans_errors_.empty()) return; + + double trans_sum = 0, rot_sum = 0, trans_max = 0, rot_max = 0; + double trans_sq = 0, rot_sq = 0; + + for (size_t i = 0; i < trans_errors_.size(); ++i) { + trans_sum += trans_errors_[i]; + rot_sum += rot_errors_[i]; + trans_sq += trans_errors_[i] * trans_errors_[i]; + rot_sq += rot_errors_[i] * rot_errors_[i]; + trans_max = std::max(trans_max, trans_errors_[i]); + rot_max = std::max(rot_max, rot_errors_[i]); + } + + size_t n = trans_errors_.size(); + + RCLCPP_INFO(this->get_logger(), "\n========== EVALUATION STATISTICS =========="); + RCLCPP_INFO(this->get_logger(), "Samples: %zu", n); + RCLCPP_INFO(this->get_logger(), "Translation: Mean %.4f m | RMSE %.4f m | Max %.4f m", + trans_sum/n, std::sqrt(trans_sq/n), trans_max); + RCLCPP_INFO(this->get_logger(), "Rotation: Mean %.4f deg | RMSE %.4f deg | Max %.4f deg", + rot_sum/n, std::sqrt(rot_sq/n), rot_max); + RCLCPP_INFO(this->get_logger(), "==========================================\n"); + } + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + rclcpp::TimerBase::SharedPtr timer_; + std::ofstream output_; + std::vector trans_errors_, rot_errors_; + bool alignment_set_; + double offset_x_, offset_y_, offset_z_; + tf2::Quaternion offset_rot_; +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/subsystems/navigation/athena_localizer_test/src/run_evaluation.sh b/src/subsystems/navigation/athena_localizer_test/src/run_evaluation.sh new file mode 100755 index 0000000..b9f8c33 --- /dev/null +++ b/src/subsystems/navigation/athena_localizer_test/src/run_evaluation.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +BAG_FILE="${1:-/workspaces/ros2-devenv/src/athena_localizer_test/data/kitti_2011_09_26_drive_0117_synced_0.mcap}" + +cleanup() { + pkill -P $$ + exit 0 +} +trap cleanup SIGINT SIGTERM + +ros2 bag play "$BAG_FILE" & +sleep 2 +ros2 run athena_localizer_test athena_localizer_test_node & +sleep 3 +ros2 run athena_localizer_test localizer_evaluation_node + +cleanup From 3202d8befbdbc003e9233f3a65ca8a9aa7b7741a Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 23 Dec 2025 17:37:15 -0500 Subject: [PATCH 3/9] added geographic lib to dependencies.sh --- dependencies.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dependencies.sh b/dependencies.sh index a341778..40b470f 100755 --- a/dependencies.sh +++ b/dependencies.sh @@ -4,6 +4,8 @@ set -euo pipefail pkgs=( python3-colcon-common-extensions + libgeographic-dev + geographiclib-tools ) for pkg in "${pkgs[@]}"; do From 404640810206f28e2df2f6f3d3c41cccbcdbf72c Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 23 Dec 2025 20:19:55 -0500 Subject: [PATCH 4/9] add parameters and proper imu and gps transforms for localizer --- .../athena_localizer/state_estimator.h | 7 ++ .../athena_localizer/src/state_estimator.cpp | 81 +++++++++++++++---- 2 files changed, 74 insertions(+), 14 deletions(-) diff --git a/src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h b/src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h index 7df7956..f0a80a8 100644 --- a/src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h +++ b/src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h @@ -15,6 +15,7 @@ #include #include #include +#include // GTSAM #include @@ -98,6 +99,12 @@ class StateEstimator StateEstimatorParams params_; rclcpp::Node::SharedPtr node_; std::unique_ptr tf_broadcaster_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::optional imu_to_base_; + std::optional gnss_to_base_; + rclcpp::TimerBase::SharedPtr tf_timer_; // GTSAM components std::unique_ptr isam_; diff --git a/src/subsystems/navigation/athena_localizer/src/state_estimator.cpp b/src/subsystems/navigation/athena_localizer/src/state_estimator.cpp index 6d52cbf..1d66edf 100644 --- a/src/subsystems/navigation/athena_localizer/src/state_estimator.cpp +++ b/src/subsystems/navigation/athena_localizer/src/state_estimator.cpp @@ -110,8 +110,32 @@ void StateEstimator::initialize(const StateEstimatorParams& params, rclcpp::Node params_ = params; node_ = node; + params_.imu_accel_noise = node_->declare_parameter("imu_accel_noise", params_.imu_accel_noise); + params_.imu_gyro_noise = node_->declare_parameter("imu_gyro_noise", params_.imu_gyro_noise); + params_.imu_bias_noise = node_->declare_parameter("imu_bias_noise", params_.imu_bias_noise); + params_.gnss_noise = node_->declare_parameter("gnss_noise", params_.gnss_noise); + params_.odom_noise = node_->declare_parameter("odom_noise", params_.odom_noise); + params_.max_time_window = node_->declare_parameter("max_time_window", params_.max_time_window); + params_.max_states = node_->declare_parameter("max_states", static_cast(params_.max_states)); + + params_.frames.tf_prefix = node_->declare_parameter("tf_prefix", params_.frames.tf_prefix); + params_.frames.map_frame = node_->declare_parameter("map_frame", params_.frames.map_frame); + params_.frames.odom_frame = node_->declare_parameter("odom_frame", params_.frames.odom_frame); + params_.frames.base_frame = node_->declare_parameter("base_frame", params_.frames.base_frame); + params_.frames.imu_frame = node_->declare_parameter("imu_frame", params_.frames.imu_frame); + params_.frames.gnss_frame = node_->declare_parameter("gnss_frame", params_.frames.gnss_frame); + + RCLCPP_INFO(node_->get_logger(), "Parameters: imu_accel=%.4f, imu_gyro=%.4f, gnss=%.2f", + params_.imu_accel_noise, params_.imu_gyro_noise, params_.gnss_noise); + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); tf_broadcaster_ = std::make_unique(node_); + tf_timer_ = node_->create_wall_timer( + std::chrono::milliseconds(100), + [this]() { publish_transforms(); }); + // Create noise models Matrix33 imu_cov = Matrix33::Identity() * params_.imu_accel_noise * params_.imu_accel_noise; Matrix33 gyro_cov = Matrix33::Identity() * params_.imu_gyro_noise * params_.imu_gyro_noise; @@ -162,6 +186,22 @@ void StateEstimator::fuse_imu(const sensor_msgs::msg::Imu::SharedPtr& imu_msg) { std::lock_guard lock(state_mutex_); + if (!imu_to_base_) { + try { + auto tf = tf_buffer_->lookupTransform( + params_.frames.base_frame, params_.frames.imu_frame, + tf2::TimePointZero, tf2::durationFromSec(0.1)); + auto& q = tf.transform.rotation; + auto& t = tf.transform.translation; + imu_to_base_ = Pose3( + Rot3::Quaternion(q.w, q.x, q.y, q.z), + Point3(t.x, t.y, t.z)); + RCLCPP_INFO(node_->get_logger(), "Cached IMU->base_link transform"); + } catch (const tf2::TransformException& ex) { + RCLCPP_DEBUG(node_->get_logger(), "IMU transform not available: %s", ex.what()); + } + } + if (!initialized_) { imu_buffer_.push_back(*imu_msg); return; @@ -188,6 +228,12 @@ void StateEstimator::fuse_imu(const sensor_msgs::msg::Imu::SharedPtr& imu_msg) imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); + if (imu_to_base_) { + Rot3 R = imu_to_base_->rotation(); + accel = R.rotate(accel); + gyro = R.rotate(gyro); + } + if (imu_preintegrated_) { imu_preintegrated_->integrateMeasurement(accel, gyro, dt); } @@ -203,20 +249,30 @@ void StateEstimator::fuse_gnss(const sensor_msgs::msg::NavSatFix::SharedPtr& gns { std::lock_guard lock(state_mutex_); - - // within the Kitti dataset, the GPS status is -2, but this - /*if (gnss_msg->status.status < 0) { - RCLCPP_INFO( - node_->get_logger(), "GNSS Status is bad: status=%d", - gnss_msg->status.status); - return; - }*/ + if (!gnss_to_base_) { + try { + auto tf = tf_buffer_->lookupTransform( + params_.frames.base_frame, params_.frames.gnss_frame, + tf2::TimePointZero, tf2::durationFromSec(0.1)); + gnss_to_base_ = Point3( + tf.transform.translation.x, + tf.transform.translation.y, + tf.transform.translation.z); + RCLCPP_INFO(node_->get_logger(), "Cached GNSS->base_link offset: (%.3f, %.3f, %.3f)", + gnss_to_base_->x(), gnss_to_base_->y(), gnss_to_base_->z()); + } catch (const tf2::TransformException& ex) { + RCLCPP_DEBUG(node_->get_logger(), "GNSS transform not available: %s", ex.what()); + } + } if (!enu_origin_set_) { set_enu_origin(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude); } auto gnss_pos = lla_to_enu(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude); + if (gnss_to_base_) { + gnss_pos = gnss_pos + *gnss_to_base_; + } if (!initialized_) { initialize_with_gnss(gnss_pos, rclcpp::Time(gnss_msg->header.stamp)); @@ -416,14 +472,8 @@ void StateEstimator::optimize_graph() state.imu_bias = result.at(current_bias_key_); } - // Update odom_to_base_ to the optimized pose odom_to_base_ = map_to_base; - // Update map_to_odom_ transform - // Relationship: map_to_base = map_to_odom * odom_to_base - // Therefore: map_to_odom = map_to_base * odom_to_base.inverse() - map_to_odom_ = map_to_base * odom_to_base_.inverse(); - // Get covariance for pose only try { state.covariance = Eigen::Matrix::Identity() * 0.1; @@ -489,6 +539,9 @@ void StateEstimator::add_odom_factor(const nav_msgs::msg::Odometry::SharedPtr& o void StateEstimator::add_gnss_factor(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg) { auto gnss_pos = lla_to_enu(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude); + if (gnss_to_base_) { + gnss_pos = gnss_pos + *gnss_to_base_; + } createNewState(); From 01d1bb54232e1f14d9fbef6e08fed1c43b29ab50 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 23 Dec 2025 20:34:13 -0500 Subject: [PATCH 5/9] add ground truth odometry --- src/description/urdf/athena_drive.urdf.xacro | 3 +++ src/description/urdf/ground_truth_odom.xacro | 16 ++++++++++++++++ src/simulation/launch/bridge.launch.py | 10 ++++++++++ 3 files changed, 29 insertions(+) create mode 100644 src/description/urdf/ground_truth_odom.xacro diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index e361ecd..9d72e52 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -14,6 +14,7 @@ + @@ -46,6 +47,8 @@ + + + + + + + ${odom_frame} + ${robot_base_frame} + ${odom_topic} + ${odom_topic}/covariance + ${odom_topic}/tf + 3 + ${update_rate} + + + + diff --git a/src/simulation/launch/bridge.launch.py b/src/simulation/launch/bridge.launch.py index 40bc388..ed85435 100644 --- a/src/simulation/launch/bridge.launch.py +++ b/src/simulation/launch/bridge.launch.py @@ -32,4 +32,14 @@ def generate_launch_description(): '/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock', ] ), + + Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='ground_truth_odom_bridge', + output='screen', + arguments=[ + '/odom/ground_truth@nav_msgs/msg/Odometry@gz.msgs.Odometry', + ] + ), ]) From 8dcb0a9552b6d337d1516ccfcc423e72a80cde22 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Thu, 25 Dec 2025 19:44:57 -0500 Subject: [PATCH 6/9] fix world loading and sensors --- src/description/CMakeLists.txt | 2 + src/description/urdf/gps.xacro | 6 +- src/description/urdf/ground_truth_odom.xacro | 2 +- src/description/urdf/imu.xacro | 3 +- src/description/worlds/empty.sdf | 72 ++++++++++++++++++++ src/simulation/launch/gz_sim.launch.py | 15 ++-- 6 files changed, 91 insertions(+), 9 deletions(-) create mode 100644 src/description/worlds/empty.sdf diff --git a/src/description/CMakeLists.txt b/src/description/CMakeLists.txt index 6d0a1dc..1ef06c7 100644 --- a/src/description/CMakeLists.txt +++ b/src/description/CMakeLists.txt @@ -11,9 +11,11 @@ install(DIRECTORY config launch meshes + models rviz ros2_control urdf + worlds DESTINATION share/${PROJECT_NAME}/ ) diff --git a/src/description/urdf/gps.xacro b/src/description/urdf/gps.xacro index 80d47b9..b35e2e0 100644 --- a/src/description/urdf/gps.xacro +++ b/src/description/urdf/gps.xacro @@ -22,9 +22,9 @@ true 10 - /gps - - + /gps/fix + ${prefix}_gps_link + diff --git a/src/description/urdf/ground_truth_odom.xacro b/src/description/urdf/ground_truth_odom.xacro index c099c38..f0c1517 100644 --- a/src/description/urdf/ground_truth_odom.xacro +++ b/src/description/urdf/ground_truth_odom.xacro @@ -2,7 +2,7 @@ - + ${odom_frame} ${robot_base_frame} ${odom_topic} diff --git a/src/description/urdf/imu.xacro b/src/description/urdf/imu.xacro index 7d0e992..944b00a 100644 --- a/src/description/urdf/imu.xacro +++ b/src/description/urdf/imu.xacro @@ -23,7 +23,8 @@ true 100 /imu - + ${prefix}_imu_link + diff --git a/src/description/worlds/empty.sdf b/src/description/worlds/empty.sdf new file mode 100644 index 0000000..0988f3d --- /dev/null +++ b/src/description/worlds/empty.sdf @@ -0,0 +1,72 @@ + + + + + EARTH_WGS84 + ENU + 38.42391162772634 + -110.78490558433397 + 0.0 + 0.0 + + + + 0.001 + 1.0 + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + diff --git a/src/simulation/launch/gz_sim.launch.py b/src/simulation/launch/gz_sim.launch.py index c6814f5..71f0707 100644 --- a/src/simulation/launch/gz_sim.launch.py +++ b/src/simulation/launch/gz_sim.launch.py @@ -20,15 +20,22 @@ def generate_launch_description(): gz_resource_path = SetEnvironmentVariable( name='GZ_SIM_RESOURCE_PATH', - value=[str(Path(pkg_description).parent.resolve())]) - + value=[ + str(Path(pkg_description).parent.resolve()), + ':', + os.path.join(pkg_description, 'models') + ]) + gz_sim_launch = PathJoinSubstitution( [pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py']) - + + world_path = PathJoinSubstitution( + [pkg_description, 'worlds', LaunchConfiguration('world')]) + gazebo_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource([gz_sim_launch]), launch_arguments=[ - ('gz_args', [LaunchConfiguration('world'), + ('gz_args', [world_path, ' -r', ' -v 4'] ) From cd98c315e7d375a893cb293e1e0a037012a0d6ce Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Thu, 25 Dec 2025 21:22:13 -0500 Subject: [PATCH 7/9] removed test package, added node functionality, renamed to just localizer --- src/description/urdf/athena_drive.urdf.xacro | 4 +- src/description/urdf/gps.xacro | 14 +- src/description/urdf/imu.xacro | 14 +- .../athena_localizer_test/CMakeLists.txt | 61 ------- .../athena_localizer_test/data/metadata.yaml | 134 --------------- .../athena_localizer_test/package.xml | 23 --- .../src/athena_localizer_test_node.cpp | 104 ----------- .../src/localizer_evaluation_node.cpp | 145 ---------------- .../src/run_evaluation.sh | 17 -- .../CMakeLists.txt | 48 ++---- .../localizer/config/localizer.yaml | 34 ++++ .../include/localizer}/state_estimator.h | 81 +++++---- .../localizer/launch/localizer.launch.py | 48 ++++++ .../package.xml | 5 +- .../src/state_estimator.cpp | 161 ++++++++++++------ 15 files changed, 266 insertions(+), 627 deletions(-) delete mode 100644 src/subsystems/navigation/athena_localizer_test/CMakeLists.txt delete mode 100644 src/subsystems/navigation/athena_localizer_test/data/metadata.yaml delete mode 100644 src/subsystems/navigation/athena_localizer_test/package.xml delete mode 100644 src/subsystems/navigation/athena_localizer_test/src/athena_localizer_test_node.cpp delete mode 100644 src/subsystems/navigation/athena_localizer_test/src/localizer_evaluation_node.cpp delete mode 100755 src/subsystems/navigation/athena_localizer_test/src/run_evaluation.sh rename src/subsystems/navigation/{athena_localizer => localizer}/CMakeLists.txt (59%) create mode 100644 src/subsystems/navigation/localizer/config/localizer.yaml rename src/subsystems/navigation/{athena_localizer/include/athena_localizer => localizer/include/localizer}/state_estimator.h (80%) create mode 100644 src/subsystems/navigation/localizer/launch/localizer.launch.py rename src/subsystems/navigation/{athena_localizer => localizer}/package.xml (91%) rename src/subsystems/navigation/{athena_localizer => localizer}/src/state_estimator.cpp (76%) diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index 9d72e52..eddfdd1 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -39,11 +39,11 @@ - + - + diff --git a/src/description/urdf/gps.xacro b/src/description/urdf/gps.xacro index b35e2e0..106ef4b 100644 --- a/src/description/urdf/gps.xacro +++ b/src/description/urdf/gps.xacro @@ -1,13 +1,13 @@ - - + + - + - + @@ -18,12 +18,12 @@ - - + + true 10 /gps/fix - ${prefix}_gps_link + gnss_link diff --git a/src/description/urdf/imu.xacro b/src/description/urdf/imu.xacro index 944b00a..e431327 100644 --- a/src/description/urdf/imu.xacro +++ b/src/description/urdf/imu.xacro @@ -1,13 +1,13 @@ - - + + - + - + @@ -18,12 +18,12 @@ - - + + true 100 /imu - ${prefix}_imu_link + imu_link diff --git a/src/subsystems/navigation/athena_localizer_test/CMakeLists.txt b/src/subsystems/navigation/athena_localizer_test/CMakeLists.txt deleted file mode 100644 index 7a3227c..0000000 --- a/src/subsystems/navigation/athena_localizer_test/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(athena_localizer_test) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(athena_localizer REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) - -add_executable(athena_localizer_test_node src/athena_localizer_test_node.cpp) -target_include_directories(athena_localizer_test_node PUBLIC - $ - $) -target_compile_features(athena_localizer_test_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 - -ament_target_dependencies(athena_localizer_test_node - athena_localizer - rclcpp - sensor_msgs - geometry_msgs - nav_msgs) - -install(TARGETS athena_localizer_test_node - DESTINATION lib/${PROJECT_NAME}) - -add_executable(localizer_evaluation_node src/localizer_evaluation_node.cpp) -target_include_directories(localizer_evaluation_node PUBLIC - $ - $) -target_compile_features(localizer_evaluation_node PUBLIC c_std_99 cxx_std_17) - -ament_target_dependencies(localizer_evaluation_node - rclcpp - geometry_msgs - tf2 - tf2_ros) - -install(TARGETS localizer_evaluation_node - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/subsystems/navigation/athena_localizer_test/data/metadata.yaml b/src/subsystems/navigation/athena_localizer_test/data/metadata.yaml deleted file mode 100644 index f2a21aa..0000000 --- a/src/subsystems/navigation/athena_localizer_test/data/metadata.yaml +++ /dev/null @@ -1,134 +0,0 @@ -rosbag2_bagfile_information: - version: 9 - storage_identifier: mcap - duration: - nanoseconds: 68172873020 - starting_time: - nanoseconds_since_epoch: 1317051643426995038 - message_count: 9240 - topics_with_message_count: - - topic_metadata: - name: /tf_static - type: tf2_msgs/msg/TFMessage - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/camera_gray_left/camera_info - type: sensor_msgs/msg/CameraInfo - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/camera_gray_right/image_raw - type: sensor_msgs/msg/Image - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/camera_color_left/camera_info - type: sensor_msgs/msg/CameraInfo - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /tf - type: tf2_msgs/msg/TFMessage - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/camera_gray_left/image_raw - type: sensor_msgs/msg/Image - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/oxts/imu - type: sensor_msgs/msg/Imu - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/oxts/gps/fix - type: sensor_msgs/msg/NavSatFix - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/velo/pointcloud - type: sensor_msgs/msg/PointCloud2 - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/camera_gray_right/camera_info - type: sensor_msgs/msg/CameraInfo - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/camera_color_left/image_raw - type: sensor_msgs/msg/Image - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/oxts/gps/vel - type: geometry_msgs/msg/TwistStamped - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/camera_color_right/image_raw - type: sensor_msgs/msg/Image - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - - topic_metadata: - name: /kitti/camera_color_right/camera_info - type: sensor_msgs/msg/CameraInfo - serialization_format: cdr - offered_qos_profiles: - [] - type_description_hash: "" - message_count: 660 - compression_format: "" - compression_mode: "" - relative_file_paths: - - kitti_2011_09_26_drive_0117_synced_0.mcap - files: - - path: kitti_2011_09_26_drive_0117_synced_0.mcap - starting_time: - nanoseconds_since_epoch: 1317051643426995038 - duration: - nanoseconds: 68172873020 - message_count: 9240 - custom_data: ~ - ros_distro: jazzy \ No newline at end of file diff --git a/src/subsystems/navigation/athena_localizer_test/package.xml b/src/subsystems/navigation/athena_localizer_test/package.xml deleted file mode 100644 index 05e7c26..0000000 --- a/src/subsystems/navigation/athena_localizer_test/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - athena_localizer_test - 0.0.0 - TODO: Package description - ros - TODO: License declaration - - ament_cmake - - athena_localizer - rclcpp - sensor_msgs - geometry_msgs - nav_msgs - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/subsystems/navigation/athena_localizer_test/src/athena_localizer_test_node.cpp b/src/subsystems/navigation/athena_localizer_test/src/athena_localizer_test_node.cpp deleted file mode 100644 index 0eb47e7..0000000 --- a/src/subsystems/navigation/athena_localizer_test/src/athena_localizer_test_node.cpp +++ /dev/null @@ -1,104 +0,0 @@ -#include -#include -#include -#include - -class AthenaLocalizerTestNode : public rclcpp::Node -{ -public: - AthenaLocalizerTestNode() - : Node("athena_localizer_test_node") - { - RCLCPP_INFO(this->get_logger(), "Starting Athena Localizer Test Node"); - - imu_sub_ = this->create_subscription( - "/kitti/oxts/imu", 10, - std::bind(&AthenaLocalizerTestNode::imu_callback, this, std::placeholders::_1)); - - gnss_sub_ = this->create_subscription( - "/kitti/oxts/gps/fix", 10, - std::bind(&AthenaLocalizerTestNode::gnss_callback, this, std::placeholders::_1)); - - odom_sub_ = this->create_subscription( - "/odom", 10, - std::bind(&AthenaLocalizerTestNode::odom_callback, this, std::placeholders::_1)); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(100), - std::bind(&AthenaLocalizerTestNode::timer_callback, this)); - - RCLCPP_INFO(this->get_logger(), "Subscribers created"); - } - - void initialize_state_estimator() - { - // Initialize state estimator - athena_localizer::StateEstimatorParams params; - params.imu_accel_noise = 0.1; - params.imu_gyro_noise = 0.01; - params.imu_bias_noise = 0.001; - params.gnss_noise = 1.0; - params.odom_noise = 0.1; - params.max_time_window = 10.0; - params.max_states = 100; - - // Get tf_prefix parameter - params.frames.tf_prefix = this->declare_parameter("tf_prefix", std::string("test_jawn")); - - state_estimator_.initialize(params, shared_from_this()); - RCLCPP_INFO(this->get_logger(), "State estimator initialized"); - } - -private: - void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) - { - RCLCPP_DEBUG(this->get_logger(), "Received IMU data"); - state_estimator_.fuse_imu(msg); - } - - void gnss_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) - { - RCLCPP_INFO( - this->get_logger(), "Received GNSS data: lat=%.6f, lon=%.6f", - msg->latitude, msg->longitude); - state_estimator_.fuse_gnss(msg); - } - - void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) - { - RCLCPP_DEBUG(this->get_logger(), "Received odom data"); - state_estimator_.fuse_odometry(msg); - } - - void timer_callback() - { auto latest_state = state_estimator_.get_latest_state(); - if (latest_state.has_value()) { - RCLCPP_DEBUG(this->get_logger(), "State estimator has valid state"); - state_estimator_.publish_transforms(); - } else { - RCLCPP_DEBUG(this->get_logger(), "State estimator not yet initialized"); - } - } - - athena_localizer::StateEstimator state_estimator_; - rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr gnss_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::TimerBase::SharedPtr debug_timer_; -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - auto node = std::make_shared(); - - node->initialize_state_estimator(); - - RCLCPP_INFO(rclcpp::get_logger("main"), "Spinning Athena Localizer Test Node"); - rclcpp::spin(node); - - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/navigation/athena_localizer_test/src/localizer_evaluation_node.cpp b/src/subsystems/navigation/athena_localizer_test/src/localizer_evaluation_node.cpp deleted file mode 100644 index d3ced5a..0000000 --- a/src/subsystems/navigation/athena_localizer_test/src/localizer_evaluation_node.cpp +++ /dev/null @@ -1,145 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class LocalizerEvaluationNode : public rclcpp::Node -{ -public: - LocalizerEvaluationNode() : Node("localizer_evaluation_node"), alignment_set_(false) - { - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - - timer_ = this->create_wall_timer(std::chrono::milliseconds(100), - std::bind(&LocalizerEvaluationNode::evaluate, this)); - - output_.open("evaluation.csv"); - output_ << "timestamp,trans_error,rot_error,x,y,z,roll,pitch,yaw\n"; - } - - ~LocalizerEvaluationNode() - { - if (output_.is_open()) output_.close(); - print_stats(); - } - -private: - void evaluate() - { - geometry_msgs::msg::TransformStamped gt, est; - - try { - gt = tf_buffer_->lookupTransform("world", "base_link", tf2::TimePointZero); - est = tf_buffer_->lookupTransform("test_jawn/map", "test_jawn/base_link", tf2::TimePointZero); - } catch (tf2::TransformException &ex) { - return; - } - - if (!alignment_set_) { - offset_x_ = gt.transform.translation.x - est.transform.translation.x; - offset_y_ = gt.transform.translation.y - est.transform.translation.y; - offset_z_ = gt.transform.translation.z - est.transform.translation.z; - - tf2::Quaternion gt_q(gt.transform.rotation.x, gt.transform.rotation.y, - gt.transform.rotation.z, gt.transform.rotation.w); - tf2::Quaternion est_q(est.transform.rotation.x, est.transform.rotation.y, - est.transform.rotation.z, est.transform.rotation.w); - - offset_rot_ = gt_q * est_q.inverse(); - alignment_set_ = true; - } - - tf2::Vector3 est_pos(est.transform.translation.x + offset_x_, - est.transform.translation.y + offset_y_, - est.transform.translation.z + offset_z_); - - tf2::Quaternion est_q(est.transform.rotation.x, est.transform.rotation.y, - est.transform.rotation.z, est.transform.rotation.w); - tf2::Quaternion est_rot = offset_rot_ * est_q; - - double dx = gt.transform.translation.x - est_pos.x(); - double dy = gt.transform.translation.y - est_pos.y(); - double dz = gt.transform.translation.z - est_pos.z(); - double trans_error = std::sqrt(dx*dx + dy*dy + dz*dz); - - tf2::Quaternion gt_q(gt.transform.rotation.x, gt.transform.rotation.y, - gt.transform.rotation.z, gt.transform.rotation.w); - - double gt_r, gt_p, gt_y, est_r, est_p, est_y; - tf2::Matrix3x3(gt_q).getRPY(gt_r, gt_p, gt_y); - tf2::Matrix3x3(est_rot).getRPY(est_r, est_p, est_y); - - double dr = normalize(gt_r - est_r) * 180.0 / M_PI; - double dp = normalize(gt_p - est_p) * 180.0 / M_PI; - double dy_angle = normalize(gt_y - est_y) * 180.0 / M_PI; - - tf2::Quaternion error_q = gt_q * est_rot.inverse(); - double rot_error = 2.0 * std::acos(std::min(1.0, std::abs(error_q.w()))) * 180.0 / M_PI; - - double timestamp = gt.header.stamp.sec + gt.header.stamp.nanosec * 1e-9; - - output_ << timestamp << "," << trans_error << "," << rot_error << "," - << dx << "," << dy << "," << dz << "," - << dr << "," << dp << "," << dy_angle << "\n"; - - trans_errors_.push_back(trans_error); - rot_errors_.push_back(rot_error); - } - - double normalize(double angle) - { - while (angle > M_PI) angle -= 2.0 * M_PI; - while (angle < -M_PI) angle += 2.0 * M_PI; - return angle; - } - - void print_stats() - { - if (trans_errors_.empty()) return; - - double trans_sum = 0, rot_sum = 0, trans_max = 0, rot_max = 0; - double trans_sq = 0, rot_sq = 0; - - for (size_t i = 0; i < trans_errors_.size(); ++i) { - trans_sum += trans_errors_[i]; - rot_sum += rot_errors_[i]; - trans_sq += trans_errors_[i] * trans_errors_[i]; - rot_sq += rot_errors_[i] * rot_errors_[i]; - trans_max = std::max(trans_max, trans_errors_[i]); - rot_max = std::max(rot_max, rot_errors_[i]); - } - - size_t n = trans_errors_.size(); - - RCLCPP_INFO(this->get_logger(), "\n========== EVALUATION STATISTICS =========="); - RCLCPP_INFO(this->get_logger(), "Samples: %zu", n); - RCLCPP_INFO(this->get_logger(), "Translation: Mean %.4f m | RMSE %.4f m | Max %.4f m", - trans_sum/n, std::sqrt(trans_sq/n), trans_max); - RCLCPP_INFO(this->get_logger(), "Rotation: Mean %.4f deg | RMSE %.4f deg | Max %.4f deg", - rot_sum/n, std::sqrt(rot_sq/n), rot_max); - RCLCPP_INFO(this->get_logger(), "==========================================\n"); - } - - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - rclcpp::TimerBase::SharedPtr timer_; - std::ofstream output_; - std::vector trans_errors_, rot_errors_; - bool alignment_set_; - double offset_x_, offset_y_, offset_z_; - tf2::Quaternion offset_rot_; -}; - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/subsystems/navigation/athena_localizer_test/src/run_evaluation.sh b/src/subsystems/navigation/athena_localizer_test/src/run_evaluation.sh deleted file mode 100755 index b9f8c33..0000000 --- a/src/subsystems/navigation/athena_localizer_test/src/run_evaluation.sh +++ /dev/null @@ -1,17 +0,0 @@ -#!/bin/bash - -BAG_FILE="${1:-/workspaces/ros2-devenv/src/athena_localizer_test/data/kitti_2011_09_26_drive_0117_synced_0.mcap}" - -cleanup() { - pkill -P $$ - exit 0 -} -trap cleanup SIGINT SIGTERM - -ros2 bag play "$BAG_FILE" & -sleep 2 -ros2 run athena_localizer_test athena_localizer_test_node & -sleep 3 -ros2 run athena_localizer_test localizer_evaluation_node - -cleanup diff --git a/src/subsystems/navigation/athena_localizer/CMakeLists.txt b/src/subsystems/navigation/localizer/CMakeLists.txt similarity index 59% rename from src/subsystems/navigation/athena_localizer/CMakeLists.txt rename to src/subsystems/navigation/localizer/CMakeLists.txt index 7631dc1..726abd9 100644 --- a/src/subsystems/navigation/athena_localizer/CMakeLists.txt +++ b/src/subsystems/navigation/localizer/CMakeLists.txt @@ -1,11 +1,10 @@ cmake_minimum_required(VERSION 3.8) -project(athena_localizer) +project(localizer) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies find_package(ament_cmake REQUIRED) find_package(GTSAM REQUIRED) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/usr/share/cmake/geographiclib/") @@ -20,22 +19,18 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -# Create library for state estimator -add_library(${PROJECT_NAME} SHARED - src/state_estimator.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC +add_executable(localizer_node src/state_estimator.cpp) +target_include_directories(localizer_node PUBLIC $ $ ${GTSAM_INCLUDE_DIRS} ${GeographicLib_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} +target_link_libraries(localizer_node gtsam ${GeographicLib_LIBRARIES}) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_features(localizer_node PUBLIC c_std_99 cxx_std_17) -# Add ROS2 dependencies -ament_target_dependencies(${PROJECT_NAME} +ament_target_dependencies(localizer_node rclcpp sensor_msgs geometry_msgs @@ -45,36 +40,17 @@ ament_target_dependencies(${PROJECT_NAME} tf2_geometry_msgs Eigen3) -# Install library and headers -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME}Targets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include) +install(TARGETS localizer_node + DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/) -# Export library for other packages -ament_export_include_directories(include) -ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) -ament_export_dependencies( - rclcpp - sensor_msgs - geometry_msgs - nav_msgs - tf2 - tf2_ros - tf2_geometry_msgs - Eigen3 - GTSAM) +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config/) -# Generate and install CMake config files -install(EXPORT ${PROJECT_NAME}Targets - FILE ${PROJECT_NAME}Targets.cmake - NAMESPACE ${PROJECT_NAME}:: - DESTINATION lib/cmake/${PROJECT_NAME}) +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch/) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/src/subsystems/navigation/localizer/config/localizer.yaml b/src/subsystems/navigation/localizer/config/localizer.yaml new file mode 100644 index 0000000..53f81d9 --- /dev/null +++ b/src/subsystems/navigation/localizer/config/localizer.yaml @@ -0,0 +1,34 @@ +localizer_node: + ros__parameters: + # Topic configuration + imu_topic: "/imu" + gnss_topic: "/gps/fix" + odom_topic: "/odom" + output_odom_topic: "/localization/odometry" + output_pose_topic: "/localization/pose" + + # Publish rate (Hz) + publish_rate: 50.0 + + # Frame names + tf_prefix: "" + map_frame: "map" + odom_frame: "odom" + base_frame: "base_link" + imu_frame: "imu_link" + gnss_frame: "gnss_link" + + # IMU noise parameters + imu_accel_noise: 0.1 # m/s^2 + imu_gyro_noise: 0.005 # rad/s + imu_bias_noise: 0.0001 # bias random walk + + # GNSS noise parameters + gnss_noise: 1.0 # meters + + # Odometry noise parameters + odom_noise: 0.1 # meters + + # State estimation window parameters + max_time_window: 10.0 # seconds + max_states: 100 diff --git a/src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h b/src/subsystems/navigation/localizer/include/localizer/state_estimator.h similarity index 80% rename from src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h rename to src/subsystems/navigation/localizer/include/localizer/state_estimator.h index f0a80a8..9ba7a14 100644 --- a/src/subsystems/navigation/athena_localizer/include/athena_localizer/state_estimator.h +++ b/src/subsystems/navigation/localizer/include/localizer/state_estimator.h @@ -1,9 +1,3 @@ -/** - * @file state_estimator.h - * @brief Simplified state estimation for IMU and GNSS fusion using ROS2 messages - * @author Modified from MOLA StateEstimationSmoother for athena_localizer - * @date 2025 - */ #pragma once // ROS2 @@ -34,7 +28,7 @@ #include #include -namespace athena_localizer +namespace localizer { struct FrameParams @@ -75,36 +69,67 @@ struct EstimatedState geometry_msgs::msg::PoseWithCovarianceStamped to_pose(const std::string& frame_id) const; }; -class StateEstimator +class StateEstimator : public rclcpp::Node { public: StateEstimator(); ~StateEstimator(); - void initialize(const StateEstimatorParams& params, rclcpp::Node::SharedPtr node); void reset(); - void fuse_imu(const sensor_msgs::msg::Imu::SharedPtr& imu_msg); - void fuse_gnss(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg); - void fuse_odometry(const nav_msgs::msg::Odometry::SharedPtr& odom_msg); - std::optional get_latest_state() const; std::optional get_state_at_time(const rclcpp::Time& timestamp) const; - void publish_transforms(); - bool is_initialized() const { return initialized_; } private: + // Sensor fusion (used as subscription callbacks) + void fuse_imu(sensor_msgs::msg::Imu::SharedPtr imu_msg); + void fuse_gnss(sensor_msgs::msg::NavSatFix::SharedPtr gnss_msg); + void fuse_odometry(nav_msgs::msg::Odometry::SharedPtr odom_msg); + + // Publishing + void publish_state(); + void publish_transforms(); + + // Graph optimization + void optimize_graph(); + void add_imu_factor(); + void add_gnss_factor(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg); + void add_odom_factor(const nav_msgs::msg::Odometry::SharedPtr& odom_msg); + void createNewState(); + void marginalize_old_states(); + void initialize_with_gnss(const gtsam::Point3& gnss_pos, const rclcpp::Time& timestamp); + + // Coordinate conversion + gtsam::Point3 lla_to_enu(double lat, double lon, double alt) const; + void set_enu_origin(double lat, double lon, double alt); + + // Key generation + gtsam::Key pose_key(size_t i) const { return gtsam::Symbol('x', i); } + gtsam::Key vel_key(size_t i) const { return gtsam::Symbol('v', i); } + gtsam::Key bias_key(size_t i) const { return gtsam::Symbol('b', i); } + + // Parameters StateEstimatorParams params_; - rclcpp::Node::SharedPtr node_; + + // ROS2 subscriptions and publishers + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr gnss_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::TimerBase::SharedPtr publish_timer_; + + // TF2 components std::unique_ptr tf_broadcaster_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; + rclcpp::TimerBase::SharedPtr tf_timer_; + // Transform caches std::optional imu_to_base_; std::optional gnss_to_base_; - rclcpp::TimerBase::SharedPtr tf_timer_; // GTSAM components std::unique_ptr isam_; @@ -125,30 +150,16 @@ class StateEstimator rclcpp::Time last_optimization_time_; std::optional prev_odom_pose_; - mutable std::mutex state_mutex_; - - void optimize_graph(); - void add_imu_factor(); - void add_gnss_factor(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg); - void add_odom_factor(const nav_msgs::msg::Odometry::SharedPtr& odom_msg); - void createNewState(); - void marginalize_old_states(); - void initialize_with_gnss(const gtsam::Point3& gnss_pos, const rclcpp::Time& timestamp); - - gtsam::Point3 lla_to_enu(double lat, double lon, double alt) const; - void set_enu_origin(double lat, double lon, double alt); + // ENU origin bool enu_origin_set_; double origin_lat_, origin_lon_, origin_alt_; + // Noise models gtsam::SharedNoiseModel imu_noise_model_; gtsam::SharedNoiseModel gnss_noise_model_; gtsam::SharedNoiseModel odom_noise_model_; - // Key generation - gtsam::Key pose_key(size_t i) const { return gtsam::Symbol('x', i); } - gtsam::Key vel_key(size_t i) const { return gtsam::Symbol('v', i); } - gtsam::Key bias_key(size_t i) const { return gtsam::Symbol('b', i); } + mutable std::mutex state_mutex_; }; - -} // namespace athena_localizer \ No newline at end of file +} // namespace localizer \ No newline at end of file diff --git a/src/subsystems/navigation/localizer/launch/localizer.launch.py b/src/subsystems/navigation/localizer/launch/localizer.launch.py new file mode 100644 index 0000000..51c9a4d --- /dev/null +++ b/src/subsystems/navigation/localizer/launch/localizer.launch.py @@ -0,0 +1,48 @@ +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # Get the directory of this launch file + launch_file_dir = os.path.dirname(os.path.realpath(__file__)) + default_config = os.path.join(launch_file_dir, '..', 'config', 'localizer.yaml') + + # Launch arguments + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + config_file = LaunchConfiguration('config_file') + + return LaunchDescription([ + # Declare launch arguments + DeclareLaunchArgument( + 'namespace', + default_value='', + description='Robot namespace' + ), + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation time' + ), + DeclareLaunchArgument( + 'config_file', + default_value=default_config, + description='Path to the localizer configuration file' + ), + + # Athena Localizer Node + Node( + package='localizer', + executable='localizer_node', + namespace=namespace, + name='localizer_node', + parameters=[ + config_file, + {'use_sim_time': use_sim_time} + ], + output='screen', + emulate_tty=True, + ), + ]) diff --git a/src/subsystems/navigation/athena_localizer/package.xml b/src/subsystems/navigation/localizer/package.xml similarity index 91% rename from src/subsystems/navigation/athena_localizer/package.xml rename to src/subsystems/navigation/localizer/package.xml index 736012c..bb900ba 100644 --- a/src/subsystems/navigation/athena_localizer/package.xml +++ b/src/subsystems/navigation/localizer/package.xml @@ -1,14 +1,13 @@ - athena_localizer + localizer 0.0.0 - TODO: Package description + IMU-GNSS fusion state estimator using GTSAM ros TODO: License declaration gtsam - libgeographic-dev rclcpp sensor_msgs geometry_msgs diff --git a/src/subsystems/navigation/athena_localizer/src/state_estimator.cpp b/src/subsystems/navigation/localizer/src/state_estimator.cpp similarity index 76% rename from src/subsystems/navigation/athena_localizer/src/state_estimator.cpp rename to src/subsystems/navigation/localizer/src/state_estimator.cpp index 1d66edf..af97ebb 100644 --- a/src/subsystems/navigation/athena_localizer/src/state_estimator.cpp +++ b/src/subsystems/navigation/localizer/src/state_estimator.cpp @@ -1,4 +1,4 @@ -#include "athena_localizer/state_estimator.h" +#include "localizer/state_estimator.h" #include #include #include @@ -9,7 +9,7 @@ using namespace gtsam; -namespace athena_localizer +namespace localizer { nav_msgs::msg::Odometry EstimatedState::to_odometry(const std::string& frame_id, const std::string& child_frame_id) const @@ -82,9 +82,9 @@ geometry_msgs::msg::PoseWithCovarianceStamped EstimatedState::to_pose(const std: return pose_msg; } -// StateEstimator constructor StateEstimator::StateEstimator() - : odom_to_base_(Pose3()) + : Node("localizer_node") + , odom_to_base_(Pose3()) , map_to_odom_(Pose3()) , key_index_(0) , initialized_(false) @@ -100,43 +100,40 @@ StateEstimator::StateEstimator() isam_ = std::make_unique(isam_params); new_factors_ = std::make_unique(); new_values_ = std::make_unique(); -} - -// StateEstimator destructor -StateEstimator::~StateEstimator() = default; -void StateEstimator::initialize(const StateEstimatorParams& params, rclcpp::Node::SharedPtr node) -{ - params_ = params; - node_ = node; - - params_.imu_accel_noise = node_->declare_parameter("imu_accel_noise", params_.imu_accel_noise); - params_.imu_gyro_noise = node_->declare_parameter("imu_gyro_noise", params_.imu_gyro_noise); - params_.imu_bias_noise = node_->declare_parameter("imu_bias_noise", params_.imu_bias_noise); - params_.gnss_noise = node_->declare_parameter("gnss_noise", params_.gnss_noise); - params_.odom_noise = node_->declare_parameter("odom_noise", params_.odom_noise); - params_.max_time_window = node_->declare_parameter("max_time_window", params_.max_time_window); - params_.max_states = node_->declare_parameter("max_states", static_cast(params_.max_states)); - - params_.frames.tf_prefix = node_->declare_parameter("tf_prefix", params_.frames.tf_prefix); - params_.frames.map_frame = node_->declare_parameter("map_frame", params_.frames.map_frame); - params_.frames.odom_frame = node_->declare_parameter("odom_frame", params_.frames.odom_frame); - params_.frames.base_frame = node_->declare_parameter("base_frame", params_.frames.base_frame); - params_.frames.imu_frame = node_->declare_parameter("imu_frame", params_.frames.imu_frame); - params_.frames.gnss_frame = node_->declare_parameter("gnss_frame", params_.frames.gnss_frame); - - RCLCPP_INFO(node_->get_logger(), "Parameters: imu_accel=%.4f, imu_gyro=%.4f, gnss=%.2f", + std::string imu_topic = this->declare_parameter("imu_topic", "/imu"); + std::string gnss_topic = this->declare_parameter("gnss_topic", "/gps/fix"); + std::string odom_topic = this->declare_parameter("odom_topic", "/odom"); + std::string output_odom_topic = this->declare_parameter("output_odom_topic", "/localization/odometry"); + std::string output_pose_topic = this->declare_parameter("output_pose_topic", "/localization/pose"); + double publish_rate = this->declare_parameter("publish_rate", 50.0); + + params_.imu_accel_noise = this->declare_parameter("imu_accel_noise", params_.imu_accel_noise); + params_.imu_gyro_noise = this->declare_parameter("imu_gyro_noise", params_.imu_gyro_noise); + params_.imu_bias_noise = this->declare_parameter("imu_bias_noise", params_.imu_bias_noise); + params_.gnss_noise = this->declare_parameter("gnss_noise", params_.gnss_noise); + params_.odom_noise = this->declare_parameter("odom_noise", params_.odom_noise); + params_.max_time_window = this->declare_parameter("max_time_window", params_.max_time_window); + params_.max_states = this->declare_parameter("max_states", static_cast(params_.max_states)); + + params_.frames.tf_prefix = this->declare_parameter("tf_prefix", params_.frames.tf_prefix); + params_.frames.map_frame = this->declare_parameter("map_frame", params_.frames.map_frame); + params_.frames.base_frame = this->declare_parameter("base_frame", params_.frames.base_frame); + params_.frames.odom_frame = this->declare_parameter("odom_frame", params_.frames.odom_frame); + params_.frames.imu_frame = this->declare_parameter("imu_frame", params_.frames.imu_frame); + params_.frames.gnss_frame = this->declare_parameter("gnss_frame", params_.frames.gnss_frame); + + RCLCPP_INFO(this->get_logger(), "Parameters: imu_accel=%.4f, imu_gyro=%.4f, gnss=%.2f", params_.imu_accel_noise, params_.imu_gyro_noise, params_.gnss_noise); - tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - tf_broadcaster_ = std::make_unique(node_); + tf_broadcaster_ = std::make_unique(*this); - tf_timer_ = node_->create_wall_timer( + tf_timer_ = this->create_wall_timer( std::chrono::milliseconds(100), [this]() { publish_transforms(); }); - // Create noise models Matrix33 imu_cov = Matrix33::Identity() * params_.imu_accel_noise * params_.imu_accel_noise; Matrix33 gyro_cov = Matrix33::Identity() * params_.imu_gyro_noise * params_.imu_gyro_noise; Matrix33 bias_cov = Matrix33::Identity() * params_.imu_bias_noise * params_.imu_bias_noise; @@ -151,9 +148,36 @@ void StateEstimator::initialize(const StateEstimatorParams& params, rclcpp::Node gnss_noise_model_ = noiseModel::Diagonal::Sigmas((Vector3() << params_.gnss_noise, params_.gnss_noise, params_.gnss_noise).finished()); odom_noise_model_ = noiseModel::Diagonal::Sigmas((Vector6() << params_.odom_noise, params_.odom_noise, params_.odom_noise, params_.odom_noise, params_.odom_noise, params_.odom_noise).finished()); - RCLCPP_INFO(node_->get_logger(), "StateEstimator initialized"); + imu_sub_ = this->create_subscription( + imu_topic, rclcpp::SensorDataQoS(), + std::bind(&StateEstimator::fuse_imu, this, std::placeholders::_1)); + + gnss_sub_ = this->create_subscription( + gnss_topic, rclcpp::SensorDataQoS(), + std::bind(&StateEstimator::fuse_gnss, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + odom_topic, rclcpp::SensorDataQoS(), + std::bind(&StateEstimator::fuse_odometry, this, std::placeholders::_1)); + + odom_pub_ = this->create_publisher(output_odom_topic, 10); + pose_pub_ = this->create_publisher(output_pose_topic, 10); + + auto period = std::chrono::duration(1.0 / publish_rate); + publish_timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&StateEstimator::publish_state, this)); + + RCLCPP_INFO(this->get_logger(), "Athena Localizer Node initialized"); + RCLCPP_INFO(this->get_logger(), " IMU topic: %s", imu_topic.c_str()); + RCLCPP_INFO(this->get_logger(), " GNSS topic: %s", gnss_topic.c_str()); + RCLCPP_INFO(this->get_logger(), " Odom topic: %s", odom_topic.c_str()); + RCLCPP_INFO(this->get_logger(), " Output odom: %s", output_odom_topic.c_str()); + RCLCPP_INFO(this->get_logger(), " Output pose: %s", output_pose_topic.c_str()); } +StateEstimator::~StateEstimator() = default; + void StateEstimator::reset() { std::lock_guard lock(state_mutex_); @@ -179,10 +203,29 @@ void StateEstimator::reset() imu_preintegrated_->resetIntegration(); } - RCLCPP_INFO(node_->get_logger(), "StateEstimator reset"); + RCLCPP_INFO(this->get_logger(), "StateEstimator reset"); +} + +void StateEstimator::publish_state() +{ + auto latest_state = get_latest_state(); + if (!latest_state.has_value()) { + return; + } + + std::string full_map_frame = params_.frames.tf_prefix.empty() ? + params_.frames.map_frame : params_.frames.tf_prefix + "/" + params_.frames.map_frame; + std::string full_base_frame = params_.frames.tf_prefix.empty() ? + params_.frames.base_frame : params_.frames.tf_prefix + "/" + params_.frames.base_frame; + + auto odom_msg = latest_state->to_odometry(full_map_frame, full_base_frame); + odom_pub_->publish(odom_msg); + + auto pose_msg = latest_state->to_pose(full_map_frame); + pose_pub_->publish(pose_msg); } -void StateEstimator::fuse_imu(const sensor_msgs::msg::Imu::SharedPtr& imu_msg) +void StateEstimator::fuse_imu(sensor_msgs::msg::Imu::SharedPtr imu_msg) { std::lock_guard lock(state_mutex_); @@ -196,9 +239,9 @@ void StateEstimator::fuse_imu(const sensor_msgs::msg::Imu::SharedPtr& imu_msg) imu_to_base_ = Pose3( Rot3::Quaternion(q.w, q.x, q.y, q.z), Point3(t.x, t.y, t.z)); - RCLCPP_INFO(node_->get_logger(), "Cached IMU->base_link transform"); + RCLCPP_INFO(this->get_logger(), "Cached IMU->base_link transform"); } catch (const tf2::TransformException& ex) { - RCLCPP_DEBUG(node_->get_logger(), "IMU transform not available: %s", ex.what()); + RCLCPP_DEBUG(this->get_logger(), "IMU transform not available: %s", ex.what()); } } @@ -216,7 +259,7 @@ void StateEstimator::fuse_imu(const sensor_msgs::msg::Imu::SharedPtr& imu_msg) dt = std::max(0.001, std::min(0.1, computed_dt)); if (computed_dt <= 0.0) { - RCLCPP_WARN(node_->get_logger(), "Non-positive dt detected: %.6f, using default", computed_dt); + RCLCPP_WARN(this->get_logger(), "Non-positive dt detected: %.6f, using default", computed_dt); dt = 0.01; } } @@ -245,8 +288,11 @@ void StateEstimator::fuse_imu(const sensor_msgs::msg::Imu::SharedPtr& imu_msg) } } -void StateEstimator::fuse_gnss(const sensor_msgs::msg::NavSatFix::SharedPtr& gnss_msg) +void StateEstimator::fuse_gnss(sensor_msgs::msg::NavSatFix::SharedPtr gnss_msg) { + RCLCPP_DEBUG(this->get_logger(), "Received GNSS: lat=%.6f, lon=%.6f", + gnss_msg->latitude, gnss_msg->longitude); + std::lock_guard lock(state_mutex_); if (!gnss_to_base_) { @@ -258,10 +304,10 @@ void StateEstimator::fuse_gnss(const sensor_msgs::msg::NavSatFix::SharedPtr& gns tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z); - RCLCPP_INFO(node_->get_logger(), "Cached GNSS->base_link offset: (%.3f, %.3f, %.3f)", + RCLCPP_INFO(this->get_logger(), "Cached GNSS->base_link offset: (%.3f, %.3f, %.3f)", gnss_to_base_->x(), gnss_to_base_->y(), gnss_to_base_->z()); } catch (const tf2::TransformException& ex) { - RCLCPP_DEBUG(node_->get_logger(), "GNSS transform not available: %s", ex.what()); + RCLCPP_DEBUG(this->get_logger(), "GNSS transform not available: %s", ex.what()); } } @@ -284,7 +330,7 @@ void StateEstimator::fuse_gnss(const sensor_msgs::msg::NavSatFix::SharedPtr& gns marginalize_old_states(); } -void StateEstimator::fuse_odometry(const nav_msgs::msg::Odometry::SharedPtr& odom_msg) +void StateEstimator::fuse_odometry(nav_msgs::msg::Odometry::SharedPtr odom_msg) { std::lock_guard lock(state_mutex_); @@ -313,12 +359,12 @@ void StateEstimator::initialize_with_gnss(const Point3& gnss_pos, const rclcpp:: if (q.length2() > 0.01) { q.normalize(); initial_rotation = Rot3::Quaternion(q.w(), q.x(), q.y(), q.z()); - RCLCPP_INFO(node_->get_logger(), "Initializing with IMU orientation"); + RCLCPP_INFO(this->get_logger(), "Initializing with IMU orientation"); } else { - RCLCPP_WARN(node_->get_logger(), "IMU orientation invalid, using identity rotation"); + RCLCPP_WARN(this->get_logger(), "IMU orientation invalid, using identity rotation"); } } else { - RCLCPP_WARN(node_->get_logger(), "No IMU data available, using identity rotation"); + RCLCPP_WARN(this->get_logger(), "No IMU data available, using identity rotation"); } auto initial_pose = Pose3(initial_rotation, gnss_pos); @@ -363,7 +409,7 @@ void StateEstimator::initialize_with_gnss(const Point3& gnss_pos, const rclcpp:: initialized_ = true; last_optimization_time_ = timestamp; - RCLCPP_INFO(node_->get_logger(), "StateEstimator initialized with GNSS at (%.2f, %.2f, %.2f)", + RCLCPP_INFO(this->get_logger(), "StateEstimator initialized with GNSS at (%.2f, %.2f, %.2f)", gnss_pos.x(), gnss_pos.y(), gnss_pos.z()); } @@ -462,7 +508,7 @@ void StateEstimator::optimize_graph() if (!result.empty() && result.exists(current_pose_key_) && result.exists(current_vel_key_)) { EstimatedState state; - state.timestamp = node_->get_clock()->now(); + state.timestamp = this->get_clock()->now(); auto map_to_base = result.at(current_pose_key_); auto vel = result.at(current_vel_key_); @@ -482,7 +528,7 @@ void StateEstimator::optimize_graph() state.covariance.block<6, 6>(0, 0) = pose_cov; } } catch (const std::exception& e) { - RCLCPP_WARN(node_->get_logger(), "Failed to get covariance: %s", e.what()); + RCLCPP_WARN(this->get_logger(), "Failed to get covariance: %s", e.what()); state.covariance = Eigen::Matrix::Identity() * 0.1; } @@ -490,7 +536,7 @@ void StateEstimator::optimize_graph() } } catch (const std::exception& e) { - RCLCPP_WARN(node_->get_logger(), "ISAM2 update failed: %s", e.what()); + RCLCPP_WARN(this->get_logger(), "ISAM2 update failed: %s", e.what()); } } @@ -591,7 +637,7 @@ void StateEstimator::marginalize_old_states() return; } - auto time_threshold = node_->get_clock()->now() - rclcpp::Duration::from_seconds(params_.max_time_window); + auto time_threshold = this->get_clock()->now() - rclcpp::Duration::from_seconds(params_.max_time_window); while (!state_history_.empty() && state_history_.front().timestamp < time_threshold) { state_history_.pop_front(); @@ -618,7 +664,16 @@ void StateEstimator::set_enu_origin(double lat, double lon, double alt) origin_alt_ = alt; enu_origin_set_ = true; - RCLCPP_INFO(node_->get_logger(), "ENU origin set to (%.6f, %.6f, %.2f)", lat, lon, alt); + RCLCPP_INFO(this->get_logger(), "ENU origin set to (%.6f, %.6f, %.2f)", lat, lon, alt); } -} // namespace athena_localizer +} // namespace localizer + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From 28f09d2ec5a6748a84de308c2c98840c1535ce3c Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Thu, 25 Dec 2025 23:17:06 -0500 Subject: [PATCH 8/9] make more realistic --- src/description/urdf/athena_drive.urdf.xacro | 4 +- src/description/urdf/gps.xacro | 2 +- src/description/urdf/imu.xacro | 2 +- src/simulation/launch/bridge.launch.py | 2 +- .../localizer/config/localizer.yaml | 17 +++--- .../include/localizer/state_estimator.h | 4 -- .../localizer/launch/localizer.launch.py | 5 -- .../localizer/src/state_estimator.cpp | 52 ++++--------------- 8 files changed, 26 insertions(+), 62 deletions(-) diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index eddfdd1..6a5756f 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -40,11 +40,11 @@ - + - + diff --git a/src/description/urdf/gps.xacro b/src/description/urdf/gps.xacro index 106ef4b..2f9f4c4 100644 --- a/src/description/urdf/gps.xacro +++ b/src/description/urdf/gps.xacro @@ -21,7 +21,7 @@ true - 10 + 50 /gps/fix gnss_link diff --git a/src/description/urdf/imu.xacro b/src/description/urdf/imu.xacro index e431327..de3902a 100644 --- a/src/description/urdf/imu.xacro +++ b/src/description/urdf/imu.xacro @@ -21,7 +21,7 @@ true - 100 + 200 /imu imu_link diff --git a/src/simulation/launch/bridge.launch.py b/src/simulation/launch/bridge.launch.py index ed85435..d4ce7a9 100644 --- a/src/simulation/launch/bridge.launch.py +++ b/src/simulation/launch/bridge.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): name='clock_bridge', output='screen', arguments=[ - '/clock@rosgraph_msgs/msg/Clock@gz.msgs.Clock', + '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', ] ), diff --git a/src/subsystems/navigation/localizer/config/localizer.yaml b/src/subsystems/navigation/localizer/config/localizer.yaml index 53f81d9..54216e0 100644 --- a/src/subsystems/navigation/localizer/config/localizer.yaml +++ b/src/subsystems/navigation/localizer/config/localizer.yaml @@ -4,8 +4,7 @@ localizer_node: imu_topic: "/imu" gnss_topic: "/gps/fix" odom_topic: "/odom" - output_odom_topic: "/localization/odometry" - output_pose_topic: "/localization/pose" + output_odom_topic: "/localization/odom" # Publish rate (Hz) publish_rate: 50.0 @@ -19,16 +18,20 @@ localizer_node: gnss_frame: "gnss_link" # IMU noise parameters - imu_accel_noise: 0.1 # m/s^2 - imu_gyro_noise: 0.005 # rad/s - imu_bias_noise: 0.0001 # bias random walk + imu_accel_noise: 0.2 # m/s^2 + imu_gyro_noise: 0.01 # rad/s + imu_bias_noise: 0.0002 # bias random walk # GNSS noise parameters - gnss_noise: 1.0 # meters + gnss_noise: 0.25 # meters # Odometry noise parameters - odom_noise: 0.1 # meters + odom_noise: 0.5 # meters # State estimation window parameters max_time_window: 10.0 # seconds max_states: 100 + + origin_lat: 38.42391162772634 + origin_lon: -110.78490558433397 + origin_alt: 0.0 diff --git a/src/subsystems/navigation/localizer/include/localizer/state_estimator.h b/src/subsystems/navigation/localizer/include/localizer/state_estimator.h index 9ba7a14..0cef53a 100644 --- a/src/subsystems/navigation/localizer/include/localizer/state_estimator.h +++ b/src/subsystems/navigation/localizer/include/localizer/state_estimator.h @@ -4,8 +4,6 @@ #include #include #include -#include -#include #include #include #include @@ -66,7 +64,6 @@ struct EstimatedState Eigen::Matrix covariance; nav_msgs::msg::Odometry to_odometry(const std::string& frame_id, const std::string& child_frame_id) const; - geometry_msgs::msg::PoseWithCovarianceStamped to_pose(const std::string& frame_id) const; }; class StateEstimator : public rclcpp::Node @@ -118,7 +115,6 @@ class StateEstimator : public rclcpp::Node rclcpp::Subscription::SharedPtr gnss_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::TimerBase::SharedPtr publish_timer_; // TF2 components diff --git a/src/subsystems/navigation/localizer/launch/localizer.launch.py b/src/subsystems/navigation/localizer/launch/localizer.launch.py index 51c9a4d..ca36a60 100644 --- a/src/subsystems/navigation/localizer/launch/localizer.launch.py +++ b/src/subsystems/navigation/localizer/launch/localizer.launch.py @@ -5,17 +5,14 @@ from launch_ros.actions import Node def generate_launch_description(): - # Get the directory of this launch file launch_file_dir = os.path.dirname(os.path.realpath(__file__)) default_config = os.path.join(launch_file_dir, '..', 'config', 'localizer.yaml') - # Launch arguments namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') config_file = LaunchConfiguration('config_file') return LaunchDescription([ - # Declare launch arguments DeclareLaunchArgument( 'namespace', default_value='', @@ -31,8 +28,6 @@ def generate_launch_description(): default_value=default_config, description='Path to the localizer configuration file' ), - - # Athena Localizer Node Node( package='localizer', executable='localizer_node', diff --git a/src/subsystems/navigation/localizer/src/state_estimator.cpp b/src/subsystems/navigation/localizer/src/state_estimator.cpp index af97ebb..c32dbbf 100644 --- a/src/subsystems/navigation/localizer/src/state_estimator.cpp +++ b/src/subsystems/navigation/localizer/src/state_estimator.cpp @@ -51,37 +51,6 @@ nav_msgs::msg::Odometry EstimatedState::to_odometry(const std::string& frame_id, return odom; } -geometry_msgs::msg::PoseWithCovarianceStamped EstimatedState::to_pose(const std::string& frame_id) const -{ - geometry_msgs::msg::PoseWithCovarianceStamped pose_msg; - pose_msg.header.stamp = timestamp; - pose_msg.header.frame_id = frame_id; - - auto pose = nav_state.pose(); - pose_msg.pose.pose.position.x = pose.x(); - pose_msg.pose.pose.position.y = pose.y(); - pose_msg.pose.pose.position.z = pose.z(); - - auto quat = pose.rotation().toQuaternion(); - pose_msg.pose.pose.orientation.w = quat.w(); - pose_msg.pose.pose.orientation.x = quat.x(); - pose_msg.pose.pose.orientation.y = quat.y(); - pose_msg.pose.pose.orientation.z = quat.z(); - - for (int i = 0; i < 36; ++i) { - pose_msg.pose.covariance[i] = 0.0; - } - for (int i = 0; i < 6; ++i) { - for (int j = 0; j < 6; ++j) { - if (i < 3 && j < 3) { - pose_msg.pose.covariance[i*6 + j] = covariance(i, j); - } - } - } - - return pose_msg; -} - StateEstimator::StateEstimator() : Node("localizer_node") , odom_to_base_(Pose3()) @@ -104,8 +73,7 @@ StateEstimator::StateEstimator() std::string imu_topic = this->declare_parameter("imu_topic", "/imu"); std::string gnss_topic = this->declare_parameter("gnss_topic", "/gps/fix"); std::string odom_topic = this->declare_parameter("odom_topic", "/odom"); - std::string output_odom_topic = this->declare_parameter("output_odom_topic", "/localization/odometry"); - std::string output_pose_topic = this->declare_parameter("output_pose_topic", "/localization/pose"); + std::string output_odom_topic = this->declare_parameter("output_odom_topic", "/localization/odom"); double publish_rate = this->declare_parameter("publish_rate", 50.0); params_.imu_accel_noise = this->declare_parameter("imu_accel_noise", params_.imu_accel_noise); @@ -123,6 +91,14 @@ StateEstimator::StateEstimator() params_.frames.imu_frame = this->declare_parameter("imu_frame", params_.frames.imu_frame); params_.frames.gnss_frame = this->declare_parameter("gnss_frame", params_.frames.gnss_frame); + double origin_lat = this->declare_parameter("origin_lat", 0.0); + double origin_lon = this->declare_parameter("origin_lon", 0.0); + double origin_alt = this->declare_parameter("origin_alt", 0.0); + + if (std::abs(origin_lat) > 1e-6 || std::abs(origin_lon) > 1e-6) { + set_enu_origin(origin_lat, origin_lon, origin_alt); + } + RCLCPP_INFO(this->get_logger(), "Parameters: imu_accel=%.4f, imu_gyro=%.4f, gnss=%.2f", params_.imu_accel_noise, params_.imu_gyro_noise, params_.gnss_noise); @@ -161,19 +137,17 @@ StateEstimator::StateEstimator() std::bind(&StateEstimator::fuse_odometry, this, std::placeholders::_1)); odom_pub_ = this->create_publisher(output_odom_topic, 10); - pose_pub_ = this->create_publisher(output_pose_topic, 10); auto period = std::chrono::duration(1.0 / publish_rate); publish_timer_ = this->create_wall_timer( std::chrono::duration_cast(period), std::bind(&StateEstimator::publish_state, this)); - RCLCPP_INFO(this->get_logger(), "Athena Localizer Node initialized"); + RCLCPP_INFO(this->get_logger(), "Localizer Node initialized"); RCLCPP_INFO(this->get_logger(), " IMU topic: %s", imu_topic.c_str()); RCLCPP_INFO(this->get_logger(), " GNSS topic: %s", gnss_topic.c_str()); RCLCPP_INFO(this->get_logger(), " Odom topic: %s", odom_topic.c_str()); RCLCPP_INFO(this->get_logger(), " Output odom: %s", output_odom_topic.c_str()); - RCLCPP_INFO(this->get_logger(), " Output pose: %s", output_pose_topic.c_str()); } StateEstimator::~StateEstimator() = default; @@ -218,11 +192,7 @@ void StateEstimator::publish_state() std::string full_base_frame = params_.frames.tf_prefix.empty() ? params_.frames.base_frame : params_.frames.tf_prefix + "/" + params_.frames.base_frame; - auto odom_msg = latest_state->to_odometry(full_map_frame, full_base_frame); - odom_pub_->publish(odom_msg); - - auto pose_msg = latest_state->to_pose(full_map_frame); - pose_pub_->publish(pose_msg); + odom_pub_->publish(latest_state->to_odometry(full_map_frame, full_base_frame)); } void StateEstimator::fuse_imu(sensor_msgs::msg::Imu::SharedPtr imu_msg) From 7717d9eef2652ff023cecc998a4547aca0799248 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Mon, 29 Dec 2025 19:32:30 -0500 Subject: [PATCH 9/9] remove old .rviz file --- src/description/rviz/new_athena_drive.rviz | 255 --------------------- 1 file changed, 255 deletions(-) delete mode 100644 src/description/rviz/new_athena_drive.rviz diff --git a/src/description/rviz/new_athena_drive.rviz b/src/description/rviz/new_athena_drive.rviz deleted file mode 100644 index d8e5739..0000000 --- a/src/description/rviz/new_athena_drive.rviz +++ /dev/null @@ -1,255 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 87 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /RobotModel1 - Splitter Ratio: 0.5 - Tree Height: 756 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - bracket_back_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bracket_back2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bracket_front_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bracket_front2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - suspension_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - suspension_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - swerve_back_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - swerve_back2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - swerve_front_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - swerve_front2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - swerve_module_back_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - swerve_module_back2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - swerve_module_front_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - swerve_module_front2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wheel_back_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wheel_back2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wheel_front_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wheel_front2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 2.962221622467041 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.34074074029922485 - Y: -0.6165840029716492 - Z: 0.3352876305580139 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.3553987741470337 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.713573932647705 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1027 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000398fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000398000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011000000398fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004400000398000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005040000039800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 1920 - Y: 24 \ No newline at end of file