diff --git a/README.md b/README.md index 82f0e5c..e5b3807 100644 --- a/README.md +++ b/README.md @@ -135,7 +135,7 @@ cargo run --example battery_soc_estimation ``` Produces `libaegis_filter.a` for direct linking with C/C++ toolchains (GCC, Clang, MSVC). -### Integration Example: NASA F Prime GNC Component +### Integration Example 1: NASA F Prime GNC Component The repository includes a reference adapter demonstrating how to bind `aegis-filter` into flight software architectures such as NASA's [F Prime](https://nasa.github.io/fprime/) framework: diff --git a/examples/ros2_adapter/CMakeLists.txt b/examples/ros2_adapter/CMakeLists.txt new file mode 100644 index 0000000..1eb3599 --- /dev/null +++ b/examples/ros2_adapter/CMakeLists.txt @@ -0,0 +1,111 @@ +cmake_minimum_required(VERSION 3.8) +project(aegis_ros2_adapter) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=return-type) +endif() + +# Find ROS 2 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +# Path to aegis-filter C-FFI header and compiled library +set(AEGIS_FILTER_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/../..") +set(AEGIS_FILTER_INCLUDE_DIR "${AEGIS_FILTER_ROOT}/include" CACHE PATH "Path to aegis-filter include directory") +set(AEGIS_FILTER_LIB_DIR "${AEGIS_FILTER_ROOT}/target/release" CACHE PATH "Path to aegis-filter compiled library directory") + +# Allow fallback to debug target if release is not yet built +if(NOT EXISTS "${AEGIS_FILTER_LIB_DIR}/libaegis_filter.a" AND NOT EXISTS "${AEGIS_FILTER_LIB_DIR}/libaegis_filter.so") + if(EXISTS "${AEGIS_FILTER_ROOT}/target/debug/libaegis_filter.a" OR EXISTS "${AEGIS_FILTER_ROOT}/target/debug/libaegis_filter.so") + set(AEGIS_FILTER_LIB_DIR "${AEGIS_FILTER_ROOT}/target/debug") + endif() +endif() + +find_library(AEGIS_FILTER_LIB + NAMES aegis_filter + PATHS ${AEGIS_FILTER_LIB_DIR} + NO_DEFAULT_PATH +) + +if(NOT AEGIS_FILTER_LIB) + message(WARNING "libaegis_filter library not found at ${AEGIS_FILTER_LIB_DIR}. Run 'cargo build --release --features ffi' in the repository root prior to building.") + # Define target placeholder so colcon can still parse the package + add_library(aegis_filter_lib UNKNOWN IMPORTED) +else() + add_library(aegis_filter_lib UNKNOWN IMPORTED) + set_target_properties(aegis_filter_lib PROPERTIES + IMPORTED_LOCATION "${AEGIS_FILTER_LIB}" + INTERFACE_INCLUDE_DIRECTORIES "${AEGIS_FILTER_INCLUDE_DIR}" + ) +endif() + +# Include directories +include_directories( + include + ${AEGIS_FILTER_INCLUDE_DIR} +) + +# Core ROS 2 adapter library +add_library(aegis_ros2_adapter_lib + src/aegis_node.cpp +) + +target_include_directories(aegis_ros2_adapter_lib PUBLIC + $ + $ + $ +) + +ament_target_dependencies(aegis_ros2_adapter_lib + rclcpp + sensor_msgs + geometry_msgs + nav_msgs + std_msgs +) + +if(TARGET aegis_filter_lib) + target_link_libraries(aegis_ros2_adapter_lib PUBLIC aegis_filter_lib) +endif() + +# Executable node +add_executable(aegis_filter_node + src/main.cpp +) + +target_link_libraries(aegis_filter_node + aegis_ros2_adapter_lib +) + +ament_target_dependencies(aegis_filter_node + rclcpp +) + +# Install targets +install(TARGETS + aegis_ros2_adapter_lib + aegis_filter_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/examples/ros2_adapter/README.md b/examples/ros2_adapter/README.md new file mode 100644 index 0000000..eae6d48 --- /dev/null +++ b/examples/ros2_adapter/README.md @@ -0,0 +1,182 @@ +# ROS 2 C++ Node Adapter for aegis-filter + +High-integrity, zero-allocation Robot Operating System 2 (ROS 2) C++ node adapter integrating the **`aegis-filter`** state estimation, sensor fusion, and Fault Detection, Isolation, and Recovery (FDIR) engine into modern robotics stacks. + +Designed for Autonomous Mobile Robots (AMRs), industrial manipulators, UAVs, and autonomous driving architectures where **adversarial outlier rejection**, **numerical stability**, and **deterministic execution** are mission-critical. + +--- + +## 1. Architectural Overview + +``` + ROS 2 Topics + │ + /imu/data │ /pose/sample + (sensor_msgs/msg/Imu) │ (geometry_msgs/msg/PoseWithCovarianceStamped) + │ │ │ + ▼ │ ▼ +┌─────────────────────────┐ │ ┌─────────────────────────┐ +│ imu_callback(msg) │ │ │ pose_callback(msg) │ +│ - Compute dt │ │ │ - Extract raw z-pos │ +│ - Control u = a_z * dt │ │ │ - FDIR Chi-square gate │ +└────────────┬────────────┘ │ └───────────┬─────────────┘ + │ │ │ + ▼ │ ▼ +┌─────────────────────────────────────────────────────────────────────┐ +│ aegis-filter C-FFI Engine │ +│ │ +│ • aegis_fusion4_predict() ──> Propagate state & covariance │ +│ • aegis_fusion4_fuse() ──> Joseph-form covariance update │ +│ • Chi-square Gating (3.0σ) ──> Discard adversarial / spoofed spikes│ +│ • Health Degradation (FDIR)──> Quarantine persistent anomalies │ +└──────────────────────────────────┬──────────────────────────────────┘ + │ + ▼ + ┌───────────────────────────────────────────┐ + │ Published Topics │ + │ • /aegis/filtered_odometry (nav_msgs) │ + │ • /aegis/sensor_health (std_msgs) │ + └───────────────────────────────────────────┘ +``` + +### Key Performance & Safety Guarantees +- **Zero Heap Allocations on Hot Path**: The `AegisFusion4` engine struct is embedded directly as a value member within the `AegisFilterNode` class, eliminating runtime `malloc`/`free` calls during high-rate callbacks. +- **Joseph Form Covariance Updates**: Unconditionally guarantees positive semi-definiteness and matrix symmetry under finite floating-point arithmetic. +- **Automated FDIR Gating**: Transient sensor glitches or adversarial spoofing bursts are rejected before state contamination, with automated recovery after consecutive valid samples. + +--- + +## 2. Prerequisites + +- **ROS 2**: Compatible with ROS 2 **Humble Hawksbill**, **Iron Irwini**, **Jazzy Jalisco**, or **Rolling Ridley**. +- **Rust Toolchain**: Rust stable (`1.70+`) with `cargo`. +- **C++ Compiler**: GCC 9+ or Clang 11+ with C++17 support. +- **Build Tools**: `colcon` and `ament_cmake`. + +--- + +## 3. Build Instructions (Cargo + Colcon) + +Building the adapter is a standard two-stage process: compile the Rust C-FFI static library, then build the ROS 2 package using `colcon`. + +### Step 1: Build the aegis-filter C-FFI Library + +From the repository root: + +```bash +# Build the release static library (produces target/release/libaegis_filter.a) +cargo build --release --features ffi +``` + +For ARM Cortex-M or single-precision robotics platforms: +```bash +cargo build --release --no-default-features --features ffi,single-precision +``` + +### Step 2: Build the ROS 2 Package with Colcon + +From your ROS 2 workspace root (e.g. `~/ros2_ws`): + +```bash +# Source your ROS 2 distribution +source /opt/ros/humble/setup.bash + +# Option A: Symlink or place this repository in your workspace src/ directory: +# ln -s /path/to/aegis-filter/examples/ros2_adapter src/aegis_ros2_adapter + +# Option B: Build directly pointing to this package directory: +colcon build --packages-select aegis_ros2_adapter --symlink-install +``` + +### Step 3: Source the Workspace Overlay + +```bash +source install/setup.bash +``` + +--- + +## 4. Running the Node + +Run the node with default parameters: + +```bash +ros2 run aegis_ros2_adapter aegis_filter_node +``` + +### Node Parameters + +Parameters can be passed via command line or a YAML configuration file: + +| Parameter | Type | Default | Description | +| :--- | :--- | :--- | :--- | +| `frame_id` | `string` | `"odom"` | Reference frame for published odometry | +| `child_frame_id` | `string` | `"base_link"` | Child frame ID | +| `initial_state` | `double` | `0.0` | Initial state mean ($x_0$) | +| `initial_variance` | `double` | `1.0` | Initial error variance ($P_0 > 0$) | +| `process_noise_q` | `double` | `0.01` | Process noise variance ($Q$) | +| `measurement_noise_r` | `double` | `0.1` | Sensor observation noise variance ($R$) | +| `gate_sigma` | `double` | `3.0` | Chi-square gating threshold ($3.0\sigma$) | +| `max_consecutive_anomalies`| `int` | `5` | Anomalies before quarantine | +| `auto_recovery_threshold` | `int` | `10` | Valid samples required to restore sensor | + +Example running with custom 4.0-sigma gating: + +```bash +ros2 run aegis_ros2_adapter aegis_filter_node --ros-args -p gate_sigma:=4.0 -p measurement_noise_r:=0.05 +``` + +--- + +## 5. Verification & Synthetic Attack Testing + +### Check Subscribed and Published Topics + +```bash +ros2 topic list +``` +Expected output: +```text +/aegis/filtered_odometry +/aegis/sensor_health +/imu/data +/pose/sample +/parameter_events +/rosout +``` + +### Test 1: Ingest Nominal Sensor Data + +Publish valid pose measurements: +```bash +ros2 topic pub /pose/sample geometry_msgs/msg/PoseWithCovarianceStamped \ + "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'odom'}, pose: {pose: {position: {x: 0.0, y: 0.0, z: 1.05}}}}" -1 +``` + +Monitor filtered odometry: +```bash +ros2 topic echo /aegis/filtered_odometry +``` + +### Test 2: Ingest Adversarial Outlier (GPS / LiDAR Spoofing) + +Publish a wild sensor spike ($z = 999.0$ meters): +```bash +ros2 topic pub /pose/sample geometry_msgs/msg/PoseWithCovarianceStamped \ + "{header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'odom'}, pose: {pose: {position: {x: 0.0, y: 0.0, z: 999.0}}}}" -1 +``` + +**Node Console Log**: +```text +[WARN] [aegis_filter_node]: [AEGIS-FDIR GATE] Outlier measurement rejected! Raw value: 999.000 (Total gated: 1) +``` + +The filter **rejects** the measurement without contaminating the state, preserving nominal control continuity. + +--- + +## 6. License + +Dual-licensed under either of: +- Apache License, Version 2.0 ([LICENSE-APACHE](../../LICENSE-APACHE)) +- MIT license ([LICENSE-MIT](../../LICENSE-MIT)) diff --git a/examples/ros2_adapter/include/aegis_ros2_adapter/aegis_node.hpp b/examples/ros2_adapter/include/aegis_ros2_adapter/aegis_node.hpp new file mode 100644 index 0000000..c1ee77a --- /dev/null +++ b/examples/ros2_adapter/include/aegis_ros2_adapter/aegis_node.hpp @@ -0,0 +1,127 @@ +/** + * @file aegis_node.hpp + * @brief High-Integrity ROS 2 C++ Node integrating aegis-filter state estimation. + * @author Exploidus (https://github.com/Exploidus) + * @copyright 2026 Exploidus. Dual-licensed MIT / Apache-2.0. + * + * Implements a zero-allocation, deterministic ROS 2 sensor fusion adapter node. + * Binds directly to aegis-filter's C-FFI entrypoints without dynamic heap + * allocation in the real-time callback loop. + */ + +#ifndef AEGIS_ROS2_ADAPTER__AEGIS_NODE_HPP_ +#define AEGIS_ROS2_ADAPTER__AEGIS_NODE_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/float32.hpp" + +#include "aegis_filter.h" + +namespace aegis_ros2_adapter +{ + +/** + * @brief Sensor IDs registered in the multi-sensor FDIR fusion engine. + */ +enum SensorChannel : uint8_t +{ + SENSOR_PRIMARY_POSE = 1, + SENSOR_SECONDARY_ALTIMETER = 2, +}; + +/** + * @class AegisFilterNode + * @brief ROS 2 Node subscribing to IMU and Pose topics, running aegis-filter FDIR, + * and publishing filtered odometry and health metrics. + */ +class AegisFilterNode : public rclcpp::Node +{ +public: + /** + * @brief Construct a new AegisFilterNode with configurable parameters. + * @param options ROS 2 NodeOptions for composable execution. + */ + explicit AegisFilterNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /** + * @brief Destructor ensuring safe lifecycle teardown. + */ + virtual ~AegisFilterNode() = default; + + /** + * @brief Get count of successfully fused measurements. + */ + uint64_t get_fused_count() const { return fused_count_; } + + /** + * @brief Get count of gated (rejected outlier) measurements. + */ + uint64_t get_gated_count() const { return gated_count_; } + + /** + * @brief Get count of quarantined sensor events. + */ + uint64_t get_quarantine_count() const { return quarantine_count_; } + +private: + /** + * @brief Initialize parameters, publishers, and subscriptions. + */ + void initialize_node(); + + /** + * @brief Initialize filter structs with deterministic, pre-allocated memory. + */ + void initialize_filters(); + + /** + * @brief Callback invoked when raw IMU data arrives (high rate, e.g. 100-500 Hz). + * Performs the state propagation (predict) step. + * @param msg ConstSharedPtr to sensor_msgs::msg::Imu + */ + void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg); + + /** + * @brief Callback invoked when pose / localization data arrives (e.g. 10-50 Hz). + * Performs Chi-square gated update and publishes filtered odometry. + * @param msg ConstSharedPtr to geometry_msgs::msg::PoseWithCovarianceStamped + */ + void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + + // Subscriptions & Publishers + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr health_pub_; + + // Pre-allocated filter memory (Zero heap allocation in real-time execution path) + AegisFusion4 fusion_engine_; + + // Node parameters + std::string frame_id_; + std::string child_frame_id_; + double initial_state_; + double initial_variance_; + double process_noise_q_; + double measurement_noise_r_; + double gate_sigma_; + int max_consecutive_anomalies_; + int auto_recovery_threshold_; + + // Telemetry metrics + uint64_t fused_count_{0}; + uint64_t gated_count_{0}; + uint64_t quarantine_count_{0}; + rclcpp::Time last_imu_time_{0, 0, RCL_ROS_TIME}; + bool filter_initialized_{false}; +}; + +} // namespace aegis_ros2_adapter + +#endif // AEGIS_ROS2_ADAPTER__AEGIS_NODE_HPP_ diff --git a/examples/ros2_adapter/package.xml b/examples/ros2_adapter/package.xml new file mode 100644 index 0000000..e4b97a8 --- /dev/null +++ b/examples/ros2_adapter/package.xml @@ -0,0 +1,32 @@ + + + + aegis_ros2_adapter + 0.4.0 + + High-integrity ROS 2 C++ node adapter integrating the aegis-filter state estimation + and FDIR engine with zero heap allocation in real-time sensor processing pipelines. + + Rahad Bhuiya + MIT + Apache-2.0 + + https://github.com/Exploidus/aegis-filter + https://github.com/Exploidus/aegis-filter + https://github.com/Exploidus/aegis-filter/issues + + ament_cmake + + rclcpp + sensor_msgs + geometry_msgs + nav_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/examples/ros2_adapter/src/aegis_node.cpp b/examples/ros2_adapter/src/aegis_node.cpp new file mode 100644 index 0000000..7e35084 --- /dev/null +++ b/examples/ros2_adapter/src/aegis_node.cpp @@ -0,0 +1,234 @@ +/** + * @file aegis_node.cpp + * @brief Implementation of high-integrity ROS 2 C++ Node for aegis-filter. + * @author Exploidus (https://github.com/Exploidus) + * @copyright 2026 Exploidus. Dual-licensed MIT / Apache-2.0. + */ + +#include "aegis_ros2_adapter/aegis_node.hpp" + +#include + +namespace aegis_ros2_adapter +{ + +AegisFilterNode::AegisFilterNode(const rclcpp::NodeOptions & options) +: Node("aegis_filter_node", options) +{ + initialize_node(); + initialize_filters(); + + RCLCPP_INFO( + this->get_logger(), + "Aegis-Filter ROS 2 Node initialized. Zero-allocation FDIR pipeline active (Gate: %.1f-sigma).", + gate_sigma_); +} + +void AegisFilterNode::initialize_node() +{ + // Declare parameters with safe fail-closed defaults + this->declare_parameter("frame_id", "odom"); + this->declare_parameter("child_frame_id", "base_link"); + this->declare_parameter("initial_state", 0.0); + this->declare_parameter("initial_variance", 1.0); + this->declare_parameter("process_noise_q", 0.01); + this->declare_parameter("measurement_noise_r", 0.1); + this->declare_parameter("gate_sigma", 3.0); + this->declare_parameter("max_consecutive_anomalies", 5); + this->declare_parameter("auto_recovery_threshold", 10); + + // Retrieve parameters + frame_id_ = this->get_parameter("frame_id").as_string(); + child_frame_id_ = this->get_parameter("child_frame_id").as_string(); + initial_state_ = this->get_parameter("initial_state").as_double(); + initial_variance_ = this->get_parameter("initial_variance").as_double(); + process_noise_q_ = this->get_parameter("process_noise_q").as_double(); + measurement_noise_r_ = this->get_parameter("measurement_noise_r").as_double(); + gate_sigma_ = this->get_parameter("gate_sigma").as_double(); + max_consecutive_anomalies_ = this->get_parameter("max_consecutive_anomalies").as_int(); + auto_recovery_threshold_ = this->get_parameter("auto_recovery_threshold").as_int(); + + // Create real-time sensor subscriptions + rclcpp::QoS qos_sensor = rclcpp::SensorDataQoS(); + + imu_sub_ = this->create_subscription( + "/imu/data", + qos_sensor, + std::bind(&AegisFilterNode::imu_callback, this, std::placeholders::_1)); + + pose_sub_ = this->create_subscription( + "/pose/sample", + qos_sensor, + std::bind(&AegisFilterNode::pose_callback, this, std::placeholders::_1)); + + // Create output publishers + odom_pub_ = this->create_publisher( + "/aegis/filtered_odometry", + rclcpp::SystemDefaultsQoS()); + + health_pub_ = this->create_publisher( + "/aegis/sensor_health", + rclcpp::SystemDefaultsQoS()); +} + +void AegisFilterNode::initialize_filters() +{ + // Zero-allocation initialization of 4-channel Multi-Sensor Fusion FDIR engine + AegisStatusCode rc = aegis_fusion4_init( + &fusion_engine_, + static_cast(initial_state_), + static_cast(initial_variance_), + static_cast(process_noise_q_)); + + if (rc != AEGIS_OK) { + RCLCPP_FATAL(this->get_logger(), "Failed to initialize aegis_fusion4 engine! Error code: %d", rc); + throw std::runtime_error("Aegis-Filter initialization failed"); + } + + // Register primary pose/altimeter sensor with Chi-square innovation gating + rc = aegis_fusion4_register_sensor( + &fusion_engine_, + SENSOR_PRIMARY_POSE, + static_cast(measurement_noise_r_), + static_cast(max_consecutive_anomalies_), + static_cast(gate_sigma_), + static_cast(auto_recovery_threshold_)); + + if (rc != AEGIS_OK) { + RCLCPP_FATAL(this->get_logger(), "Failed to register primary sensor! Error code: %d", rc); + throw std::runtime_error("Sensor registration failed"); + } + + filter_initialized_ = true; +} + +void AegisFilterNode::imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) +{ + if (!filter_initialized_) { + return; + } + + rclcpp::Time current_time = msg->header.stamp; + + // Calculate dynamic delta-t for prediction + if (last_imu_time_.nanoseconds() > 0) { + double dt = (current_time - last_imu_time_).seconds(); + if (dt > 0.0 && dt < 1.0) { + // Ingest vertical/linear acceleration as control input + double accel_z = msg->linear_acceleration.z; + aegis_real control_u = static_cast(accel_z * dt); + + // Perform state propagation via C-FFI + AegisStatusCode rc = aegis_fusion4_predict(&fusion_engine_, control_u); + if (rc != AEGIS_OK) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), + *this->get_clock(), + 2000, + "Aegis filter predict error: %d", rc); + } + } + } + + last_imu_time_ = current_time; +} + +void AegisFilterNode::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + if (!filter_initialized_) { + return; + } + + // Extract observation (e.g. altitude or position along measurement axis) + aegis_real raw_measurement = static_cast(msg->pose.pose.position.z); + AegisFusionStatusCode fusion_status = AEGIS_FUSION_FUSED; + + // Execute zero-copy Joseph-stabilized measurement update with FDIR gating + AegisStatusCode rc = aegis_fusion4_fuse( + &fusion_engine_, + SENSOR_PRIMARY_POSE, + raw_measurement, + &fusion_status); + + if (rc != AEGIS_OK) { + RCLCPP_ERROR_THROTTLE( + this->get_logger(), + *this->get_clock(), + 2000, + "Aegis fusion FFI call error: %d", rc); + return; + } + + // Handle FDIR outcomes + switch (fusion_status) { + case AEGIS_FUSION_FUSED: + fused_count_++; + break; + + case AEGIS_FUSION_GATED: + gated_count_++; + RCLCPP_WARN_THROTTLE( + this->get_logger(), + *this->get_clock(), + 1000, + "[AEGIS-FDIR GATE] Outlier measurement rejected! Raw value: %.3f (Total gated: %lu)", + raw_measurement, gated_count_); + break; + + case AEGIS_FUSION_QUARANTINED: + quarantine_count_++; + RCLCPP_ERROR( + this->get_logger(), + "[AEGIS-FDIR QUARANTINE] Sensor %u breached anomaly threshold! Quarantined from state.", + SENSOR_PRIMARY_POSE); + break; + + case AEGIS_FUSION_IGNORED_QUARANTINED: + RCLCPP_WARN_THROTTLE( + this->get_logger(), + *this->get_clock(), + 5000, + "[AEGIS-FDIR] Ingest ignored for quarantined sensor %u.", + SENSOR_PRIMARY_POSE); + break; + + case AEGIS_FUSION_RECOVERED: + RCLCPP_INFO( + this->get_logger(), + "[AEGIS-FDIR RECOVERY] Sensor %u passed consecutive valid sample gate. Restored to nominal.", + SENSOR_PRIMARY_POSE); + break; + + default: + break; + } + + // Read filtered state and covariance + aegis_real filtered_state = aegis_fusion4_get_state(&fusion_engine_); + aegis_real filtered_variance = aegis_fusion4_get_variance(&fusion_engine_); + aegis_real sensor_health = aegis_fusion4_get_sensor_health(&fusion_engine_, SENSOR_PRIMARY_POSE); + + // Construct and publish filtered Odometry message + nav_msgs::msg::Odometry odom_msg; + odom_msg.header.stamp = msg->header.stamp; + odom_msg.header.frame_id = frame_id_; + odom_msg.child_frame_id = child_frame_id_; + + // Set position (preserve x and y from pose, replace z with high-integrity estimate) + odom_msg.pose.pose.position.x = msg->pose.pose.position.x; + odom_msg.pose.pose.position.y = msg->pose.pose.position.y; + odom_msg.pose.pose.position.z = static_cast(filtered_state); + odom_msg.pose.pose.orientation = msg->pose.pose.orientation; + + // Fill diagonal covariance (row-major 6x6 matrix, index 14 is z-variance) + odom_msg.pose.covariance[14] = static_cast(filtered_variance); + + odom_pub_->publish(odom_msg); + + // Publish real-time sensor health metric + std_msgs::msg::Float32 health_msg; + health_msg.data = static_cast(sensor_health); + health_pub_->publish(health_msg); +} + +} // namespace aegis_ros2_adapter diff --git a/examples/ros2_adapter/src/main.cpp b/examples/ros2_adapter/src/main.cpp new file mode 100644 index 0000000..d75d8d9 --- /dev/null +++ b/examples/ros2_adapter/src/main.cpp @@ -0,0 +1,26 @@ +/** + * @file main.cpp + * @brief Executable entrypoint for the aegis_ros2_adapter node. + * @author Exploidus (https://github.com/Exploidus) + * @copyright 2026 Exploidus. Dual-licensed MIT / Apache-2.0. + */ + +#include "aegis_ros2_adapter/aegis_node.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + try { + auto node = std::make_shared(); + rclcpp::spin(node); + } catch (const std::exception & e) { + RCLCPP_FATAL(rclcpp::get_logger("aegis_main"), "Exception caught: %s", e.what()); + rclcpp::shutdown(); + return 1; + } + + rclcpp::shutdown(); + return 0; +}