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/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 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/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 deleted file mode 100644 index b0197ef..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 - chassis_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 diff --git a/src/description/urdf/athena_drive.urdf.xacro b/src/description/urdf/athena_drive.urdf.xacro index dbce67b..6a5756f 100644 --- a/src/description/urdf/athena_drive.urdf.xacro +++ b/src/description/urdf/athena_drive.urdf.xacro @@ -14,6 +14,7 @@ + @@ -37,6 +38,17 @@ + + + + + + + + + + + - + @@ -52,7 +52,7 @@ - + @@ -325,7 +325,7 @@ - + diff --git a/src/description/urdf/gps.xacro b/src/description/urdf/gps.xacro index 80d47b9..2f9f4c4 100644 --- a/src/description/urdf/gps.xacro +++ b/src/description/urdf/gps.xacro @@ -1,13 +1,13 @@ - - + + - + - + @@ -18,13 +18,13 @@ - - + + true - 10 - /gps - - + 50 + /gps/fix + gnss_link + diff --git a/src/description/urdf/ground_truth_odom.xacro b/src/description/urdf/ground_truth_odom.xacro new file mode 100644 index 0000000..f0c1517 --- /dev/null +++ b/src/description/urdf/ground_truth_odom.xacro @@ -0,0 +1,16 @@ + + + + + + ${odom_frame} + ${robot_base_frame} + ${odom_topic} + ${odom_topic}/covariance + ${odom_topic}/tf + 3 + ${update_rate} + + + + diff --git a/src/description/urdf/imu.xacro b/src/description/urdf/imu.xacro index 7d0e992..de3902a 100644 --- a/src/description/urdf/imu.xacro +++ b/src/description/urdf/imu.xacro @@ -1,13 +1,13 @@ - - + + - + - + @@ -18,12 +18,13 @@ - - + + true - 100 + 200 /imu - + 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/bridge.launch.py b/src/simulation/launch/bridge.launch.py index 40bc388..d4ce7a9 100644 --- a/src/simulation/launch/bridge.launch.py +++ b/src/simulation/launch/bridge.launch.py @@ -29,7 +29,17 @@ 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', + ] + ), + + 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', ] ), ]) 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'] ) diff --git a/src/subsystems/navigation/localizer/CMakeLists.txt b/src/subsystems/navigation/localizer/CMakeLists.txt new file mode 100644 index 0000000..726abd9 --- /dev/null +++ b/src/subsystems/navigation/localizer/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(localizer) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +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) + +add_executable(localizer_node src/state_estimator.cpp) +target_include_directories(localizer_node PUBLIC + $ + $ + ${GTSAM_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS}) +target_link_libraries(localizer_node + gtsam + ${GeographicLib_LIBRARIES}) +target_compile_features(localizer_node PUBLIC c_std_99 cxx_std_17) + +ament_target_dependencies(localizer_node + rclcpp + sensor_msgs + geometry_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + Eigen3) + +install(TARGETS localizer_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY include/ + DESTINATION include/) + +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config/) + +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch/) + +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/localizer/config/localizer.yaml b/src/subsystems/navigation/localizer/config/localizer.yaml new file mode 100644 index 0000000..54216e0 --- /dev/null +++ b/src/subsystems/navigation/localizer/config/localizer.yaml @@ -0,0 +1,37 @@ +localizer_node: + ros__parameters: + # Topic configuration + imu_topic: "/imu" + gnss_topic: "/gps/fix" + odom_topic: "/odom" + output_odom_topic: "/localization/odom" + + # 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.2 # m/s^2 + imu_gyro_noise: 0.01 # rad/s + imu_bias_noise: 0.0002 # bias random walk + + # GNSS noise parameters + gnss_noise: 0.25 # meters + + # Odometry noise parameters + 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 new file mode 100644 index 0000000..0cef53a --- /dev/null +++ b/src/subsystems/navigation/localizer/include/localizer/state_estimator.h @@ -0,0 +1,161 @@ +#pragma once + +// ROS2 +#include +#include +#include +#include +#include +#include +#include + +// GTSAM +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// std +#include +#include +#include +#include + +namespace 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; +}; + +class StateEstimator : public rclcpp::Node +{ +public: + StateEstimator(); + ~StateEstimator(); + + void reset(); + + std::optional get_latest_state() const; + std::optional get_state_at_time(const rclcpp::Time& timestamp) const; + + 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_; + + // 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::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_; + + // 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_; + + // 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_; + + mutable std::mutex state_mutex_; +}; + +} // 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..ca36a60 --- /dev/null +++ b/src/subsystems/navigation/localizer/launch/localizer.launch.py @@ -0,0 +1,43 @@ +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(): + launch_file_dir = os.path.dirname(os.path.realpath(__file__)) + default_config = os.path.join(launch_file_dir, '..', 'config', 'localizer.yaml') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + config_file = LaunchConfiguration('config_file') + + return LaunchDescription([ + 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' + ), + 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/localizer/package.xml b/src/subsystems/navigation/localizer/package.xml new file mode 100644 index 0000000..bb900ba --- /dev/null +++ b/src/subsystems/navigation/localizer/package.xml @@ -0,0 +1,36 @@ + + + + localizer + 0.0.0 + IMU-GNSS fusion state estimator using GTSAM + ros + TODO: License declaration + + gtsam + 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/localizer/src/state_estimator.cpp b/src/subsystems/navigation/localizer/src/state_estimator.cpp new file mode 100644 index 0000000..c32dbbf --- /dev/null +++ b/src/subsystems/navigation/localizer/src/state_estimator.cpp @@ -0,0 +1,649 @@ +#include "localizer/state_estimator.h" +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +namespace 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; +} + +StateEstimator::StateEstimator() + : Node("localizer_node") + , 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(); + + 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/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); + 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); + + 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); + + tf_buffer_ = std::make_shared(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + tf_broadcaster_ = std::make_unique(*this); + + tf_timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + [this]() { publish_transforms(); }); + + 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()); + + 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); + + 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(), "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()); +} + +StateEstimator::~StateEstimator() = default; + +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(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; + + odom_pub_->publish(latest_state->to_odometry(full_map_frame, full_base_frame)); +} + +void StateEstimator::fuse_imu(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(this->get_logger(), "Cached IMU->base_link transform"); + } catch (const tf2::TransformException& ex) { + RCLCPP_DEBUG(this->get_logger(), "IMU transform not available: %s", ex.what()); + } + } + + 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(this->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_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); + } + + imu_buffer_.push_back(*imu_msg); + + if (imu_buffer_.size() > 1000) { + imu_buffer_.pop_front(); + } +} + +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_) { + 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(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(this->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)); + return; + } + + add_gnss_factor(gnss_msg); + optimize_graph(); + marginalize_old_states(); +} + +void StateEstimator::fuse_odometry(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(this->get_logger(), "Initializing with IMU orientation"); + } else { + RCLCPP_WARN(this->get_logger(), "IMU orientation invalid, using identity rotation"); + } + } else { + RCLCPP_WARN(this->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(this->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 = this->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_); + } + + odom_to_base_ = map_to_base; + + // 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(this->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(this->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); + if (gnss_to_base_) { + gnss_pos = gnss_pos + *gnss_to_base_; + } + + 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 = 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(); + } +} + +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(this->get_logger(), "ENU origin set to (%.6f, %.6f, %.2f)", lat, lon, alt); +} + +} // namespace localizer + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}