Add localizer package back in and add base-link in properly

.gitignore

@@ -207,3 +207,5 @@ cython_debug/
 *.out
 *.app
 
+.DS_Store
+*.mcap

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:

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

src/description/urdf/athena_drive.urdf.xacro

@@ -37,6 +37,15 @@
   <!-- Contains description of drive -->
   <xacro:description/>
 
+
+  <xacro:imu_sensor parent="base_link" prefix="rover">
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+  </xacro:imu_sensor>
+
+  <xacro:gps_sensor parent="base_link" prefix="rover">
+    <origin xyz="0 0 0.3" rpy="0 0 0"/>
+  </xacro:gps_sensor>
+
   <xacro:if value="$(arg use_mock_hardware)">
     <xacro:athena_drive_ros2_control
     name="athena_drive"

src/description/urdf/athena_drive_description.urdf.xacro

@@ -4,7 +4,7 @@
      For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="description">
-        <link name="chassis_base_link">
+        <link name="base_link">
             <inertial>
                 <origin xyz="0.775305042176281 -0.0692748019047986 -0.475175318827691" rpy="0 0 0" />
                 <mass value="44.0454429414672" />
@@ -52,7 +52,7 @@
 
         <joint name="suspension_front_left_joint" type="fixed">
             <origin xyz="0.61263 0.22146 -0.74592" rpy="1.5708 0 0.094743" />
-            <parent link="chassis_base_link" />
+            <parent link="base_link" />
             <child link="suspension_front_left_link" />
             <axis xyz="0 0 -1" />
             <limit lower="-1.0471975512" upper="1.0471975512" effort="200" velocity="3" />
@@ -325,7 +325,7 @@
 
         <joint name="suspension_front_right_joint" type="fixed">
             <origin xyz="0.66803 -0.36146 -0.74592" rpy="1.5708 0 0.094743" />
-            <parent link="chassis_base_link" />
+            <parent link="base_link" />
             <child link="suspension_front_right_link" />
             <axis xyz="0 0 1" />
             <limit lower="-1.0471975512" upper="1.0471975512" effort="200" velocity="3" />

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${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()

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 <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/buffer.h>
+
+// GTSAM
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/navigation/NavState.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/navigation/GPSFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/inference/Symbol.h>
+
+// std
+#include <mutex>
+#include <optional>
+#include <deque>
+#include <memory>
+
+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<double, 15, 15> 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<EstimatedState> get_latest_state() const;
+    std::optional<EstimatedState> 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<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+    // GTSAM components
+    std::unique_ptr<gtsam::ISAM2> isam_;
+    std::unique_ptr<gtsam::NonlinearFactorGraph> new_factors_;
+    std::unique_ptr<gtsam::Values> new_values_;
+    std::unique_ptr<gtsam::PreintegratedImuMeasurements> imu_preintegrated_;
+
+    // State management
+    std::deque<EstimatedState> state_history_;
+    std::deque<sensor_msgs::msg::Imu> 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<gtsam::Pose3> 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

src/subsystems/navigation/athena_localizer/package.xml

@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>athena_localizer</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="[email protected]">ros</maintainer>
+    <license>TODO: License declaration</license>
+
+    <depend>gtsam</depend>
+    <depend>libgeographic-dev</depend>
+    <depend>rclcpp</depend>
+    <depend>sensor_msgs</depend>
+    <depend>geometry_msgs</depend>
+    <depend>nav_msgs</depend>
+    <depend>tf2</depend>
+    <depend>tf2_ros</depend>
+    <depend>tf2_geometry_msgs</depend>
+    <depend>eigen3_cmake_module</depend>
+    <build_depend>libboost-serialization-dev</build_depend>
+    <build_depend>libboost-system-dev</build_depend>
+    <build_depend>libboost-filesystem-dev</build_depend>
+    <build_depend>libboost-thread-dev</build_depend>
+    <build_depend>libboost-program-options-dev</build_depend>
+    <build_depend>libboost-date-time-dev</build_depend>
+    <build_depend>libboost-timer-dev</build_depend>
+    <build_depend>libboost-chrono-dev</build_depend>
+    <build_depend>libboost-regex-dev</build_depend>
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>