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