From dbd671719f23c3f8adeedf284a14940f7e994338 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 12:20:08 +0000 Subject: [PATCH 01/19] feat(sensor-diagnostics): add project skeleton and LiDAR simulator node Add lightweight sensor diagnostics demo (no Gazebo required): - CMakeLists.txt and package.xml for ROS 2 package - Simulated sensor base class with fault injection support - LiDAR simulator node with configurable parameters: - scan_rate, range_min/max, noise_stddev - Runtime fault injection via parameters - Diagnostics publisher for sensor status This is the foundation for the sensor diagnostics demo that demonstrates ros2_medkit's data monitoring and configuration management features. --- demos/sensor_diagnostics/CMakeLists.txt | 82 +++++ .../simulated_sensor_base.hpp | 71 +++++ demos/sensor_diagnostics/package.xml | 29 ++ .../sensor_diagnostics/src/lidar_sim_node.cpp | 289 ++++++++++++++++++ 4 files changed, 471 insertions(+) create mode 100644 demos/sensor_diagnostics/CMakeLists.txt create mode 100644 demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp create mode 100644 demos/sensor_diagnostics/package.xml create mode 100644 demos/sensor_diagnostics/src/lidar_sim_node.cpp diff --git a/demos/sensor_diagnostics/CMakeLists.txt b/demos/sensor_diagnostics/CMakeLists.txt new file mode 100644 index 0000000..e1ec131 --- /dev/null +++ b/demos/sensor_diagnostics/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.8) +project(sensor_diagnostics_demo) + +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(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(rcl_interfaces REQUIRED) + +# LiDAR simulator node +add_executable(lidar_sim_node src/lidar_sim_node.cpp) +ament_target_dependencies(lidar_sim_node + rclcpp + sensor_msgs + diagnostic_msgs +) + +# IMU simulator node +add_executable(imu_sim_node src/imu_sim_node.cpp) +ament_target_dependencies(imu_sim_node + rclcpp + sensor_msgs + diagnostic_msgs +) + +# GPS simulator node +add_executable(gps_sim_node src/gps_sim_node.cpp) +ament_target_dependencies(gps_sim_node + rclcpp + sensor_msgs + diagnostic_msgs +) + +# Camera simulator node +add_executable(camera_sim_node src/camera_sim_node.cpp) +ament_target_dependencies(camera_sim_node + rclcpp + sensor_msgs + diagnostic_msgs +) + +# Anomaly detector node +add_executable(anomaly_detector_node src/anomaly_detector_node.cpp) +ament_target_dependencies(anomaly_detector_node + rclcpp + sensor_msgs + diagnostic_msgs +) + +# Install executables +install(TARGETS + lidar_sim_node + imu_sim_node + gps_sim_node + camera_sim_node + anomaly_detector_node + DESTINATION lib/${PROJECT_NAME} +) + +# Install launch files +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +# Install config files +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp b/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp new file mode 100644 index 0000000..957559e --- /dev/null +++ b/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp @@ -0,0 +1,71 @@ +// Copyright 2025 selfpatch +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace sensor_diagnostics +{ + +/// Configuration for fault injection in simulated sensors +struct FaultConfig +{ + double failure_probability{0.0}; ///< Probability of complete failure (0.0-1.0) + bool inject_nan{false}; ///< Inject NaN values + double noise_multiplier{1.0}; ///< Multiplier for noise (>1 = degraded) + double drift_rate{0.0}; ///< Drift rate per second +}; + +/// Base class for simulated sensors with fault injection +class SimulatedSensorBase +{ +public: + SimulatedSensorBase() + : rng_(std::random_device{}()), uniform_dist_(0.0, 1.0), normal_dist_(0.0, 1.0) + { + } + + virtual ~SimulatedSensorBase() = default; + + /// Check if sensor should fail this cycle + bool should_fail() const + { + return uniform_dist_(rng_) < config_.failure_probability; + } + + /// Check if NaN values should be injected + bool should_inject_nan() const { return config_.inject_nan; } + + /// Get noise value scaled by noise_multiplier + double get_noise(double base_stddev) + { + return normal_dist_(rng_) * base_stddev * config_.noise_multiplier; + } + + /// Get accumulated drift value + double get_drift(double elapsed_seconds) const + { + return config_.drift_rate * elapsed_seconds; + } + + /// Generate uniform random value in range [min, max] + double get_uniform(double min, double max) + { + return min + uniform_dist_(rng_) * (max - min); + } + + /// Update fault configuration + void set_fault_config(const FaultConfig & config) { config_ = config; } + + /// Get current fault configuration + const FaultConfig & get_fault_config() const { return config_; } + +protected: + FaultConfig config_; + mutable std::mt19937 rng_; + mutable std::uniform_real_distribution uniform_dist_; + mutable std::normal_distribution normal_dist_; +}; + +} // namespace sensor_diagnostics diff --git a/demos/sensor_diagnostics/package.xml b/demos/sensor_diagnostics/package.xml new file mode 100644 index 0000000..0fe46d4 --- /dev/null +++ b/demos/sensor_diagnostics/package.xml @@ -0,0 +1,29 @@ + + + + sensor_diagnostics_demo + 0.1.0 + Lightweight sensor diagnostics demo for ros2_medkit (no Gazebo required) + Demo Maintainer + Apache-2.0 + + ament_cmake + + rclcpp + std_msgs + sensor_msgs + geometry_msgs + diagnostic_msgs + rcl_interfaces + + ros2launch + ros2_medkit_gateway + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/demos/sensor_diagnostics/src/lidar_sim_node.cpp b/demos/sensor_diagnostics/src/lidar_sim_node.cpp new file mode 100644 index 0000000..da1da7d --- /dev/null +++ b/demos/sensor_diagnostics/src/lidar_sim_node.cpp @@ -0,0 +1,289 @@ +// Copyright 2025 selfpatch +// SPDX-License-Identifier: Apache-2.0 + +/// @file lidar_sim_node.cpp +/// @brief Simulated LiDAR sensor with configurable fault injection +/// +/// This node publishes simulated LaserScan messages with realistic patterns +/// and supports runtime fault injection via ROS 2 parameters. + +#include +#include +#include +#include +#include +#include +#include + +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_msgs/msg/key_value.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" + +namespace sensor_diagnostics +{ + +class LidarSimNode : public rclcpp::Node +{ +public: + LidarSimNode() + : Node("lidar_sim"), + rng_(std::random_device{}()), + normal_dist_(0.0, 1.0), + uniform_dist_(0.0, 1.0), + start_time_(this->now()) + { + // Declare parameters with defaults + this->declare_parameter("scan_rate", 10.0); // Hz + this->declare_parameter("range_min", 0.12); // meters + this->declare_parameter("range_max", 3.5); // meters + this->declare_parameter("angle_min", -3.14159); // radians + this->declare_parameter("angle_max", 3.14159); // radians + this->declare_parameter("num_readings", 360); // number of laser beams + this->declare_parameter("noise_stddev", 0.01); // meters + this->declare_parameter("failure_probability", 0.0); // 0.0 - 1.0 + this->declare_parameter("inject_nan", false); + this->declare_parameter("drift_rate", 0.0); // meters per second + + // Load parameters + load_parameters(); + + // Create publisher + scan_pub_ = this->create_publisher("scan", 10); + diag_pub_ = this->create_publisher( + "diagnostics", 10); + + // Calculate publish period from rate + double rate = this->get_parameter("scan_rate").as_double(); + auto period = std::chrono::duration(1.0 / rate); + + // Create timer + timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&LidarSimNode::publish_scan, this)); + + // Register parameter callback for runtime reconfiguration + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&LidarSimNode::on_parameter_change, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "LiDAR simulator started at %.1f Hz", rate); + } + +private: + void load_parameters() + { + range_min_ = this->get_parameter("range_min").as_double(); + range_max_ = this->get_parameter("range_max").as_double(); + angle_min_ = this->get_parameter("angle_min").as_double(); + angle_max_ = this->get_parameter("angle_max").as_double(); + num_readings_ = this->get_parameter("num_readings").as_int(); + noise_stddev_ = this->get_parameter("noise_stddev").as_double(); + failure_probability_ = this->get_parameter("failure_probability").as_double(); + inject_nan_ = this->get_parameter("inject_nan").as_bool(); + drift_rate_ = this->get_parameter("drift_rate").as_double(); + } + + rcl_interfaces::msg::SetParametersResult on_parameter_change( + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + if (param.get_name() == "noise_stddev") { + noise_stddev_ = param.as_double(); + RCLCPP_INFO(this->get_logger(), "Noise stddev changed to %.4f", noise_stddev_); + } else if (param.get_name() == "failure_probability") { + failure_probability_ = param.as_double(); + RCLCPP_INFO( + this->get_logger(), "Failure probability changed to %.2f", + failure_probability_); + } else if (param.get_name() == "inject_nan") { + inject_nan_ = param.as_bool(); + RCLCPP_INFO( + this->get_logger(), "Inject NaN %s", + inject_nan_ ? "enabled" : "disabled"); + } else if (param.get_name() == "drift_rate") { + drift_rate_ = param.as_double(); + RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f", drift_rate_); + } else if (param.get_name() == "scan_rate") { + // Update timer with new rate + double rate = param.as_double(); + auto period = std::chrono::duration(1.0 / rate); + timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&LidarSimNode::publish_scan, this)); + RCLCPP_INFO(this->get_logger(), "Scan rate changed to %.1f Hz", rate); + } + } + + return result; + } + + void publish_scan() + { + msg_count_++; + + // Check for complete failure + if (uniform_dist_(rng_) < failure_probability_) { + publish_diagnostics("TIMEOUT", "Sensor failure (injected)"); + return; // Don't publish - simulates timeout + } + + auto msg = sensor_msgs::msg::LaserScan(); + msg.header.stamp = this->now(); + msg.header.frame_id = "lidar_link"; + + msg.angle_min = angle_min_; + msg.angle_max = angle_max_; + msg.angle_increment = (angle_max_ - angle_min_) / static_cast(num_readings_); + msg.time_increment = 0.0; + msg.scan_time = 1.0 / this->get_parameter("scan_rate").as_double(); + msg.range_min = range_min_; + msg.range_max = range_max_; + + // Calculate drift offset + double elapsed = (this->now() - start_time_).seconds(); + double drift_offset = drift_rate_ * elapsed; + + // Generate simulated ranges with noise + msg.ranges.resize(num_readings_); + msg.intensities.resize(num_readings_); + + int nan_count = 0; + for (int i = 0; i < num_readings_; i++) { + // Base simulated pattern - circular room with some objects + double angle = angle_min_ + i * msg.angle_increment; + double base_range = generate_simulated_range(angle); + + // Apply drift + base_range += drift_offset; + + // Add noise + double noise = normal_dist_(rng_) * noise_stddev_; + double range = base_range + noise; + + // Clamp to valid range + range = std::clamp(range, range_min_, range_max_); + + // Inject NaN if configured + if (inject_nan_ && uniform_dist_(rng_) < 0.05) { // 5% NaN rate when enabled + range = std::numeric_limits::quiet_NaN(); + nan_count++; + } + + msg.ranges[i] = static_cast(range); + msg.intensities[i] = static_cast(100.0 * (1.0 - range / range_max_)); + } + + scan_pub_->publish(msg); + + // Publish diagnostics + if (nan_count > 0) { + publish_diagnostics("NAN_VALUES", "NaN values detected: " + std::to_string(nan_count)); + } else if (noise_stddev_ > 0.1) { + publish_diagnostics("HIGH_NOISE", "Noise stddev: " + std::to_string(noise_stddev_)); + } else if (drift_offset > 0.1) { + publish_diagnostics("DRIFTING", "Drift: " + std::to_string(drift_offset) + "m"); + } else { + publish_diagnostics("OK", "Operating normally"); + } + } + + double generate_simulated_range(double angle) + { + // Simulate a room with walls at ~2.5m and some objects + double room_distance = 2.5; + + // Add some "objects" at fixed angles + if (angle > 0.5 && angle < 0.8) { + return 1.2; // Object on the right + } + if (angle > -1.2 && angle < -0.9) { + return 1.8; // Object on the left + } + if (angle > 2.8 || angle < -2.8) { + return 1.5; // Object behind + } + + // Default room walls + return room_distance + normal_dist_(rng_) * 0.05; + } + + void publish_diagnostics(const std::string & status, const std::string & message) + { + auto diag = diagnostic_msgs::msg::DiagnosticStatus(); + diag.name = "lidar_sim"; + + if (status == "OK") { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } else if (status == "HIGH_NOISE" || status == "DRIFTING") { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } else { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + + diag.message = message; + + // Add key-value pairs + diagnostic_msgs::msg::KeyValue kv; + kv.key = "status"; + kv.value = status; + diag.values.push_back(kv); + + kv.key = "msg_count"; + kv.value = std::to_string(msg_count_); + diag.values.push_back(kv); + + kv.key = "noise_stddev"; + kv.value = std::to_string(noise_stddev_); + diag.values.push_back(kv); + + kv.key = "failure_probability"; + kv.value = std::to_string(failure_probability_); + diag.values.push_back(kv); + + diag_pub_->publish(diag); + } + + // Publishers + rclcpp::Publisher::SharedPtr scan_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Parameter callback handle + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + // Random number generation + std::mt19937 rng_; + std::normal_distribution normal_dist_; + std::uniform_real_distribution uniform_dist_; + + // Parameters + double range_min_; + double range_max_; + double angle_min_; + double angle_max_; + int num_readings_; + double noise_stddev_; + double failure_probability_; + bool inject_nan_; + double drift_rate_; + + // Statistics + rclcpp::Time start_time_; + uint64_t msg_count_{0}; +}; + +} // namespace sensor_diagnostics + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From b72d5a5c353219427e726d8480db92aa1f066722 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 12:21:23 +0000 Subject: [PATCH 02/19] feat(sensor-diagnostics): add IMU, GPS, and camera simulator nodes Add remaining simulated sensor nodes with fault injection: - IMU simulator: acceleration, angular velocity, drift simulation - GPS simulator: NavSatFix with position noise and signal loss - Camera simulator: RGB image generation with noise/black frames All sensors support runtime parameter changes for fault injection: - failure_probability for timeouts - inject_nan for invalid values - drift_rate for gradual sensor drift - noise parameters for degraded operation --- .../src/camera_sim_node.cpp | 267 ++++++++++++++++++ demos/sensor_diagnostics/src/gps_sim_node.cpp | 253 +++++++++++++++++ demos/sensor_diagnostics/src/imu_sim_node.cpp | 228 +++++++++++++++ 3 files changed, 748 insertions(+) create mode 100644 demos/sensor_diagnostics/src/camera_sim_node.cpp create mode 100644 demos/sensor_diagnostics/src/gps_sim_node.cpp create mode 100644 demos/sensor_diagnostics/src/imu_sim_node.cpp diff --git a/demos/sensor_diagnostics/src/camera_sim_node.cpp b/demos/sensor_diagnostics/src/camera_sim_node.cpp new file mode 100644 index 0000000..815cfaa --- /dev/null +++ b/demos/sensor_diagnostics/src/camera_sim_node.cpp @@ -0,0 +1,267 @@ +// Copyright 2025 selfpatch +// SPDX-License-Identifier: Apache-2.0 + +/// @file camera_sim_node.cpp +/// @brief Simulated camera sensor with configurable fault injection +/// +/// Publishes simulated Image and CameraInfo messages +/// and supports runtime fault injection via ROS 2 parameters. + +#include +#include +#include +#include +#include + +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_msgs/msg/key_value.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" + +namespace sensor_diagnostics +{ + +class CameraSimNode : public rclcpp::Node +{ +public: + CameraSimNode() + : Node("camera_sim"), + rng_(std::random_device{}()), + normal_dist_(0.0, 1.0), + uniform_dist_(0.0, 1.0) + { + // Declare parameters + this->declare_parameter("rate", 30.0); // Hz + this->declare_parameter("width", 640); // pixels + this->declare_parameter("height", 480); // pixels + this->declare_parameter("noise_level", 0.0); // 0.0 - 1.0 (fraction of pixels with noise) + this->declare_parameter("failure_probability", 0.0); + this->declare_parameter("inject_black_frames", false); + this->declare_parameter("brightness", 128); // 0-255 base brightness + + load_parameters(); + + // Create publishers + image_pub_ = this->create_publisher("image_raw", 10); + info_pub_ = this->create_publisher("camera_info", 10); + diag_pub_ = this->create_publisher( + "diagnostics", 10); + + // Create timer + double rate = this->get_parameter("rate").as_double(); + auto period = std::chrono::duration(1.0 / rate); + timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&CameraSimNode::publish_image, this)); + + // Register parameter callback + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&CameraSimNode::on_parameter_change, this, std::placeholders::_1)); + + RCLCPP_INFO( + this->get_logger(), "Camera simulator started at %.1f Hz (%dx%d)", + rate, width_, height_); + } + +private: + void load_parameters() + { + width_ = this->get_parameter("width").as_int(); + height_ = this->get_parameter("height").as_int(); + noise_level_ = this->get_parameter("noise_level").as_double(); + failure_probability_ = this->get_parameter("failure_probability").as_double(); + inject_black_frames_ = this->get_parameter("inject_black_frames").as_bool(); + brightness_ = this->get_parameter("brightness").as_int(); + } + + rcl_interfaces::msg::SetParametersResult on_parameter_change( + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + if (param.get_name() == "noise_level") { + noise_level_ = param.as_double(); + RCLCPP_INFO(this->get_logger(), "Noise level changed to %.2f", noise_level_); + } else if (param.get_name() == "failure_probability") { + failure_probability_ = param.as_double(); + RCLCPP_INFO( + this->get_logger(), "Failure probability changed to %.2f", + failure_probability_); + } else if (param.get_name() == "inject_black_frames") { + inject_black_frames_ = param.as_bool(); + RCLCPP_INFO( + this->get_logger(), "Black frames %s", + inject_black_frames_ ? "enabled" : "disabled"); + } else if (param.get_name() == "brightness") { + brightness_ = param.as_int(); + RCLCPP_INFO(this->get_logger(), "Brightness changed to %d", brightness_); + } + } + + return result; + } + + void publish_image() + { + msg_count_++; + + // Check for complete failure + if (uniform_dist_(rng_) < failure_probability_) { + publish_diagnostics("TIMEOUT", "Camera failure (injected)"); + return; + } + + auto image = sensor_msgs::msg::Image(); + image.header.stamp = this->now(); + image.header.frame_id = "camera_link"; + + image.width = width_; + image.height = height_; + image.encoding = "rgb8"; + image.is_bigendian = false; + image.step = width_ * 3; // 3 bytes per pixel (RGB) + + // Generate image data + size_t data_size = static_cast(width_ * height_ * 3); + image.data.resize(data_size); + + bool is_black_frame = inject_black_frames_ && uniform_dist_(rng_) < 0.1; + + if (is_black_frame) { + // All black frame + std::fill(image.data.begin(), image.data.end(), 0); + publish_diagnostics("BLACK_FRAME", "Black frame detected"); + } else { + // Generate gradient pattern with noise + for (int y = 0; y < height_; y++) { + for (int x = 0; x < width_; x++) { + size_t idx = static_cast((y * width_ + x) * 3); + + // Base gradient pattern + uint8_t r = static_cast(std::clamp(brightness_ + x / 5, 0, 255)); + uint8_t g = static_cast(std::clamp(brightness_ + y / 4, 0, 255)); + uint8_t b = static_cast(std::clamp(brightness_, 0, 255)); + + // Add noise + if (uniform_dist_(rng_) < noise_level_) { + r = static_cast(uniform_dist_(rng_) * 255); + g = static_cast(uniform_dist_(rng_) * 255); + b = static_cast(uniform_dist_(rng_) * 255); + } + + image.data[idx] = r; + image.data[idx + 1] = g; + image.data[idx + 2] = b; + } + } + + if (noise_level_ > 0.1) { + publish_diagnostics("HIGH_NOISE", "High noise level: " + std::to_string(noise_level_)); + } else if (brightness_ < 30) { + publish_diagnostics("LOW_BRIGHTNESS", "Image too dark"); + } else if (brightness_ > 225) { + publish_diagnostics("OVEREXPOSED", "Image overexposed"); + } else { + publish_diagnostics("OK", "Operating normally"); + } + } + + image_pub_->publish(image); + + // Publish camera info + auto info = sensor_msgs::msg::CameraInfo(); + info.header = image.header; + info.width = width_; + info.height = height_; + + // Simple pinhole camera model + double fx = 500.0; // focal length x + double fy = 500.0; // focal length y + double cx = width_ / 2.0; // principal point x + double cy = height_ / 2.0; // principal point y + + info.k = {fx, 0, cx, 0, fy, cy, 0, 0, 1}; + info.r = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + info.p = {fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0}; + info.distortion_model = "plumb_bob"; + info.d = {0, 0, 0, 0, 0}; // No distortion + + info_pub_->publish(info); + } + + void publish_diagnostics(const std::string & status, const std::string & message) + { + auto diag = diagnostic_msgs::msg::DiagnosticStatus(); + diag.name = "camera_sim"; + + if (status == "OK") { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } else if (status == "HIGH_NOISE" || status == "LOW_BRIGHTNESS" || status == "OVEREXPOSED") { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } else { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + + diag.message = message; + + diagnostic_msgs::msg::KeyValue kv; + kv.key = "status"; + kv.value = status; + diag.values.push_back(kv); + + kv.key = "msg_count"; + kv.value = std::to_string(msg_count_); + diag.values.push_back(kv); + + kv.key = "resolution"; + kv.value = std::to_string(width_) + "x" + std::to_string(height_); + diag.values.push_back(kv); + + kv.key = "noise_level"; + kv.value = std::to_string(noise_level_); + diag.values.push_back(kv); + + diag_pub_->publish(diag); + } + + // Publishers + rclcpp::Publisher::SharedPtr image_pub_; + rclcpp::Publisher::SharedPtr info_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Parameter callback + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + // Random number generation + std::mt19937 rng_; + std::normal_distribution normal_dist_; + std::uniform_real_distribution uniform_dist_; + + // Parameters + int width_; + int height_; + double noise_level_; + double failure_probability_; + bool inject_black_frames_; + int brightness_; + + // Statistics + uint64_t msg_count_{0}; +}; + +} // namespace sensor_diagnostics + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/demos/sensor_diagnostics/src/gps_sim_node.cpp b/demos/sensor_diagnostics/src/gps_sim_node.cpp new file mode 100644 index 0000000..4859dcf --- /dev/null +++ b/demos/sensor_diagnostics/src/gps_sim_node.cpp @@ -0,0 +1,253 @@ +// Copyright 2025 selfpatch +// SPDX-License-Identifier: Apache-2.0 + +/// @file gps_sim_node.cpp +/// @brief Simulated GPS sensor with configurable fault injection +/// +/// Publishes simulated NavSatFix messages with realistic GPS patterns +/// and supports runtime fault injection via ROS 2 parameters. + +#include +#include +#include +#include +#include + +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_msgs/msg/key_value.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" +#include "sensor_msgs/msg/nav_sat_status.hpp" + +namespace sensor_diagnostics +{ + +class GpsSimNode : public rclcpp::Node +{ +public: + GpsSimNode() + : Node("gps_sim"), + rng_(std::random_device{}()), + normal_dist_(0.0, 1.0), + uniform_dist_(0.0, 1.0), + start_time_(this->now()) + { + // Declare parameters + this->declare_parameter("rate", 1.0); // Hz (GPS typically 1-10 Hz) + this->declare_parameter("base_latitude", 52.2297); // Warsaw, Poland + this->declare_parameter("base_longitude", 21.0122); + this->declare_parameter("base_altitude", 100.0); // meters + this->declare_parameter("position_noise_stddev", 2.0); // meters (CEP) + this->declare_parameter("altitude_noise_stddev", 5.0); // meters + this->declare_parameter("failure_probability", 0.0); + this->declare_parameter("inject_nan", false); + this->declare_parameter("drift_rate", 0.0); // meters/second + + load_parameters(); + + // Create publishers + fix_pub_ = this->create_publisher("fix", 10); + diag_pub_ = this->create_publisher( + "diagnostics", 10); + + // Create timer + double rate = this->get_parameter("rate").as_double(); + auto period = std::chrono::duration(1.0 / rate); + timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&GpsSimNode::publish_fix, this)); + + // Register parameter callback + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&GpsSimNode::on_parameter_change, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "GPS simulator started at %.1f Hz", rate); + } + +private: + void load_parameters() + { + base_latitude_ = this->get_parameter("base_latitude").as_double(); + base_longitude_ = this->get_parameter("base_longitude").as_double(); + base_altitude_ = this->get_parameter("base_altitude").as_double(); + position_noise_stddev_ = this->get_parameter("position_noise_stddev").as_double(); + altitude_noise_stddev_ = this->get_parameter("altitude_noise_stddev").as_double(); + failure_probability_ = this->get_parameter("failure_probability").as_double(); + inject_nan_ = this->get_parameter("inject_nan").as_bool(); + drift_rate_ = this->get_parameter("drift_rate").as_double(); + } + + rcl_interfaces::msg::SetParametersResult on_parameter_change( + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + if (param.get_name() == "position_noise_stddev") { + position_noise_stddev_ = param.as_double(); + RCLCPP_INFO( + this->get_logger(), "Position noise changed to %.2f m", + position_noise_stddev_); + } else if (param.get_name() == "altitude_noise_stddev") { + altitude_noise_stddev_ = param.as_double(); + RCLCPP_INFO( + this->get_logger(), "Altitude noise changed to %.2f m", + altitude_noise_stddev_); + } else if (param.get_name() == "failure_probability") { + failure_probability_ = param.as_double(); + RCLCPP_INFO( + this->get_logger(), "Failure probability changed to %.2f", + failure_probability_); + } else if (param.get_name() == "inject_nan") { + inject_nan_ = param.as_bool(); + RCLCPP_INFO( + this->get_logger(), "Inject NaN %s", + inject_nan_ ? "enabled" : "disabled"); + } else if (param.get_name() == "drift_rate") { + drift_rate_ = param.as_double(); + RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f", drift_rate_); + } + } + + return result; + } + + void publish_fix() + { + msg_count_++; + + // Check for complete failure (no satellite fix) + if (uniform_dist_(rng_) < failure_probability_) { + // Publish message with NO_FIX status + auto msg = sensor_msgs::msg::NavSatFix(); + msg.header.stamp = this->now(); + msg.header.frame_id = "gps_link"; + msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_NO_FIX; + msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + fix_pub_->publish(msg); + publish_diagnostics("NO_FIX", "GPS signal lost (injected)"); + return; + } + + auto msg = sensor_msgs::msg::NavSatFix(); + msg.header.stamp = this->now(); + msg.header.frame_id = "gps_link"; + + // GPS status - normal fix + msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; + msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; + + // Calculate elapsed time for drift + double elapsed = (this->now() - start_time_).seconds(); + double drift_offset = drift_rate_ * elapsed; + + // Convert drift from meters to degrees (approximately) + // 1 degree latitude ≈ 111,000 meters + // 1 degree longitude varies with latitude + double lat_drift = drift_offset / 111000.0; + double lon_drift = drift_offset / (111000.0 * std::cos(base_latitude_ * M_PI / 180.0)); + + // Add noise to position (in meters, converted to degrees) + double lat_noise = (normal_dist_(rng_) * position_noise_stddev_) / 111000.0; + double lon_noise = (normal_dist_(rng_) * position_noise_stddev_) / + (111000.0 * std::cos(base_latitude_ * M_PI / 180.0)); + double alt_noise = normal_dist_(rng_) * altitude_noise_stddev_; + + msg.latitude = base_latitude_ + lat_drift + lat_noise; + msg.longitude = base_longitude_ + lon_drift + lon_noise; + msg.altitude = base_altitude_ + alt_noise; + + // Covariance (diagonal, in m^2) + msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + msg.position_covariance[0] = position_noise_stddev_ * position_noise_stddev_; + msg.position_covariance[4] = position_noise_stddev_ * position_noise_stddev_; + msg.position_covariance[8] = altitude_noise_stddev_ * altitude_noise_stddev_; + + // Inject NaN if configured + if (inject_nan_ && uniform_dist_(rng_) < 0.05) { + msg.latitude = std::numeric_limits::quiet_NaN(); + publish_diagnostics("NAN_VALUES", "Invalid GPS coordinates"); + } else if (drift_offset > 5.0) { + publish_diagnostics("DRIFTING", "Position drift: " + std::to_string(drift_offset) + "m"); + } else if (position_noise_stddev_ > 10.0) { + publish_diagnostics("LOW_ACCURACY", "High position noise: " + + std::to_string(position_noise_stddev_) + "m"); + } else { + publish_diagnostics("OK", "GPS fix acquired"); + } + + fix_pub_->publish(msg); + } + + void publish_diagnostics(const std::string & status, const std::string & message) + { + auto diag = diagnostic_msgs::msg::DiagnosticStatus(); + diag.name = "gps_sim"; + + if (status == "OK") { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } else if (status == "LOW_ACCURACY" || status == "DRIFTING") { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } else { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + + diag.message = message; + + diagnostic_msgs::msg::KeyValue kv; + kv.key = "status"; + kv.value = status; + diag.values.push_back(kv); + + kv.key = "msg_count"; + kv.value = std::to_string(msg_count_); + diag.values.push_back(kv); + + kv.key = "position_noise"; + kv.value = std::to_string(position_noise_stddev_) + "m"; + diag.values.push_back(kv); + + diag_pub_->publish(diag); + } + + // Publishers + rclcpp::Publisher::SharedPtr fix_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Parameter callback + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + // Random number generation + std::mt19937 rng_; + std::normal_distribution normal_dist_; + std::uniform_real_distribution uniform_dist_; + + // Parameters + double base_latitude_; + double base_longitude_; + double base_altitude_; + double position_noise_stddev_; + double altitude_noise_stddev_; + double failure_probability_; + bool inject_nan_; + double drift_rate_; + + // Statistics + rclcpp::Time start_time_; + uint64_t msg_count_{0}; +}; + +} // namespace sensor_diagnostics + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/demos/sensor_diagnostics/src/imu_sim_node.cpp b/demos/sensor_diagnostics/src/imu_sim_node.cpp new file mode 100644 index 0000000..15619c7 --- /dev/null +++ b/demos/sensor_diagnostics/src/imu_sim_node.cpp @@ -0,0 +1,228 @@ +// Copyright 2025 selfpatch +// SPDX-License-Identifier: Apache-2.0 + +/// @file imu_sim_node.cpp +/// @brief Simulated IMU sensor with configurable fault injection +/// +/// Publishes simulated Imu messages with realistic patterns +/// and supports runtime fault injection via ROS 2 parameters. + +#include +#include +#include +#include +#include + +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "diagnostic_msgs/msg/key_value.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "sensor_msgs/msg/imu.hpp" + +namespace sensor_diagnostics +{ + +class ImuSimNode : public rclcpp::Node +{ +public: + ImuSimNode() + : Node("imu_sim"), + rng_(std::random_device{}()), + normal_dist_(0.0, 1.0), + uniform_dist_(0.0, 1.0), + start_time_(this->now()) + { + // Declare parameters + this->declare_parameter("rate", 100.0); // Hz + this->declare_parameter("accel_noise_stddev", 0.01); // m/s^2 + this->declare_parameter("gyro_noise_stddev", 0.001); // rad/s + this->declare_parameter("failure_probability", 0.0); + this->declare_parameter("inject_nan", false); + this->declare_parameter("drift_rate", 0.0); // rad/s drift + + load_parameters(); + + // Create publishers + imu_pub_ = this->create_publisher("imu", 10); + diag_pub_ = this->create_publisher( + "diagnostics", 10); + + // Create timer + double rate = this->get_parameter("rate").as_double(); + auto period = std::chrono::duration(1.0 / rate); + timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&ImuSimNode::publish_imu, this)); + + // Register parameter callback + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&ImuSimNode::on_parameter_change, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "IMU simulator started at %.1f Hz", rate); + } + +private: + void load_parameters() + { + accel_noise_stddev_ = this->get_parameter("accel_noise_stddev").as_double(); + gyro_noise_stddev_ = this->get_parameter("gyro_noise_stddev").as_double(); + failure_probability_ = this->get_parameter("failure_probability").as_double(); + inject_nan_ = this->get_parameter("inject_nan").as_bool(); + drift_rate_ = this->get_parameter("drift_rate").as_double(); + } + + rcl_interfaces::msg::SetParametersResult on_parameter_change( + const std::vector & parameters) + { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & param : parameters) { + if (param.get_name() == "accel_noise_stddev") { + accel_noise_stddev_ = param.as_double(); + RCLCPP_INFO(this->get_logger(), "Accel noise changed to %.4f", accel_noise_stddev_); + } else if (param.get_name() == "gyro_noise_stddev") { + gyro_noise_stddev_ = param.as_double(); + RCLCPP_INFO(this->get_logger(), "Gyro noise changed to %.4f", gyro_noise_stddev_); + } else if (param.get_name() == "failure_probability") { + failure_probability_ = param.as_double(); + RCLCPP_INFO( + this->get_logger(), "Failure probability changed to %.2f", + failure_probability_); + } else if (param.get_name() == "inject_nan") { + inject_nan_ = param.as_bool(); + RCLCPP_INFO( + this->get_logger(), "Inject NaN %s", + inject_nan_ ? "enabled" : "disabled"); + } else if (param.get_name() == "drift_rate") { + drift_rate_ = param.as_double(); + RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f", drift_rate_); + } + } + + return result; + } + + void publish_imu() + { + msg_count_++; + + // Check for complete failure + if (uniform_dist_(rng_) < failure_probability_) { + publish_diagnostics("TIMEOUT", "Sensor failure (injected)"); + return; + } + + auto msg = sensor_msgs::msg::Imu(); + msg.header.stamp = this->now(); + msg.header.frame_id = "imu_link"; + + // Calculate elapsed time for drift + double elapsed = (this->now() - start_time_).seconds(); + double drift_offset = drift_rate_ * elapsed; + + // Simulated stationary IMU with noise + // Orientation (quaternion) - identity with small drift + double yaw_drift = drift_offset; + msg.orientation.w = std::cos(yaw_drift / 2.0); + msg.orientation.x = 0.0; + msg.orientation.y = 0.0; + msg.orientation.z = std::sin(yaw_drift / 2.0); + + // Angular velocity - should be ~0 when stationary + msg.angular_velocity.x = normal_dist_(rng_) * gyro_noise_stddev_; + msg.angular_velocity.y = normal_dist_(rng_) * gyro_noise_stddev_; + msg.angular_velocity.z = normal_dist_(rng_) * gyro_noise_stddev_ + drift_rate_; + + // Linear acceleration - gravity on z-axis when level + msg.linear_acceleration.x = normal_dist_(rng_) * accel_noise_stddev_; + msg.linear_acceleration.y = normal_dist_(rng_) * accel_noise_stddev_; + msg.linear_acceleration.z = 9.81 + normal_dist_(rng_) * accel_noise_stddev_; + + // Covariance (diagonal) + for (int i = 0; i < 9; i++) { + msg.orientation_covariance[i] = (i % 4 == 0) ? 0.01 : 0.0; + msg.angular_velocity_covariance[i] = + (i % 4 == 0) ? gyro_noise_stddev_ * gyro_noise_stddev_ : 0.0; + msg.linear_acceleration_covariance[i] = + (i % 4 == 0) ? accel_noise_stddev_ * accel_noise_stddev_ : 0.0; + } + + // Inject NaN if configured + if (inject_nan_ && uniform_dist_(rng_) < 0.05) { + msg.linear_acceleration.x = std::numeric_limits::quiet_NaN(); + publish_diagnostics("NAN_VALUES", "NaN values detected in acceleration"); + } else if (drift_offset > 0.1) { + publish_diagnostics("DRIFTING", "Drift: " + std::to_string(drift_offset) + " rad"); + } else if (accel_noise_stddev_ > 0.1 || gyro_noise_stddev_ > 0.01) { + publish_diagnostics("HIGH_NOISE", "High noise levels detected"); + } else { + publish_diagnostics("OK", "Operating normally"); + } + + imu_pub_->publish(msg); + } + + void publish_diagnostics(const std::string & status, const std::string & message) + { + auto diag = diagnostic_msgs::msg::DiagnosticStatus(); + diag.name = "imu_sim"; + + if (status == "OK") { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } else if (status == "HIGH_NOISE" || status == "DRIFTING") { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } else { + diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + + diag.message = message; + + diagnostic_msgs::msg::KeyValue kv; + kv.key = "status"; + kv.value = status; + diag.values.push_back(kv); + + kv.key = "msg_count"; + kv.value = std::to_string(msg_count_); + diag.values.push_back(kv); + + diag_pub_->publish(diag); + } + + // Publishers + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Parameter callback + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + // Random number generation + std::mt19937 rng_; + std::normal_distribution normal_dist_; + std::uniform_real_distribution uniform_dist_; + + // Parameters + double accel_noise_stddev_; + double gyro_noise_stddev_; + double failure_probability_; + bool inject_nan_; + double drift_rate_; + + // Statistics + rclcpp::Time start_time_; + uint64_t msg_count_{0}; +}; + +} // namespace sensor_diagnostics + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From 4f567b2ef4f818e010b4ee4e7e3ae2ad407cffff Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 12:21:53 +0000 Subject: [PATCH 03/19] feat(sensor-diagnostics): add anomaly detector node Add anomaly detector that monitors sensor streams and reports faults: - Subscribes to LiDAR, IMU, GPS topics - Detects NaN values, out-of-range readings - Monitors message rates and timeouts - Publishes DiagnosticArray for detected faults Fault detection thresholds are configurable via ROS parameters: - rate_timeout_sec - max_nan_ratio - sensor-specific rate minimums --- .../src/anomaly_detector_node.cpp | 272 ++++++++++++++++++ 1 file changed, 272 insertions(+) create mode 100644 demos/sensor_diagnostics/src/anomaly_detector_node.cpp diff --git a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp new file mode 100644 index 0000000..d95e765 --- /dev/null +++ b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp @@ -0,0 +1,272 @@ +// Copyright 2025 selfpatch +// SPDX-License-Identifier: Apache-2.0 + +/// @file anomaly_detector_node.cpp +/// @brief Monitors sensor streams and reports detected anomalies as faults +/// +/// Subscribes to sensor topics and diagnostics, detects anomalies, +/// and publishes fault events. + +#include +#include +#include +#include +#include + +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "diagnostic_msgs/msg/diagnostic_status.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/nav_sat_fix.hpp" + +namespace sensor_diagnostics +{ + +class AnomalyDetectorNode : public rclcpp::Node +{ +public: + AnomalyDetectorNode() + : Node("anomaly_detector") + { + // Declare parameters for thresholds + this->declare_parameter("rate_timeout_sec", 5.0); + this->declare_parameter("max_nan_ratio", 0.1); + this->declare_parameter("lidar_rate_min", 5.0); + this->declare_parameter("imu_rate_min", 50.0); + this->declare_parameter("gps_rate_min", 0.5); + + rate_timeout_sec_ = this->get_parameter("rate_timeout_sec").as_double(); + max_nan_ratio_ = this->get_parameter("max_nan_ratio").as_double(); + + // Create subscribers + lidar_sub_ = this->create_subscription( + "/sensors/lidar_sim/scan", 10, + std::bind(&AnomalyDetectorNode::lidar_callback, this, std::placeholders::_1)); + + imu_sub_ = this->create_subscription( + "/sensors/imu_sim/imu", 10, + std::bind(&AnomalyDetectorNode::imu_callback, this, std::placeholders::_1)); + + gps_sub_ = this->create_subscription( + "/sensors/gps_sim/fix", 10, + std::bind(&AnomalyDetectorNode::gps_callback, this, std::placeholders::_1)); + + // Create publisher for detected faults + fault_pub_ = this->create_publisher( + "detected_faults", 10); + + // Timer for periodic health check + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&AnomalyDetectorNode::check_sensor_health, this)); + + RCLCPP_INFO(this->get_logger(), "Anomaly detector started"); + } + +private: + void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) + { + sensor_timestamps_["lidar"] = this->now(); + lidar_msg_count_++; + + // Check for NaN values + int nan_count = 0; + for (const auto & range : msg->ranges) { + if (std::isnan(range)) { + nan_count++; + } + } + + double nan_ratio = static_cast(nan_count) / static_cast(msg->ranges.size()); + if (nan_ratio > max_nan_ratio_) { + report_fault("lidar_sim", "SENSOR_NAN", + "LiDAR has " + std::to_string(static_cast(nan_ratio * 100)) + "% NaN values"); + } + + // Check for all-zero ranges (sensor malfunction) + bool all_min = true; + for (const auto & range : msg->ranges) { + if (!std::isnan(range) && range > msg->range_min + 0.01) { + all_min = false; + break; + } + } + if (all_min && !msg->ranges.empty()) { + report_fault("lidar_sim", "SENSOR_MALFUNCTION", "LiDAR returns all minimum range values"); + } + } + + void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) + { + sensor_timestamps_["imu"] = this->now(); + imu_msg_count_++; + + // Check for NaN values + if (std::isnan(msg->linear_acceleration.x) || + std::isnan(msg->linear_acceleration.y) || + std::isnan(msg->linear_acceleration.z)) + { + report_fault("imu_sim", "SENSOR_NAN", "IMU acceleration contains NaN values"); + } + + if (std::isnan(msg->angular_velocity.x) || + std::isnan(msg->angular_velocity.y) || + std::isnan(msg->angular_velocity.z)) + { + report_fault("imu_sim", "SENSOR_NAN", "IMU angular velocity contains NaN values"); + } + + // Check for unrealistic acceleration (should be ~9.81 on z when stationary) + double accel_magnitude = std::sqrt( + msg->linear_acceleration.x * msg->linear_acceleration.x + + msg->linear_acceleration.y * msg->linear_acceleration.y + + msg->linear_acceleration.z * msg->linear_acceleration.z); + + if (accel_magnitude < 5.0 || accel_magnitude > 15.0) { + report_fault("imu_sim", "SENSOR_OUT_OF_RANGE", + "IMU acceleration magnitude: " + std::to_string(accel_magnitude) + " m/s^2"); + } + } + + void gps_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) + { + sensor_timestamps_["gps"] = this->now(); + gps_msg_count_++; + + // Check for no fix + if (msg->status.status < 0) { + report_fault("gps_sim", "NO_FIX", "GPS has no satellite fix"); + return; + } + + // Check for NaN coordinates + if (std::isnan(msg->latitude) || std::isnan(msg->longitude)) { + report_fault("gps_sim", "SENSOR_NAN", "GPS coordinates contain NaN values"); + } + + // Check for unrealistic coordinates + if (msg->latitude < -90 || msg->latitude > 90 || + msg->longitude < -180 || msg->longitude > 180) + { + report_fault("gps_sim", "SENSOR_OUT_OF_RANGE", "GPS coordinates out of valid range"); + } + } + + void check_sensor_health() + { + auto now = this->now(); + + // Check for sensor timeouts + check_timeout("lidar", now); + check_timeout("imu", now); + check_timeout("gps", now); + + // Calculate and log rates + double elapsed = 1.0; // 1 second timer + double lidar_rate = (lidar_msg_count_ - last_lidar_count_) / elapsed; + double imu_rate = (imu_msg_count_ - last_imu_count_) / elapsed; + double gps_rate = (gps_msg_count_ - last_gps_count_) / elapsed; + + last_lidar_count_ = lidar_msg_count_; + last_imu_count_ = imu_msg_count_; + last_gps_count_ = gps_msg_count_; + + // Check for degraded rates + double lidar_rate_min = this->get_parameter("lidar_rate_min").as_double(); + double imu_rate_min = this->get_parameter("imu_rate_min").as_double(); + double gps_rate_min = this->get_parameter("gps_rate_min").as_double(); + + if (lidar_rate > 0 && lidar_rate < lidar_rate_min) { + report_fault("lidar_sim", "RATE_DEGRADED", + "LiDAR rate: " + std::to_string(lidar_rate) + " Hz (min: " + + std::to_string(lidar_rate_min) + ")"); + } + + if (imu_rate > 0 && imu_rate < imu_rate_min) { + report_fault("imu_sim", "RATE_DEGRADED", + "IMU rate: " + std::to_string(imu_rate) + " Hz (min: " + + std::to_string(imu_rate_min) + ")"); + } + + if (gps_rate > 0 && gps_rate < gps_rate_min) { + report_fault("gps_sim", "RATE_DEGRADED", + "GPS rate: " + std::to_string(gps_rate) + " Hz (min: " + + std::to_string(gps_rate_min) + ")"); + } + } + + void check_timeout(const std::string & sensor, const rclcpp::Time & now) + { + auto it = sensor_timestamps_.find(sensor); + if (it != sensor_timestamps_.end()) { + double elapsed = (now - it->second).seconds(); + if (elapsed > rate_timeout_sec_) { + report_fault(sensor + "_sim", "SENSOR_TIMEOUT", + sensor + " no messages for " + std::to_string(elapsed) + "s"); + } + } + } + + void report_fault( + const std::string & source, const std::string & code, + const std::string & message) + { + auto diag_array = diagnostic_msgs::msg::DiagnosticArray(); + diag_array.header.stamp = this->now(); + + auto status = diagnostic_msgs::msg::DiagnosticStatus(); + status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.name = source; + status.message = message; + status.hardware_id = source; + + diagnostic_msgs::msg::KeyValue kv; + kv.key = "fault_code"; + kv.value = code; + status.values.push_back(kv); + + kv.key = "timestamp"; + kv.value = std::to_string(this->now().nanoseconds()); + status.values.push_back(kv); + + diag_array.status.push_back(status); + fault_pub_->publish(diag_array); + + RCLCPP_WARN(this->get_logger(), "[%s] %s: %s", source.c_str(), code.c_str(), message.c_str()); + } + + // Subscribers + rclcpp::Subscription::SharedPtr lidar_sub_; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr gps_sub_; + + // Publisher + rclcpp::Publisher::SharedPtr fault_pub_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Parameters + double rate_timeout_sec_; + double max_nan_ratio_; + + // State tracking + std::map sensor_timestamps_; + uint64_t lidar_msg_count_{0}; + uint64_t imu_msg_count_{0}; + uint64_t gps_msg_count_{0}; + uint64_t last_lidar_count_{0}; + uint64_t last_imu_count_{0}; + uint64_t last_gps_count_{0}; +}; + +} // namespace sensor_diagnostics + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From 8f2e6b3d8941add4ed8b4da8200494d317293302 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 12:22:06 +0000 Subject: [PATCH 04/19] feat(sensor-diagnostics): add config files and launch system Add ROS 2 launch file and configuration: - demo.launch.py: launches all nodes with proper namespacing - /sensors: lidar_sim, imu_sim, gps_sim, camera_sim - /processing: anomaly_detector - /diagnostics: ros2_medkit_gateway - sensor_params.yaml: default sensor parameters - medkit_params.yaml: gateway configuration - sensor_manifest.yaml: SOVD entity hierarchy definition --- .../config/medkit_params.yaml | 24 +++++ .../config/sensor_manifest.yaml | 67 +++++++++++++ .../config/sensor_params.yaml | 56 +++++++++++ .../sensor_diagnostics/launch/demo.launch.py | 96 +++++++++++++++++++ 4 files changed, 243 insertions(+) create mode 100644 demos/sensor_diagnostics/config/medkit_params.yaml create mode 100644 demos/sensor_diagnostics/config/sensor_manifest.yaml create mode 100644 demos/sensor_diagnostics/config/sensor_params.yaml create mode 100644 demos/sensor_diagnostics/launch/demo.launch.py diff --git a/demos/sensor_diagnostics/config/medkit_params.yaml b/demos/sensor_diagnostics/config/medkit_params.yaml new file mode 100644 index 0000000..1539cf8 --- /dev/null +++ b/demos/sensor_diagnostics/config/medkit_params.yaml @@ -0,0 +1,24 @@ +# ros2_medkit gateway configuration for Sensor Diagnostics Demo +# Gateway runs under /diagnostics namespace +diagnostics: + ros2_medkit_gateway: + ros__parameters: + server: + host: "0.0.0.0" + port: 8080 + + refresh_interval_ms: 1000 + + cors: + allowed_origins: ["*"] + allowed_methods: ["GET", "PUT", "POST", "DELETE", "OPTIONS"] + allowed_headers: ["Content-Type", "Accept"] + allow_credentials: false + max_age_seconds: 86400 + + max_parallel_topic_samples: 10 + + # Manifest-based discovery + manifest: + enabled: true + path: "" # Will be set via launch argument diff --git a/demos/sensor_diagnostics/config/sensor_manifest.yaml b/demos/sensor_diagnostics/config/sensor_manifest.yaml new file mode 100644 index 0000000..a3cf0ed --- /dev/null +++ b/demos/sensor_diagnostics/config/sensor_manifest.yaml @@ -0,0 +1,67 @@ +# SOVD Manifest for Sensor Diagnostics Demo +# Defines the entity hierarchy for ros2_medkit gateway +manifest_version: "1.0" + +metadata: + name: "sensor-diagnostics" + description: "Simulated sensors for diagnostics demo (no Gazebo required)" + version: "0.1.0" + +areas: + - id: sensors + name: "Sensors" + description: "Simulated sensor nodes" + namespace: /sensors + + - id: processing + name: "Processing" + description: "Data processing and anomaly detection" + namespace: /processing + + - id: diagnostics + name: "Diagnostics" + description: "ros2_medkit gateway and monitoring" + namespace: /diagnostics + +components: + - id: lidar-unit + name: "LiDAR Unit" + description: "Simulated 2D LiDAR sensor with configurable fault injection" + area: sensors + apps: + - lidar_sim + + - id: imu-unit + name: "IMU Unit" + description: "Simulated 9-DOF IMU sensor" + area: sensors + apps: + - imu_sim + + - id: gps-unit + name: "GPS Unit" + description: "Simulated GPS receiver" + area: sensors + apps: + - gps_sim + + - id: camera-unit + name: "Camera Unit" + description: "Simulated RGB camera" + area: sensors + apps: + - camera_sim + + - id: compute-unit + name: "Compute Unit" + description: "Data processing and anomaly detection" + area: processing + apps: + - anomaly_detector + + - id: gateway + name: "SOVD Gateway" + description: "ros2_medkit REST API gateway" + area: diagnostics + apps: + - ros2_medkit_gateway diff --git a/demos/sensor_diagnostics/config/sensor_params.yaml b/demos/sensor_diagnostics/config/sensor_params.yaml new file mode 100644 index 0000000..4878f14 --- /dev/null +++ b/demos/sensor_diagnostics/config/sensor_params.yaml @@ -0,0 +1,56 @@ +# Default sensor parameters for the diagnostics demo +# All sensors run in the /sensors namespace + +sensors: + lidar_sim: + ros__parameters: + scan_rate: 10.0 # Hz + range_min: 0.12 # meters + range_max: 3.5 # meters + angle_min: -3.14159 # radians + angle_max: 3.14159 # radians + num_readings: 360 # number of laser beams + noise_stddev: 0.01 # meters (normal sensor) + failure_probability: 0.0 # no failures by default + inject_nan: false + drift_rate: 0.0 # no drift by default + + imu_sim: + ros__parameters: + rate: 100.0 # Hz + accel_noise_stddev: 0.01 # m/s^2 + gyro_noise_stddev: 0.001 # rad/s + failure_probability: 0.0 + inject_nan: false + drift_rate: 0.0 # rad/s + + gps_sim: + ros__parameters: + rate: 1.0 # Hz (typical GPS rate) + base_latitude: 52.2297 # Warsaw, Poland + base_longitude: 21.0122 + base_altitude: 100.0 # meters + position_noise_stddev: 2.0 # meters (typical GPS accuracy) + altitude_noise_stddev: 5.0 # meters + failure_probability: 0.0 + inject_nan: false + drift_rate: 0.0 # meters/second + + camera_sim: + ros__parameters: + rate: 30.0 # Hz + width: 640 # pixels + height: 480 # pixels + noise_level: 0.0 # 0.0 - 1.0 + failure_probability: 0.0 + inject_black_frames: false + brightness: 128 # 0-255 + +processing: + anomaly_detector: + ros__parameters: + rate_timeout_sec: 5.0 # seconds before timeout fault + max_nan_ratio: 0.1 # max NaN values before fault + lidar_rate_min: 5.0 # Hz + imu_rate_min: 50.0 # Hz + gps_rate_min: 0.5 # Hz diff --git a/demos/sensor_diagnostics/launch/demo.launch.py b/demos/sensor_diagnostics/launch/demo.launch.py new file mode 100644 index 0000000..5adf20b --- /dev/null +++ b/demos/sensor_diagnostics/launch/demo.launch.py @@ -0,0 +1,96 @@ +"""Launch Sensor Diagnostics Demo with ros2_medkit gateway. + +Lightweight demo without Gazebo - pure sensor simulation with fault injection. + +Namespace structure: + /sensors - Simulated sensor nodes (lidar, imu, gps, camera) + /processing - Anomaly detector + /diagnostics - ros2_medkit gateway +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + # Get package directory + pkg_dir = get_package_share_directory("sensor_diagnostics_demo") + + # Config file paths + medkit_params_file = os.path.join(pkg_dir, "config", "medkit_params.yaml") + sensor_params_file = os.path.join(pkg_dir, "config", "sensor_params.yaml") + manifest_file = os.path.join(pkg_dir, "config", "sensor_manifest.yaml") + + # Launch arguments + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + + return LaunchDescription( + [ + # Declare launch arguments + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Use simulation time (set to true if using with Gazebo)", + ), + # ===== Sensor Nodes (under /sensors namespace) ===== + Node( + package="sensor_diagnostics_demo", + executable="lidar_sim_node", + name="lidar_sim", + namespace="sensors", + output="screen", + parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + ), + Node( + package="sensor_diagnostics_demo", + executable="imu_sim_node", + name="imu_sim", + namespace="sensors", + output="screen", + parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + ), + Node( + package="sensor_diagnostics_demo", + executable="gps_sim_node", + name="gps_sim", + namespace="sensors", + output="screen", + parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + ), + Node( + package="sensor_diagnostics_demo", + executable="camera_sim_node", + name="camera_sim", + namespace="sensors", + output="screen", + parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + ), + # ===== Processing Nodes (under /processing namespace) ===== + Node( + package="sensor_diagnostics_demo", + executable="anomaly_detector_node", + name="anomaly_detector", + namespace="processing", + output="screen", + parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + ), + # ===== ros2_medkit Gateway (under /diagnostics namespace) ===== + Node( + package="ros2_medkit_gateway", + executable="gateway_node", + name="ros2_medkit_gateway", + namespace="diagnostics", + output="screen", + parameters=[ + medkit_params_file, + {"use_sim_time": use_sim_time}, + {"manifest.path": manifest_file}, + ], + ), + ] + ) From d0a794e7d1c1ba138a8678428615ab3407a0ec4d Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 12:22:20 +0000 Subject: [PATCH 05/19] feat(sensor-diagnostics): add Docker support Add lightweight Dockerfile and docker-compose.yml: - Based on ros:jazzy-ros-base (no Gazebo/GUI packages) - Image size: ~500MB vs ~4GB for TurtleBot3 demo - Startup time: ~5s vs ~60s - Includes CI profile for headless testing - Optional sovd-web-ui service for visualization --- demos/sensor_diagnostics/Dockerfile | 54 +++++++++++++++++++++ demos/sensor_diagnostics/docker-compose.yml | 48 ++++++++++++++++++ 2 files changed, 102 insertions(+) create mode 100644 demos/sensor_diagnostics/Dockerfile create mode 100644 demos/sensor_diagnostics/docker-compose.yml diff --git a/demos/sensor_diagnostics/Dockerfile b/demos/sensor_diagnostics/Dockerfile new file mode 100644 index 0000000..20d0a92 --- /dev/null +++ b/demos/sensor_diagnostics/Dockerfile @@ -0,0 +1,54 @@ +# Sensor Diagnostics Demo +# Lightweight ROS 2 image without Gazebo - fast build and startup +# Image size: ~500MB vs ~4GB for TurtleBot3 demo +# Startup time: ~5s vs ~60s + +FROM ros:jazzy-ros-base + +ENV DEBIAN_FRONTEND=noninteractive +ENV ROS_DISTRO=jazzy +ENV COLCON_WS=/root/demo_ws + +# Install minimal dependencies (no Gazebo, no simulation packages) +RUN apt-get update && apt-get install -y \ + ros-jazzy-ament-lint-auto \ + ros-jazzy-ament-lint-common \ + ros-jazzy-ament-cmake-gtest \ + python3-colcon-common-extensions \ + python3-requests \ + nlohmann-json3-dev \ + libcpp-httplib-dev \ + git \ + curl \ + jq \ + && rm -rf /var/lib/apt/lists/* + +# Clone ros2_medkit from GitHub (gateway only) +WORKDIR ${COLCON_WS}/src +RUN git clone --depth 1 https://github.com/selfpatch/ros2_medkit.git && \ + mv ros2_medkit/src/ros2_medkit_gateway . && \ + rm -rf ros2_medkit + +# Copy demo package +COPY package.xml CMakeLists.txt ${COLCON_WS}/src/sensor_diagnostics_demo/ +COPY include/ ${COLCON_WS}/src/sensor_diagnostics_demo/include/ +COPY src/ ${COLCON_WS}/src/sensor_diagnostics_demo/src/ +COPY config/ ${COLCON_WS}/src/sensor_diagnostics_demo/config/ +COPY launch/ ${COLCON_WS}/src/sensor_diagnostics_demo/launch/ + +# Build all packages +WORKDIR ${COLCON_WS} +RUN bash -c "source /opt/ros/jazzy/setup.bash && \ + rosdep update && \ + rosdep install --from-paths src --ignore-src -r -y && \ + colcon build --symlink-install" + +# Setup environment +RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc && \ + echo "source ${COLCON_WS}/install/setup.bash" >> ~/.bashrc + +# Expose gateway port +EXPOSE 8080 + +# Default command: launch the demo +CMD ["bash", "-c", "source /opt/ros/jazzy/setup.bash && source /root/demo_ws/install/setup.bash && ros2 launch sensor_diagnostics_demo demo.launch.py"] diff --git a/demos/sensor_diagnostics/docker-compose.yml b/demos/sensor_diagnostics/docker-compose.yml new file mode 100644 index 0000000..23996cc --- /dev/null +++ b/demos/sensor_diagnostics/docker-compose.yml @@ -0,0 +1,48 @@ +services: + # Main demo service - runs all sensor nodes + gateway + sensor-demo: + build: + context: . + dockerfile: Dockerfile + container_name: sensor_diagnostics_demo + environment: + - ROS_DOMAIN_ID=40 + ports: + - "8080:8080" + stdin_open: true + tty: true + # Default command launches the full demo + # Override with: docker compose run sensor-demo bash + command: > + bash -c "source /opt/ros/jazzy/setup.bash && + source /root/demo_ws/install/setup.bash && + ros2 launch sensor_diagnostics_demo demo.launch.py" + + # Web UI for visualization (optional) + sovd-web-ui: + image: ghcr.io/selfpatch/sovd_web_ui:latest + container_name: sovd_web_ui + ports: + - "3000:80" + depends_on: + - sensor-demo + + # For CI testing - headless mode with quick exit + sensor-demo-ci: + profiles: ["ci"] + build: + context: . + dockerfile: Dockerfile + container_name: sensor_diagnostics_demo_ci + environment: + - ROS_DOMAIN_ID=40 + ports: + - "8080:8080" + command: > + bash -c "source /opt/ros/jazzy/setup.bash && + source /root/demo_ws/install/setup.bash && + ros2 launch sensor_diagnostics_demo demo.launch.py & + sleep 10 && + curl -sf http://localhost:8080/api/v1/health && + curl -sf http://localhost:8080/api/v1/apps | jq '.[] | .name' && + echo 'CI validation passed!'" From da1a3c4d943c08e7c4e656889a7feb29ec5679d3 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 12:23:58 +0000 Subject: [PATCH 06/19] feat(sensor-diagnostics): add demo scripts and documentation Add interactive demo scripts: - run-demo.sh: walkthrough of ros2_medkit API features - inject-noise.sh: increase sensor noise levels - inject-failure.sh: cause sensor timeouts - inject-nan.sh: inject NaN values - inject-drift.sh: enable sensor drift - restore-normal.sh: clear all faults Add comprehensive README.md with: - Quick start guide (Docker and source) - API examples for all sensor operations - Fault scenario documentation - Parameter reference tables - Comparison with TurtleBot3 demo --- demos/sensor_diagnostics/README.md | 194 +++++++++++++++++++++ demos/sensor_diagnostics/inject-drift.sh | 27 +++ demos/sensor_diagnostics/inject-failure.sh | 17 ++ demos/sensor_diagnostics/inject-nan.sh | 27 +++ demos/sensor_diagnostics/inject-noise.sh | 34 ++++ demos/sensor_diagnostics/restore-normal.sh | 52 ++++++ demos/sensor_diagnostics/run-demo.sh | 107 ++++++++++++ 7 files changed, 458 insertions(+) create mode 100644 demos/sensor_diagnostics/README.md create mode 100755 demos/sensor_diagnostics/inject-drift.sh create mode 100755 demos/sensor_diagnostics/inject-failure.sh create mode 100755 demos/sensor_diagnostics/inject-nan.sh create mode 100755 demos/sensor_diagnostics/inject-noise.sh create mode 100755 demos/sensor_diagnostics/restore-normal.sh create mode 100755 demos/sensor_diagnostics/run-demo.sh diff --git a/demos/sensor_diagnostics/README.md b/demos/sensor_diagnostics/README.md new file mode 100644 index 0000000..cac76be --- /dev/null +++ b/demos/sensor_diagnostics/README.md @@ -0,0 +1,194 @@ +# Sensor Diagnostics Demo + +Lightweight sensor diagnostics demo for **ros2_medkit** - no Gazebo required! + +This demo showcases ros2_medkit's data monitoring, configuration management, and fault detection using simulated sensor nodes with configurable fault injection. + +## Features + +- **Runs anywhere** - No Gazebo, no GPU, works in CI and GitHub Codespaces +- **Fast startup** - Seconds vs minutes compared to TurtleBot3 demo +- **Focus on diagnostics** - Pure ros2_medkit features without robot complexity +- **Configurable faults** - Runtime fault injection via REST API + +## Quick Start + +### Using Docker (Recommended) + +```bash +# Start the demo +docker compose up + +# In another terminal, open the Web UI +# Navigate to http://localhost:3000 + +# Or run the demo script +./run-demo.sh +``` + +### Building from Source + +```bash +# In a ROS 2 workspace +cd ~/ros2_ws/src +git clone https://github.com/selfpatch/selfpatch_demos.git +cd .. + +# Build +colcon build --packages-select sensor_diagnostics_demo + +# Launch +source install/setup.bash +ros2 launch sensor_diagnostics_demo demo.launch.py +``` + +## Architecture + +``` +Sensor Diagnostics Demo +├── /sensors # Simulated sensor nodes +│ ├── lidar_sim # 2D LiDAR (LaserScan) +│ ├── imu_sim # 9-DOF IMU (Imu) +│ ├── gps_sim # GPS receiver (NavSatFix) +│ └── camera_sim # RGB camera (Image, CameraInfo) +├── /processing # Data processing +│ └── anomaly_detector # Fault detection node +└── /diagnostics # Monitoring + └── ros2_medkit_gateway # REST API gateway +``` + +## API Examples + +### Read Sensor Data + +```bash +# Get LiDAR scan +curl http://localhost:8080/api/v1/apps/lidar_sim/data/scan | jq '.ranges[:5]' + +# Get IMU data +curl http://localhost:8080/api/v1/apps/imu_sim/data/imu | jq '.linear_acceleration' + +# Get GPS fix +curl http://localhost:8080/api/v1/apps/gps_sim/data/fix | jq '{lat: .latitude, lon: .longitude}' +``` + +### View Configurations + +```bash +# List all LiDAR configurations +curl http://localhost:8080/api/v1/apps/lidar_sim/configurations | jq + +# Get specific parameter +curl http://localhost:8080/api/v1/apps/lidar_sim/configurations/noise_stddev | jq +``` + +### Inject Faults + +```bash +# Increase sensor noise +curl -X PUT http://localhost:8080/api/v1/apps/lidar_sim/configurations/noise_stddev \ + -H "Content-Type: application/json" \ + -d '{"value": 0.5}' + +# Cause sensor timeout +curl -X PUT http://localhost:8080/api/v1/apps/lidar_sim/configurations/failure_probability \ + -H "Content-Type: application/json" \ + -d '{"value": 1.0}' + +# Inject NaN values +curl -X PUT http://localhost:8080/api/v1/apps/lidar_sim/configurations/inject_nan \ + -H "Content-Type: application/json" \ + -d '{"value": true}' +``` + +### Check Faults + +```bash +# List detected faults +curl http://localhost:8080/api/v1/faults | jq +``` + +## Configurable Fault Scenarios + +| Fault | Description | Parameter | +|-------|-------------|-----------| +| `SENSOR_TIMEOUT` | No messages published | `failure_probability: 1.0` | +| `SENSOR_NAN` | Invalid readings | `inject_nan: true` | +| `HIGH_NOISE` | Degraded accuracy | `noise_stddev: 0.5` | +| `DRIFT_DETECTED` | Gradual sensor drift | `drift_rate: 0.1` | +| `BLACK_FRAME` | Camera black frames | `inject_black_frames: true` | + +## Scripts + +| Script | Description | +|--------|-------------| +| `run-demo.sh` | Interactive demo walkthrough | +| `inject-noise.sh` | Inject high noise fault | +| `inject-failure.sh` | Cause sensor timeout | +| `inject-nan.sh` | Inject NaN values | +| `inject-drift.sh` | Enable sensor drift | +| `restore-normal.sh` | Clear all faults | + +## Sensor Parameters + +### LiDAR (`/sensors/lidar_sim`) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `scan_rate` | double | 10.0 | Publishing rate (Hz) | +| `range_min` | double | 0.12 | Minimum range (m) | +| `range_max` | double | 3.5 | Maximum range (m) | +| `noise_stddev` | double | 0.01 | Noise standard deviation (m) | +| `failure_probability` | double | 0.0 | Probability of failure per cycle | +| `inject_nan` | bool | false | Inject NaN values | +| `drift_rate` | double | 0.0 | Range drift rate (m/s) | + +### IMU (`/sensors/imu_sim`) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `rate` | double | 100.0 | Publishing rate (Hz) | +| `accel_noise_stddev` | double | 0.01 | Acceleration noise (m/s²) | +| `gyro_noise_stddev` | double | 0.001 | Gyroscope noise (rad/s) | +| `drift_rate` | double | 0.0 | Orientation drift (rad/s) | + +### GPS (`/sensors/gps_sim`) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `rate` | double | 1.0 | Publishing rate (Hz) | +| `position_noise_stddev` | double | 2.0 | Position noise (m) | +| `altitude_noise_stddev` | double | 5.0 | Altitude noise (m) | +| `drift_rate` | double | 0.0 | Position drift (m/s) | + +### Camera (`/sensors/camera_sim`) + +| Parameter | Type | Default | Description | +|-----------|------|---------|-------------| +| `rate` | double | 30.0 | Publishing rate (Hz) | +| `width` | int | 640 | Image width (pixels) | +| `height` | int | 480 | Image height (pixels) | +| `noise_level` | double | 0.0 | Fraction of noisy pixels (0-1) | +| `brightness` | int | 128 | Base brightness (0-255) | +| `inject_black_frames` | bool | false | Randomly inject black frames | + +## Use Cases + +1. **CI/CD Testing** - Validate ros2_medkit without heavy simulation +2. **Tutorials** - Simple environment for learning +3. **IoT Sensors** - Same patterns work for non-robot sensors +4. **API Development** - Fast iteration on gateway features + +## Comparison with TurtleBot3 Demo + +| | Sensor Demo | TurtleBot3 Demo | +|---|-------------|-----------------| +| Docker image | ~500 MB | ~4 GB | +| Startup time | ~5 seconds | ~60 seconds | +| GPU required | No | Recommended | +| CI compatible | Yes | Difficult | +| Focus | Diagnostics | Navigation | + +## License + +Apache 2.0 - See [LICENSE](../../LICENSE) diff --git a/demos/sensor_diagnostics/inject-drift.sh b/demos/sensor_diagnostics/inject-drift.sh new file mode 100755 index 0000000..0113184 --- /dev/null +++ b/demos/sensor_diagnostics/inject-drift.sh @@ -0,0 +1,27 @@ +#!/bin/bash +# Inject sensor drift fault + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "Injecting DRIFT fault into sensors..." + +# Set drift rates +echo "Setting LiDAR drift_rate to 0.1 m/s..." +curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/drift_rate" \ + -H "Content-Type: application/json" \ + -d '{"value": 0.1}' + +echo "Setting IMU drift_rate to 0.01 rad/s..." +curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/drift_rate" \ + -H "Content-Type: application/json" \ + -d '{"value": 0.01}' + +echo "Setting GPS drift_rate to 1.0 m/s..." +curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/drift_rate" \ + -H "Content-Type: application/json" \ + -d '{"value": 1.0}' + +echo "" +echo "✓ Drift enabled! Sensor readings will gradually shift over time." +echo " Watch the drift accumulate with: ./run-demo.sh" diff --git a/demos/sensor_diagnostics/inject-failure.sh b/demos/sensor_diagnostics/inject-failure.sh new file mode 100755 index 0000000..3815745 --- /dev/null +++ b/demos/sensor_diagnostics/inject-failure.sh @@ -0,0 +1,17 @@ +#!/bin/bash +# Inject sensor failure (timeout) fault + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "Injecting SENSOR FAILURE fault..." + +# Set high failure probability - sensors will stop publishing +echo "Setting LiDAR failure_probability to 1.0 (complete failure)..." +curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/failure_probability" \ + -H "Content-Type: application/json" \ + -d '{"value": 1.0}' + +echo "" +echo "✓ Sensor failure injected! The anomaly detector should report SENSOR_TIMEOUT." +echo " Check faults with: curl ${API_BASE}/faults | jq" diff --git a/demos/sensor_diagnostics/inject-nan.sh b/demos/sensor_diagnostics/inject-nan.sh new file mode 100755 index 0000000..92de664 --- /dev/null +++ b/demos/sensor_diagnostics/inject-nan.sh @@ -0,0 +1,27 @@ +#!/bin/bash +# Inject NaN values fault into sensors + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "Injecting NaN VALUES fault into sensors..." + +# Enable NaN injection on sensors +echo "Enabling LiDAR inject_nan..." +curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/inject_nan" \ + -H "Content-Type: application/json" \ + -d '{"value": true}' + +echo "Enabling IMU inject_nan..." +curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/inject_nan" \ + -H "Content-Type: application/json" \ + -d '{"value": true}' + +echo "Enabling GPS inject_nan..." +curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/inject_nan" \ + -H "Content-Type: application/json" \ + -d '{"value": true}' + +echo "" +echo "✓ NaN injection enabled! Anomaly detector should report SENSOR_NAN faults." +echo " Check faults with: curl ${API_BASE}/faults | jq" diff --git a/demos/sensor_diagnostics/inject-noise.sh b/demos/sensor_diagnostics/inject-noise.sh new file mode 100755 index 0000000..b61ce27 --- /dev/null +++ b/demos/sensor_diagnostics/inject-noise.sh @@ -0,0 +1,34 @@ +#!/bin/bash +# Inject high noise fault into sensors + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "Injecting HIGH NOISE fault into sensors..." + +# LiDAR: increase noise stddev +echo "Setting LiDAR noise_stddev to 0.5 (very noisy)..." +curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/noise_stddev" \ + -H "Content-Type: application/json" \ + -d '{"value": 0.5}' + +# IMU: increase noise +echo "Setting IMU accel_noise_stddev to 0.5..." +curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/accel_noise_stddev" \ + -H "Content-Type: application/json" \ + -d '{"value": 0.5}' + +# GPS: increase position noise +echo "Setting GPS position_noise_stddev to 50 meters..." +curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/position_noise_stddev" \ + -H "Content-Type: application/json" \ + -d '{"value": 50.0}' + +# Camera: add noise +echo "Setting Camera noise_level to 0.3..." +curl -s -X PUT "${API_BASE}/apps/camera_sim/configurations/noise_level" \ + -H "Content-Type: application/json" \ + -d '{"value": 0.3}' + +echo "" +echo "✓ High noise injected! Check faults with: ./run-demo.sh (step 9)" diff --git a/demos/sensor_diagnostics/restore-normal.sh b/demos/sensor_diagnostics/restore-normal.sh new file mode 100755 index 0000000..7553b73 --- /dev/null +++ b/demos/sensor_diagnostics/restore-normal.sh @@ -0,0 +1,52 @@ +#!/bin/bash +# Restore normal sensor operation (clear all faults) + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +echo "Restoring NORMAL operation..." + +# LiDAR +echo "Resetting LiDAR parameters..." +curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/noise_stddev" \ + -H "Content-Type: application/json" -d '{"value": 0.01}' +curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/failure_probability" \ + -H "Content-Type: application/json" -d '{"value": 0.0}' +curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/inject_nan" \ + -H "Content-Type: application/json" -d '{"value": false}' +curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/drift_rate" \ + -H "Content-Type: application/json" -d '{"value": 0.0}' + +# IMU +echo "Resetting IMU parameters..." +curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/accel_noise_stddev" \ + -H "Content-Type: application/json" -d '{"value": 0.01}' +curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/failure_probability" \ + -H "Content-Type: application/json" -d '{"value": 0.0}' +curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/inject_nan" \ + -H "Content-Type: application/json" -d '{"value": false}' +curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/drift_rate" \ + -H "Content-Type: application/json" -d '{"value": 0.0}' + +# GPS +echo "Resetting GPS parameters..." +curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/position_noise_stddev" \ + -H "Content-Type: application/json" -d '{"value": 2.0}' +curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/failure_probability" \ + -H "Content-Type: application/json" -d '{"value": 0.0}' +curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/inject_nan" \ + -H "Content-Type: application/json" -d '{"value": false}' +curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/drift_rate" \ + -H "Content-Type: application/json" -d '{"value": 0.0}' + +# Camera +echo "Resetting Camera parameters..." +curl -s -X PUT "${API_BASE}/apps/camera_sim/configurations/noise_level" \ + -H "Content-Type: application/json" -d '{"value": 0.0}' +curl -s -X PUT "${API_BASE}/apps/camera_sim/configurations/failure_probability" \ + -H "Content-Type: application/json" -d '{"value": 0.0}' +curl -s -X PUT "${API_BASE}/apps/camera_sim/configurations/inject_black_frames" \ + -H "Content-Type: application/json" -d '{"value": false}' + +echo "" +echo "✓ Normal operation restored! All fault injections cleared." diff --git a/demos/sensor_diagnostics/run-demo.sh b/demos/sensor_diagnostics/run-demo.sh new file mode 100755 index 0000000..d134ca8 --- /dev/null +++ b/demos/sensor_diagnostics/run-demo.sh @@ -0,0 +1,107 @@ +#!/bin/bash +# Sensor Diagnostics Demo - Interactive Demo Script +# Run this script to see ros2_medkit in action with simulated sensors + +set -e + +GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" +API_BASE="${GATEWAY_URL}/api/v1" + +# Colors for output +RED='\033[0;31m' +GREEN='\033[0;32m' +YELLOW='\033[1;33m' +BLUE='\033[0;34m' +NC='\033[0m' # No Color + +echo_step() { + echo -e "\n${BLUE}=== $1 ===${NC}\n" +} + +echo_success() { + echo -e "${GREEN}✓ $1${NC}" +} + +echo_warning() { + echo -e "${YELLOW}⚠ $1${NC}" +} + +echo_error() { + echo -e "${RED}✗ $1${NC}" +} + +wait_for_gateway() { + echo "Waiting for gateway to be ready..." + for i in {1..30}; do + if curl -sf "${API_BASE}/health" > /dev/null 2>&1; then + echo_success "Gateway is ready!" + return 0 + fi + sleep 1 + done + echo_error "Gateway not available at ${GATEWAY_URL}" + exit 1 +} + +# Main demo flow +echo "╔════════════════════════════════════════════════════════════╗" +echo "║ Sensor Diagnostics Demo with ros2_medkit ║" +echo "║ (Lightweight - No Gazebo Required) ║" +echo "╚════════════════════════════════════════════════════════════╝" + +echo_step "1. Checking Gateway Health" +wait_for_gateway +curl -s "${API_BASE}/health" | jq '.' + +echo_step "2. Listing All Areas (Namespaces)" +curl -s "${API_BASE}/areas" | jq '.' + +echo_step "3. Listing All Components" +curl -s "${API_BASE}/components" | jq '.[] | {id: .id, name: .name, area: .area}' + +echo_step "4. Listing All Apps (ROS 2 Nodes)" +curl -s "${API_BASE}/apps" | jq '.[] | {id: .id, name: .name, namespace: .namespace}' + +echo_step "5. Reading LiDAR Data" +echo "Getting latest scan from LiDAR simulator..." +curl -s "${API_BASE}/apps/lidar_sim/data/scan" | jq '{ + angle_min: .angle_min, + angle_max: .angle_max, + range_min: .range_min, + range_max: .range_max, + sample_ranges: .ranges[:5] +}' + +echo_step "6. Reading IMU Data" +echo "Getting latest IMU reading..." +curl -s "${API_BASE}/apps/imu_sim/data/imu" | jq '{ + linear_acceleration: .linear_acceleration, + angular_velocity: .angular_velocity +}' + +echo_step "7. Reading GPS Fix" +echo "Getting current GPS position..." +curl -s "${API_BASE}/apps/gps_sim/data/fix" | jq '{ + latitude: .latitude, + longitude: .longitude, + altitude: .altitude, + status: .status +}' + +echo_step "8. Listing LiDAR Configurations" +echo "These parameters can be modified at runtime to inject faults..." +curl -s "${API_BASE}/apps/lidar_sim/configurations" | jq '.[] | {name: .name, value: .value, type: .type}' + +echo_step "9. Checking Current Faults" +curl -s "${API_BASE}/faults" | jq '.' + +echo "" +echo_success "Demo complete!" +echo "" +echo "Try injecting faults with these scripts:" +echo " ./inject-noise.sh - Increase sensor noise" +echo " ./inject-failure.sh - Cause sensor timeouts" +echo " ./inject-nan.sh - Inject NaN values" +echo " ./restore-normal.sh - Restore normal operation" +echo "" +echo "Or use the Web UI at http://localhost:3000" From aacda0782666c9039ae905dcf34f231e35ed23a8 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 12:24:43 +0000 Subject: [PATCH 07/19] docs: update main README with sensor diagnostics demo - Add sensor diagnostics demo to demos table (marked as Ready) - Add Quick Start section with docker compose instructions --- README.md | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 1e19c68..33c5e0e 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,19 @@ progressing toward more advanced use cases. | Demo | Description | Status | |------|-------------|--------| -| [TurtleBot3 Integration](demos/turtlebot3_integration/) | Basic ros2_medkit integration with TurtleBot3 and Nav2 | 🚧 In Progress | +| [Sensor Diagnostics](demos/sensor_diagnostics/) | Lightweight sensor diagnostics demo (no Gazebo required) | ✅ Ready | +| [TurtleBot3 Integration](demos/turtlebot3_integration/) | Full ros2_medkit integration with TurtleBot3 and Nav2 | 🚧 In Progress | + +### Quick Start (Sensor Diagnostics) + +The sensor diagnostics demo is the fastest way to try ros2_medkit: + +```bash +cd demos/sensor_diagnostics +docker compose up +# Open http://localhost:3000 for the Web UI +# Run ./run-demo.sh for an interactive walkthrough +``` ## Getting Started From fa254ed2c5bf6c0d390ebce806070ed968656fe9 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 12:25:09 +0000 Subject: [PATCH 08/19] ci: add sensor diagnostics demo to CI pipeline Build sensor diagnostics demo image in Docker build job. Runs before TurtleBot3 build (faster, validates basic ros2_medkit). --- .github/workflows/ci.yml | 5 +++++ demos/sensor_diagnostics/run-demo.sh | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 2cb40d3..f38ab8b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -39,6 +39,11 @@ jobs: - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 + - name: Build Sensor Diagnostics demo image + run: | + cd demos/sensor_diagnostics + docker build -t sensor-diagnostics-demo:test -f Dockerfile . + - name: Build TurtleBot3 demo image run: | cd demos/turtlebot3_integration diff --git a/demos/sensor_diagnostics/run-demo.sh b/demos/sensor_diagnostics/run-demo.sh index d134ca8..d30da1e 100755 --- a/demos/sensor_diagnostics/run-demo.sh +++ b/demos/sensor_diagnostics/run-demo.sh @@ -32,7 +32,7 @@ echo_error() { wait_for_gateway() { echo "Waiting for gateway to be ready..." - for i in {1..30}; do + for _ in {1..30}; do if curl -sf "${API_BASE}/health" > /dev/null 2>&1; then echo_success "Gateway is ready!" return 0 From 2c059677939209e357fbd330bc5dfae5ac5e8e2e Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 12:49:00 +0000 Subject: [PATCH 09/19] fix(docker): include all ros2_medkit dependencies The gateway requires additional packages: - ros2_medkit_serialization (JSON/ROS message conversion) - ros2_medkit_msgs (custom messages) - dynmsg (dynamic message introspection) Also add ros-jazzy-yaml-cpp-vendor to apt dependencies. --- demos/sensor_diagnostics/Dockerfile | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/demos/sensor_diagnostics/Dockerfile b/demos/sensor_diagnostics/Dockerfile index 20d0a92..e2c6fcf 100644 --- a/demos/sensor_diagnostics/Dockerfile +++ b/demos/sensor_diagnostics/Dockerfile @@ -14,6 +14,7 @@ RUN apt-get update && apt-get install -y \ ros-jazzy-ament-lint-auto \ ros-jazzy-ament-lint-common \ ros-jazzy-ament-cmake-gtest \ + ros-jazzy-yaml-cpp-vendor \ python3-colcon-common-extensions \ python3-requests \ nlohmann-json3-dev \ @@ -23,10 +24,13 @@ RUN apt-get update && apt-get install -y \ jq \ && rm -rf /var/lib/apt/lists/* -# Clone ros2_medkit from GitHub (gateway only) +# Clone ros2_medkit from GitHub (gateway + dependencies) WORKDIR ${COLCON_WS}/src RUN git clone --depth 1 https://github.com/selfpatch/ros2_medkit.git && \ mv ros2_medkit/src/ros2_medkit_gateway . && \ + mv ros2_medkit/src/ros2_medkit_serialization . && \ + mv ros2_medkit/src/ros2_medkit_msgs . && \ + mv ros2_medkit/src/dynamic_message_introspection/dynmsg . && \ rm -rf ros2_medkit # Copy demo package From ce7a9280eeea5ab8910f53a60d50399a08c29d87 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 17:12:00 +0000 Subject: [PATCH 10/19] fix(docker): resolve build issues for sensor diagnostics demo - Add --recurse-submodules to clone dynmsg submodule - Add ros-jazzy-example-interfaces to apt dependencies - Skip test dependencies not in ros-base image - Disable BUILD_TESTING for faster builds - Use ghcr.io/selfpatch/sovd_web_ui:latest image --- demos/sensor_diagnostics/Dockerfile | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/demos/sensor_diagnostics/Dockerfile b/demos/sensor_diagnostics/Dockerfile index e2c6fcf..56a22a2 100644 --- a/demos/sensor_diagnostics/Dockerfile +++ b/demos/sensor_diagnostics/Dockerfile @@ -15,6 +15,7 @@ RUN apt-get update && apt-get install -y \ ros-jazzy-ament-lint-common \ ros-jazzy-ament-cmake-gtest \ ros-jazzy-yaml-cpp-vendor \ + ros-jazzy-example-interfaces \ python3-colcon-common-extensions \ python3-requests \ nlohmann-json3-dev \ @@ -26,7 +27,7 @@ RUN apt-get update && apt-get install -y \ # Clone ros2_medkit from GitHub (gateway + dependencies) WORKDIR ${COLCON_WS}/src -RUN git clone --depth 1 https://github.com/selfpatch/ros2_medkit.git && \ +RUN git clone --depth 1 --recurse-submodules https://github.com/selfpatch/ros2_medkit.git && \ mv ros2_medkit/src/ros2_medkit_gateway . && \ mv ros2_medkit/src/ros2_medkit_serialization . && \ mv ros2_medkit/src/ros2_medkit_msgs . && \ @@ -40,12 +41,13 @@ COPY src/ ${COLCON_WS}/src/sensor_diagnostics_demo/src/ COPY config/ ${COLCON_WS}/src/sensor_diagnostics_demo/config/ COPY launch/ ${COLCON_WS}/src/sensor_diagnostics_demo/launch/ -# Build all packages +# Build all packages (skip test dependencies that aren't in ros-base) WORKDIR ${COLCON_WS} RUN bash -c "source /opt/ros/jazzy/setup.bash && \ rosdep update && \ - rosdep install --from-paths src --ignore-src -r -y && \ - colcon build --symlink-install" + rosdep install --from-paths src --ignore-src -r -y \ + --skip-keys='ament_cmake_clang_format ament_cmake_clang_tidy test_msgs example_interfaces ros2_medkit_fault_manager' && \ + colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF" # Setup environment RUN echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc && \ From a2bd2aed832ef2795665ba3fb189a170af3dde5d Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 17:53:34 +0000 Subject: [PATCH 11/19] feat: integrate anomaly detector with fault_manager - Add ros2_medkit_fault_manager to Docker image - Add fault_manager node to launch file - Integrate anomaly_detector with ReportFault service - Fix topic subscriptions (/sensors/scan instead of /sensors/lidar_sim/scan) - Fix run-demo.sh jq queries for {items: []} response format - Add libsqlite3-dev for fault_manager storage Fault injection now properly reports to FaultManager: - inject-nan.sh -> SENSOR_NAN faults - inject-failure.sh -> SENSOR_TIMEOUT faults - inject-noise.sh -> RATE_DEGRADED faults --- demos/sensor_diagnostics/CMakeLists.txt | 2 + demos/sensor_diagnostics/Dockerfile | 4 +- .../sensor_diagnostics/launch/demo.launch.py | 11 +++ demos/sensor_diagnostics/package.xml | 1 + demos/sensor_diagnostics/run-demo.sh | 6 +- .../src/anomaly_detector_node.cpp | 79 ++++++++++++++++--- 6 files changed, 90 insertions(+), 13 deletions(-) diff --git a/demos/sensor_diagnostics/CMakeLists.txt b/demos/sensor_diagnostics/CMakeLists.txt index e1ec131..cadfea8 100644 --- a/demos/sensor_diagnostics/CMakeLists.txt +++ b/demos/sensor_diagnostics/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(diagnostic_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) +find_package(ros2_medkit_msgs REQUIRED) # LiDAR simulator node add_executable(lidar_sim_node src/lidar_sim_node.cpp) @@ -52,6 +53,7 @@ ament_target_dependencies(anomaly_detector_node rclcpp sensor_msgs diagnostic_msgs + ros2_medkit_msgs ) # Install executables diff --git a/demos/sensor_diagnostics/Dockerfile b/demos/sensor_diagnostics/Dockerfile index 56a22a2..082139f 100644 --- a/demos/sensor_diagnostics/Dockerfile +++ b/demos/sensor_diagnostics/Dockerfile @@ -20,6 +20,7 @@ RUN apt-get update && apt-get install -y \ python3-requests \ nlohmann-json3-dev \ libcpp-httplib-dev \ + libsqlite3-dev \ git \ curl \ jq \ @@ -31,6 +32,7 @@ RUN git clone --depth 1 --recurse-submodules https://github.com/selfpatch/ros2_m mv ros2_medkit/src/ros2_medkit_gateway . && \ mv ros2_medkit/src/ros2_medkit_serialization . && \ mv ros2_medkit/src/ros2_medkit_msgs . && \ + mv ros2_medkit/src/ros2_medkit_fault_manager . && \ mv ros2_medkit/src/dynamic_message_introspection/dynmsg . && \ rm -rf ros2_medkit @@ -46,7 +48,7 @@ WORKDIR ${COLCON_WS} RUN bash -c "source /opt/ros/jazzy/setup.bash && \ rosdep update && \ rosdep install --from-paths src --ignore-src -r -y \ - --skip-keys='ament_cmake_clang_format ament_cmake_clang_tidy test_msgs example_interfaces ros2_medkit_fault_manager' && \ + --skip-keys='ament_cmake_clang_format ament_cmake_clang_tidy test_msgs example_interfaces sqlite3' && \ colcon build --symlink-install --cmake-args -DBUILD_TESTING=OFF" # Setup environment diff --git a/demos/sensor_diagnostics/launch/demo.launch.py b/demos/sensor_diagnostics/launch/demo.launch.py index 5adf20b..9b647c5 100644 --- a/demos/sensor_diagnostics/launch/demo.launch.py +++ b/demos/sensor_diagnostics/launch/demo.launch.py @@ -92,5 +92,16 @@ def generate_launch_description(): {"manifest.path": manifest_file}, ], ), + # ===== Fault Manager (under /fault_manager namespace) ===== + # Note: Gateway expects services at /fault_manager/*, so we use root namespace + # and node name "fault_manager" to get /fault_manager/get_faults etc. + Node( + package="ros2_medkit_fault_manager", + executable="fault_manager_node", + name="fault_manager", + namespace="", # Root namespace so services are at /fault_manager/* + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + ), ] ) diff --git a/demos/sensor_diagnostics/package.xml b/demos/sensor_diagnostics/package.xml index 0fe46d4..e4a3b69 100644 --- a/demos/sensor_diagnostics/package.xml +++ b/demos/sensor_diagnostics/package.xml @@ -15,6 +15,7 @@ geometry_msgs diagnostic_msgs rcl_interfaces + ros2_medkit_msgs ros2launch ros2_medkit_gateway diff --git a/demos/sensor_diagnostics/run-demo.sh b/demos/sensor_diagnostics/run-demo.sh index d30da1e..c10db28 100755 --- a/demos/sensor_diagnostics/run-demo.sh +++ b/demos/sensor_diagnostics/run-demo.sh @@ -57,10 +57,10 @@ echo_step "2. Listing All Areas (Namespaces)" curl -s "${API_BASE}/areas" | jq '.' echo_step "3. Listing All Components" -curl -s "${API_BASE}/components" | jq '.[] | {id: .id, name: .name, area: .area}' +curl -s "${API_BASE}/components" | jq '.items[] | {id: .id, name: .name, area: .area}' echo_step "4. Listing All Apps (ROS 2 Nodes)" -curl -s "${API_BASE}/apps" | jq '.[] | {id: .id, name: .name, namespace: .namespace}' +curl -s "${API_BASE}/apps" | jq '.items[] | {id: .id, name: .name, namespace: .namespace}' echo_step "5. Reading LiDAR Data" echo "Getting latest scan from LiDAR simulator..." @@ -90,7 +90,7 @@ curl -s "${API_BASE}/apps/gps_sim/data/fix" | jq '{ echo_step "8. Listing LiDAR Configurations" echo "These parameters can be modified at runtime to inject faults..." -curl -s "${API_BASE}/apps/lidar_sim/configurations" | jq '.[] | {name: .name, value: .value, type: .type}' +curl -s "${API_BASE}/apps/lidar_sim/configurations" | jq '.items[] | {name: .name, value: .value, type: .type}' echo_step "9. Checking Current Faults" curl -s "${API_BASE}/faults" | jq '.' diff --git a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp index d95e765..5a44e07 100644 --- a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp +++ b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp @@ -5,17 +5,19 @@ /// @brief Monitors sensor streams and reports detected anomalies as faults /// /// Subscribes to sensor topics and diagnostics, detects anomalies, -/// and publishes fault events. +/// and reports faults to the FaultManager service. #include #include #include #include +#include #include #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "rclcpp/rclcpp.hpp" +#include "ros2_medkit_msgs/srv/report_fault.hpp" #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" @@ -39,28 +41,37 @@ class AnomalyDetectorNode : public rclcpp::Node rate_timeout_sec_ = this->get_parameter("rate_timeout_sec").as_double(); max_nan_ratio_ = this->get_parameter("max_nan_ratio").as_double(); - // Create subscribers + // Create subscribers - topics are in /sensors namespace with simple names lidar_sub_ = this->create_subscription( - "/sensors/lidar_sim/scan", 10, + "/sensors/scan", 10, std::bind(&AnomalyDetectorNode::lidar_callback, this, std::placeholders::_1)); imu_sub_ = this->create_subscription( - "/sensors/imu_sim/imu", 10, + "/sensors/imu", 10, std::bind(&AnomalyDetectorNode::imu_callback, this, std::placeholders::_1)); gps_sub_ = this->create_subscription( - "/sensors/gps_sim/fix", 10, + "/sensors/fix", 10, std::bind(&AnomalyDetectorNode::gps_callback, this, std::placeholders::_1)); - // Create publisher for detected faults + // Create publisher for detected faults (legacy diagnostic topic) fault_pub_ = this->create_publisher( "detected_faults", 10); + // Create service client for FaultManager + report_fault_client_ = this->create_client( + "/fault_manager/report_fault"); + // Timer for periodic health check timer_ = this->create_wall_timer( std::chrono::seconds(1), std::bind(&AnomalyDetectorNode::check_sensor_health, this)); + // Timer for clearing passed faults (sensors recovered) + clear_timer_ = this->create_wall_timer( + std::chrono::seconds(2), + std::bind(&AnomalyDetectorNode::clear_passed_faults, this)); + RCLCPP_INFO(this->get_logger(), "Anomaly detector started"); } @@ -210,8 +221,13 @@ class AnomalyDetectorNode : public rclcpp::Node void report_fault( const std::string & source, const std::string & code, - const std::string & message) + const std::string & message, uint8_t severity = 2) { + // Track active faults for later clearing + std::string fault_key = source + ":" + code; + active_faults_.insert(fault_key); + + // Publish to diagnostic topic (legacy) auto diag_array = diagnostic_msgs::msg::DiagnosticArray(); diag_array.header.stamp = this->now(); @@ -233,7 +249,47 @@ class AnomalyDetectorNode : public rclcpp::Node diag_array.status.push_back(status); fault_pub_->publish(diag_array); - RCLCPP_WARN(this->get_logger(), "[%s] %s: %s", source.c_str(), code.c_str(), message.c_str()); + // Call FaultManager service + if (report_fault_client_->service_is_ready()) { + auto request = std::make_shared(); + request->fault_code = code; + request->event_type = ros2_medkit_msgs::srv::ReportFault::Request::EVENT_FAILED; + request->severity = severity; + request->description = message; + request->source_id = "/processing/anomaly_detector/" + source; + + // Async call - don't block + report_fault_client_->async_send_request(request); + RCLCPP_WARN(this->get_logger(), "[%s] FAULT REPORTED: %s - %s", + source.c_str(), code.c_str(), message.c_str()); + } else { + RCLCPP_WARN(this->get_logger(), "[%s] %s: %s (FaultManager unavailable)", + source.c_str(), code.c_str(), message.c_str()); + } + } + + void report_passed(const std::string & source, const std::string & code) + { + std::string fault_key = source + ":" + code; + active_faults_.erase(fault_key); + + if (report_fault_client_->service_is_ready()) { + auto request = std::make_shared(); + request->fault_code = code; + request->event_type = ros2_medkit_msgs::srv::ReportFault::Request::EVENT_PASSED; + request->severity = 0; + request->description = ""; + request->source_id = "/processing/anomaly_detector/" + source; + + report_fault_client_->async_send_request(request); + RCLCPP_INFO(this->get_logger(), "[%s] FAULT CLEARED: %s", source.c_str(), code.c_str()); + } + } + + void clear_passed_faults() + { + // Clear faults for sensors that haven't reported issues recently + // This is handled implicitly by the health check timer } // Subscribers @@ -244,8 +300,12 @@ class AnomalyDetectorNode : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr fault_pub_; - // Timer + // Service client for FaultManager + rclcpp::Client::SharedPtr report_fault_client_; + + // Timers rclcpp::TimerBase::SharedPtr timer_; + rclcpp::TimerBase::SharedPtr clear_timer_; // Parameters double rate_timeout_sec_; @@ -253,6 +313,7 @@ class AnomalyDetectorNode : public rclcpp::Node // State tracking std::map sensor_timestamps_; + std::set active_faults_; uint64_t lidar_msg_count_{0}; uint64_t imu_msg_count_{0}; uint64_t gps_msg_count_{0}; From 180ebde6391986390e19925c088247108dcb69fa Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 18:10:40 +0000 Subject: [PATCH 12/19] feat: implement dual fault reporting paths (legacy + modern) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Demonstrates two fault reporting mechanisms in ros2_medkit: LEGACY PATH (diagnostics → bridge): - LiDAR and Camera publish DiagnosticArray to /diagnostics topic - ros2_medkit_diagnostic_bridge converts and reports to fault_manager - Auto-generates fault codes from diagnostic names (e.g., LIDAR_SIM) MODERN PATH (direct service call): - IMU and GPS monitored by anomaly_detector - Anomaly detector calls /fault_manager/report_fault directly - Uses specific fault codes (SENSOR_NAN, SENSOR_TIMEOUT, etc.) Changes: - Add diagnostic_bridge and fault_reporter to Dockerfile - Update lidar_sim and camera_sim to publish DiagnosticArray - Add diagnostic_bridge node to launch file - Remove LiDAR monitoring from anomaly_detector - Update all inject scripts with path documentation - Add comprehensive dual-path documentation to README --- demos/sensor_diagnostics/Dockerfile | 2 + demos/sensor_diagnostics/README.md | 71 ++++++++++++++++-- demos/sensor_diagnostics/inject-drift.sh | 30 ++++---- demos/sensor_diagnostics/inject-failure.sh | 15 ++-- demos/sensor_diagnostics/inject-nan.sh | 28 +++++-- demos/sensor_diagnostics/inject-noise.sh | 37 +++++----- .../sensor_diagnostics/launch/demo.launch.py | 48 +++++++++--- .../src/anomaly_detector_node.cpp | 74 ++++--------------- .../src/camera_sim_node.cpp | 17 ++++- .../sensor_diagnostics/src/lidar_sim_node.cpp | 17 ++++- 10 files changed, 208 insertions(+), 131 deletions(-) diff --git a/demos/sensor_diagnostics/Dockerfile b/demos/sensor_diagnostics/Dockerfile index 082139f..0c420ba 100644 --- a/demos/sensor_diagnostics/Dockerfile +++ b/demos/sensor_diagnostics/Dockerfile @@ -33,6 +33,8 @@ RUN git clone --depth 1 --recurse-submodules https://github.com/selfpatch/ros2_m mv ros2_medkit/src/ros2_medkit_serialization . && \ mv ros2_medkit/src/ros2_medkit_msgs . && \ mv ros2_medkit/src/ros2_medkit_fault_manager . && \ + mv ros2_medkit/src/ros2_medkit_fault_reporter . && \ + mv ros2_medkit/src/ros2_medkit_diagnostic_bridge . && \ mv ros2_medkit/src/dynamic_message_introspection/dynmsg . && \ rm -rf ros2_medkit diff --git a/demos/sensor_diagnostics/README.md b/demos/sensor_diagnostics/README.md index cac76be..9f257f3 100644 --- a/demos/sensor_diagnostics/README.md +++ b/demos/sensor_diagnostics/README.md @@ -10,6 +10,7 @@ This demo showcases ros2_medkit's data monitoring, configuration management, and - **Fast startup** - Seconds vs minutes compared to TurtleBot3 demo - **Focus on diagnostics** - Pure ros2_medkit features without robot complexity - **Configurable faults** - Runtime fault injection via REST API +- **Dual fault reporting** - Demonstrates both legacy (diagnostics) and modern (direct) paths ## Quick Start @@ -47,16 +48,76 @@ ros2 launch sensor_diagnostics_demo demo.launch.py ``` Sensor Diagnostics Demo ├── /sensors # Simulated sensor nodes -│ ├── lidar_sim # 2D LiDAR (LaserScan) -│ ├── imu_sim # 9-DOF IMU (Imu) -│ ├── gps_sim # GPS receiver (NavSatFix) -│ └── camera_sim # RGB camera (Image, CameraInfo) +│ ├── lidar_sim # 2D LiDAR (LaserScan) - Legacy path +│ ├── camera_sim # RGB camera (Image) - Legacy path +│ ├── imu_sim # 9-DOF IMU (Imu) - Modern path +│ └── gps_sim # GPS receiver (NavSatFix) - Modern path ├── /processing # Data processing -│ └── anomaly_detector # Fault detection node +│ └── anomaly_detector # Fault detection (modern path) +├── /bridge # Diagnostic conversion +│ └── diagnostic_bridge # /diagnostics → FaultManager (legacy path) └── /diagnostics # Monitoring └── ros2_medkit_gateway # REST API gateway ``` +## Fault Reporting Paths + +This demo demonstrates **two different fault reporting mechanisms** available in ros2_medkit: + +### Legacy Path (ROS 2 Diagnostics → Diagnostic Bridge) + +Standard ROS 2 diagnostics pattern used by **LiDAR** and **Camera** sensors: + +``` +Sensor Node → publishes DiagnosticArray → /diagnostics topic + ↓ + diagnostic_bridge subscribes and converts + ↓ + FaultManager receives via ReportFault service +``` + +- Sensors publish `diagnostic_msgs/DiagnosticArray` to `/diagnostics` +- `ros2_medkit_diagnostic_bridge` converts DiagnosticStatus levels to fault severities: + - `OK (0)` → PASSED event (fault healing) + - `WARN (1)` → WARNING severity fault + - `ERROR (2)` → ERROR severity fault + - `STALE (3)` → CRITICAL severity fault + +### Modern Path (Direct ReportFault Service) + +Direct ros2_medkit integration used by **IMU** and **GPS** sensors: + +``` +Sensor Topics → anomaly_detector monitors + ↓ + Detects anomalies (NaN, timeout, out-of-range) + ↓ + FaultManager receives via ReportFault service +``` + +- `anomaly_detector` subscribes to sensor topics +- Analyzes data for anomalies in real-time +- Calls `/fault_manager/report_fault` service directly + +### Fault Reporting Summary + +| Sensor | Reporting Path | Fault Reporter | Fault Types | +|--------|---------------|----------------|-------------| +| **LiDAR** | Legacy (diagnostics) | diagnostic_bridge | NAN_VALUES, HIGH_NOISE, DRIFTING, TIMEOUT | +| **Camera** | Legacy (diagnostics) | diagnostic_bridge | BLACK_FRAME, HIGH_NOISE, LOW_BRIGHTNESS, OVEREXPOSED, TIMEOUT | +| **IMU** | Modern (direct) | anomaly_detector | SENSOR_NAN, SENSOR_OUT_OF_RANGE, RATE_DEGRADED, SENSOR_TIMEOUT | +| **GPS** | Modern (direct) | anomaly_detector | SENSOR_NAN, NO_FIX, SENSOR_OUT_OF_RANGE, RATE_DEGRADED, SENSOR_TIMEOUT | + +### Inject Scripts and Fault Paths + +| Script | Target Sensor | Reporting Path | Fault Scenario | +|--------|---------------|----------------|----------------| +| `inject-nan.sh` | LiDAR, IMU, GPS | Both paths | NaN values in sensor data | +| `inject-noise.sh` | LiDAR, Camera | Legacy | High noise levels | +| `inject-drift.sh` | LiDAR | Legacy | Gradual sensor drift | +| `inject-failure.sh` | IMU | Modern | Complete sensor timeout | +| `restore-normal.sh` | All | Both | Clears all faults | + ## API Examples ### Read Sensor Data diff --git a/demos/sensor_diagnostics/inject-drift.sh b/demos/sensor_diagnostics/inject-drift.sh index 0113184..4b19189 100755 --- a/demos/sensor_diagnostics/inject-drift.sh +++ b/demos/sensor_diagnostics/inject-drift.sh @@ -1,27 +1,25 @@ #!/bin/bash -# Inject sensor drift fault +# Inject sensor drift fault - demonstrates LEGACY fault reporting path +# LiDAR drift → DiagnosticArray → /diagnostics → diagnostic_bridge → FaultManager GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" API_BASE="${GATEWAY_URL}/api/v1" -echo "Injecting DRIFT fault into sensors..." +echo "Injecting DRIFT fault (Legacy path: LiDAR → diagnostic_bridge)..." +echo "" -# Set drift rates -echo "Setting LiDAR drift_rate to 0.1 m/s..." +# LiDAR drift: uses legacy diagnostics path +echo "[LEGACY PATH] Setting LiDAR drift_rate to 0.1 m/s..." +echo " Fault path: lidar_sim → /diagnostics topic → diagnostic_bridge → FaultManager" curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/drift_rate" \ -H "Content-Type: application/json" \ -d '{"value": 0.1}' -echo "Setting IMU drift_rate to 0.01 rad/s..." -curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/drift_rate" \ - -H "Content-Type: application/json" \ - -d '{"value": 0.01}' - -echo "Setting GPS drift_rate to 1.0 m/s..." -curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/drift_rate" \ - -H "Content-Type: application/json" \ - -d '{"value": 1.0}' - echo "" -echo "✓ Drift enabled! Sensor readings will gradually shift over time." -echo " Watch the drift accumulate with: ./run-demo.sh" +echo "✓ Drift enabled! LiDAR readings will gradually shift over time." +echo "" +echo "Fault codes expected (auto-generated from diagnostic name):" +echo " - LIDAR_SIM (DRIFTING status, WARN severity)" +echo "" +echo "Watch the drift accumulate with: curl ${GATEWAY_URL}/api/v1/apps/lidar_sim/data/scan | jq '.ranges[:5]'" +echo "Check faults with: curl ${GATEWAY_URL}/api/v1/faults | jq" diff --git a/demos/sensor_diagnostics/inject-failure.sh b/demos/sensor_diagnostics/inject-failure.sh index 3815745..8553aff 100755 --- a/demos/sensor_diagnostics/inject-failure.sh +++ b/demos/sensor_diagnostics/inject-failure.sh @@ -1,17 +1,20 @@ #!/bin/bash -# Inject sensor failure (timeout) fault +# Inject sensor failure (timeout) fault - demonstrates MODERN fault reporting path +# IMU sensor → anomaly_detector → FaultManager (via ReportFault service) GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" API_BASE="${GATEWAY_URL}/api/v1" -echo "Injecting SENSOR FAILURE fault..." +echo "Injecting SENSOR FAILURE fault (Modern path: IMU → anomaly_detector)..." -# Set high failure probability - sensors will stop publishing -echo "Setting LiDAR failure_probability to 1.0 (complete failure)..." -curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/failure_probability" \ +# Set high failure probability - IMU will stop publishing +echo "Setting IMU failure_probability to 1.0 (complete failure)..." +curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/failure_probability" \ -H "Content-Type: application/json" \ -d '{"value": 1.0}' echo "" -echo "✓ Sensor failure injected! The anomaly detector should report SENSOR_TIMEOUT." +echo "✓ IMU failure injected!" +echo " Fault reporting path: imu_sim → anomaly_detector → /fault_manager/report_fault" +echo " The anomaly detector should report SENSOR_TIMEOUT fault directly to FaultManager." echo " Check faults with: curl ${API_BASE}/faults | jq" diff --git a/demos/sensor_diagnostics/inject-nan.sh b/demos/sensor_diagnostics/inject-nan.sh index 92de664..f46ae2e 100755 --- a/demos/sensor_diagnostics/inject-nan.sh +++ b/demos/sensor_diagnostics/inject-nan.sh @@ -1,27 +1,39 @@ #!/bin/bash -# Inject NaN values fault into sensors +# Inject NaN values fault - demonstrates BOTH fault reporting paths GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" API_BASE="${GATEWAY_URL}/api/v1" -echo "Injecting NaN VALUES fault into sensors..." +echo "Injecting NaN VALUES fault (demonstrates both fault reporting paths)..." +echo "" -# Enable NaN injection on sensors -echo "Enabling LiDAR inject_nan..." +# LEGACY PATH: LiDAR publishes DiagnosticArray → diagnostic_bridge → FaultManager +echo "[LEGACY PATH] Enabling LiDAR inject_nan..." +echo " Fault path: lidar_sim → /diagnostics topic → diagnostic_bridge → FaultManager" curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/inject_nan" \ -H "Content-Type: application/json" \ -d '{"value": true}' +echo "" -echo "Enabling IMU inject_nan..." +# MODERN PATH: IMU/GPS → anomaly_detector → FaultManager (direct service call) +echo "[MODERN PATH] Enabling IMU inject_nan..." +echo " Fault path: imu_sim → anomaly_detector → /fault_manager/report_fault" curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/inject_nan" \ -H "Content-Type: application/json" \ -d '{"value": true}' +echo "" -echo "Enabling GPS inject_nan..." +echo "[MODERN PATH] Enabling GPS inject_nan..." +echo " Fault path: gps_sim → anomaly_detector → /fault_manager/report_fault" curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/inject_nan" \ -H "Content-Type: application/json" \ -d '{"value": true}' echo "" -echo "✓ NaN injection enabled! Anomaly detector should report SENSOR_NAN faults." -echo " Check faults with: curl ${API_BASE}/faults | jq" +echo "✓ NaN injection enabled on multiple sensors!" +echo "" +echo "Fault codes expected:" +echo " - LIDAR_SIM (from diagnostic_bridge, auto-generated from diagnostic name)" +echo " - SENSOR_NAN (from anomaly_detector)" +echo "" +echo "Check faults with: curl ${API_BASE}/faults | jq" diff --git a/demos/sensor_diagnostics/inject-noise.sh b/demos/sensor_diagnostics/inject-noise.sh index b61ce27..52849af 100755 --- a/demos/sensor_diagnostics/inject-noise.sh +++ b/demos/sensor_diagnostics/inject-noise.sh @@ -1,34 +1,33 @@ #!/bin/bash -# Inject high noise fault into sensors +# Inject high noise fault - demonstrates LEGACY fault reporting path +# LiDAR/Camera → DiagnosticArray → /diagnostics → diagnostic_bridge → FaultManager GATEWAY_URL="${GATEWAY_URL:-http://localhost:8080}" API_BASE="${GATEWAY_URL}/api/v1" -echo "Injecting HIGH NOISE fault into sensors..." +echo "Injecting HIGH NOISE fault (Legacy path: LiDAR/Camera → diagnostic_bridge)..." +echo "" -# LiDAR: increase noise stddev -echo "Setting LiDAR noise_stddev to 0.5 (very noisy)..." +# LiDAR: increase noise stddev (uses legacy diagnostics path) +echo "[LEGACY PATH] Setting LiDAR noise_stddev to 0.5 (very noisy)..." +echo " Fault path: lidar_sim → /diagnostics topic → diagnostic_bridge → FaultManager" curl -s -X PUT "${API_BASE}/apps/lidar_sim/configurations/noise_stddev" \ -H "Content-Type: application/json" \ -d '{"value": 0.5}' +echo "" -# IMU: increase noise -echo "Setting IMU accel_noise_stddev to 0.5..." -curl -s -X PUT "${API_BASE}/apps/imu_sim/configurations/accel_noise_stddev" \ - -H "Content-Type: application/json" \ - -d '{"value": 0.5}' - -# GPS: increase position noise -echo "Setting GPS position_noise_stddev to 50 meters..." -curl -s -X PUT "${API_BASE}/apps/gps_sim/configurations/position_noise_stddev" \ - -H "Content-Type: application/json" \ - -d '{"value": 50.0}' - -# Camera: add noise -echo "Setting Camera noise_level to 0.3..." +# Camera: add noise (uses legacy diagnostics path) +echo "[LEGACY PATH] Setting Camera noise_level to 0.3..." +echo " Fault path: camera_sim → /diagnostics topic → diagnostic_bridge → FaultManager" curl -s -X PUT "${API_BASE}/apps/camera_sim/configurations/noise_level" \ -H "Content-Type: application/json" \ -d '{"value": 0.3}' echo "" -echo "✓ High noise injected! Check faults with: ./run-demo.sh (step 9)" +echo "✓ High noise injected on LiDAR and Camera!" +echo "" +echo "Fault codes expected (auto-generated from diagnostic names):" +echo " - LIDAR_SIM (HIGH_NOISE status)" +echo " - CAMERA_SIM (HIGH_NOISE status)" +echo "" +echo "Check faults with: curl ${API_BASE}/faults | jq" diff --git a/demos/sensor_diagnostics/launch/demo.launch.py b/demos/sensor_diagnostics/launch/demo.launch.py index 9b647c5..68b6d29 100644 --- a/demos/sensor_diagnostics/launch/demo.launch.py +++ b/demos/sensor_diagnostics/launch/demo.launch.py @@ -2,10 +2,20 @@ Lightweight demo without Gazebo - pure sensor simulation with fault injection. +Demonstrates two fault reporting paths: +1. Legacy path: Sensors → /diagnostics topic → diagnostic_bridge → fault_manager + - Used by: LiDAR, Camera + - Standard ROS 2 diagnostics pattern + +2. Modern path: Sensors → anomaly_detector → ReportFault service → fault_manager + - Used by: IMU, GPS + - Direct ros2_medkit fault reporting + Namespace structure: /sensors - Simulated sensor nodes (lidar, imu, gps, camera) /processing - Anomaly detector /diagnostics - ros2_medkit gateway + /bridge - Diagnostic bridge (legacy path) """ import os @@ -38,6 +48,7 @@ def generate_launch_description(): description="Use simulation time (set to true if using with Gazebo)", ), # ===== Sensor Nodes (under /sensors namespace) ===== + # Legacy path sensors: publish DiagnosticArray to /diagnostics Node( package="sensor_diagnostics_demo", executable="lidar_sim_node", @@ -48,29 +59,31 @@ def generate_launch_description(): ), Node( package="sensor_diagnostics_demo", - executable="imu_sim_node", - name="imu_sim", + executable="camera_sim_node", + name="camera_sim", namespace="sensors", output="screen", parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], ), + # Modern path sensors: monitored by anomaly_detector → ReportFault Node( package="sensor_diagnostics_demo", - executable="gps_sim_node", - name="gps_sim", + executable="imu_sim_node", + name="imu_sim", namespace="sensors", output="screen", parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], ), Node( package="sensor_diagnostics_demo", - executable="camera_sim_node", - name="camera_sim", + executable="gps_sim_node", + name="gps_sim", namespace="sensors", output="screen", parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], ), # ===== Processing Nodes (under /processing namespace) ===== + # Modern path: anomaly_detector monitors IMU/GPS and calls ReportFault Node( package="sensor_diagnostics_demo", executable="anomaly_detector_node", @@ -79,6 +92,23 @@ def generate_launch_description(): output="screen", parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], ), + # ===== Diagnostic Bridge (Legacy path) ===== + # Bridges /diagnostics topic (DiagnosticArray) → fault_manager + # Handles faults from: LiDAR, Camera + Node( + package="ros2_medkit_diagnostic_bridge", + executable="diagnostic_bridge_node", + name="diagnostic_bridge", + namespace="bridge", + output="screen", + parameters=[ + { + "use_sim_time": use_sim_time, + "diagnostics_topic": "/diagnostics", + "auto_generate_codes": True, + } + ], + ), # ===== ros2_medkit Gateway (under /diagnostics namespace) ===== Node( package="ros2_medkit_gateway", @@ -92,9 +122,9 @@ def generate_launch_description(): {"manifest.path": manifest_file}, ], ), - # ===== Fault Manager (under /fault_manager namespace) ===== - # Note: Gateway expects services at /fault_manager/*, so we use root namespace - # and node name "fault_manager" to get /fault_manager/get_faults etc. + # ===== Fault Manager (at root namespace) ===== + # Services at /fault_manager/* (e.g., /fault_manager/report_fault) + # Both paths report here: diagnostic_bridge (legacy) and anomaly_detector (modern) Node( package="ros2_medkit_fault_manager", executable="fault_manager_node", diff --git a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp index 5a44e07..3e423d4 100644 --- a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp +++ b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp @@ -2,10 +2,14 @@ // SPDX-License-Identifier: Apache-2.0 /// @file anomaly_detector_node.cpp -/// @brief Monitors sensor streams and reports detected anomalies as faults +/// @brief Monitors IMU and GPS sensors and reports detected anomalies as faults /// -/// Subscribes to sensor topics and diagnostics, detects anomalies, -/// and reports faults to the FaultManager service. +/// This node implements the MODERN fault reporting path: +/// - Subscribes to IMU and GPS topics +/// - Detects anomalies (NaN values, out-of-range, timeouts) +/// - Reports faults directly to FaultManager via ReportFault service +/// +/// Note: LiDAR and Camera use the LEGACY path (diagnostics → diagnostic_bridge → FaultManager) #include #include @@ -19,7 +23,6 @@ #include "rclcpp/rclcpp.hpp" #include "ros2_medkit_msgs/srv/report_fault.hpp" #include "sensor_msgs/msg/imu.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" namespace sensor_diagnostics @@ -34,18 +37,14 @@ class AnomalyDetectorNode : public rclcpp::Node // Declare parameters for thresholds this->declare_parameter("rate_timeout_sec", 5.0); this->declare_parameter("max_nan_ratio", 0.1); - this->declare_parameter("lidar_rate_min", 5.0); this->declare_parameter("imu_rate_min", 50.0); this->declare_parameter("gps_rate_min", 0.5); rate_timeout_sec_ = this->get_parameter("rate_timeout_sec").as_double(); max_nan_ratio_ = this->get_parameter("max_nan_ratio").as_double(); - // Create subscribers - topics are in /sensors namespace with simple names - lidar_sub_ = this->create_subscription( - "/sensors/scan", 10, - std::bind(&AnomalyDetectorNode::lidar_callback, this, std::placeholders::_1)); - + // Create subscribers for MODERN path sensors (IMU, GPS) + // Note: LiDAR and Camera use LEGACY path via /diagnostics → diagnostic_bridge imu_sub_ = this->create_subscription( "/sensors/imu", 10, std::bind(&AnomalyDetectorNode::imu_callback, this, std::placeholders::_1)); @@ -54,11 +53,11 @@ class AnomalyDetectorNode : public rclcpp::Node "/sensors/fix", 10, std::bind(&AnomalyDetectorNode::gps_callback, this, std::placeholders::_1)); - // Create publisher for detected faults (legacy diagnostic topic) + // Create publisher for detected faults (supplementary diagnostic topic) fault_pub_ = this->create_publisher( "detected_faults", 10); - // Create service client for FaultManager + // Create service client for FaultManager (MODERN path) report_fault_client_ = this->create_client( "/fault_manager/report_fault"); @@ -72,42 +71,10 @@ class AnomalyDetectorNode : public rclcpp::Node std::chrono::seconds(2), std::bind(&AnomalyDetectorNode::clear_passed_faults, this)); - RCLCPP_INFO(this->get_logger(), "Anomaly detector started"); + RCLCPP_INFO(this->get_logger(), "Anomaly detector started (modern path: IMU, GPS)"); } private: - void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) - { - sensor_timestamps_["lidar"] = this->now(); - lidar_msg_count_++; - - // Check for NaN values - int nan_count = 0; - for (const auto & range : msg->ranges) { - if (std::isnan(range)) { - nan_count++; - } - } - - double nan_ratio = static_cast(nan_count) / static_cast(msg->ranges.size()); - if (nan_ratio > max_nan_ratio_) { - report_fault("lidar_sim", "SENSOR_NAN", - "LiDAR has " + std::to_string(static_cast(nan_ratio * 100)) + "% NaN values"); - } - - // Check for all-zero ranges (sensor malfunction) - bool all_min = true; - for (const auto & range : msg->ranges) { - if (!std::isnan(range) && range > msg->range_min + 0.01) { - all_min = false; - break; - } - } - if (all_min && !msg->ranges.empty()) { - report_fault("lidar_sim", "SENSOR_MALFUNCTION", "LiDAR returns all minimum range values"); - } - } - void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) { sensor_timestamps_["imu"] = this->now(); @@ -168,32 +135,22 @@ class AnomalyDetectorNode : public rclcpp::Node { auto now = this->now(); - // Check for sensor timeouts - check_timeout("lidar", now); + // Check for sensor timeouts (modern path sensors only) check_timeout("imu", now); check_timeout("gps", now); // Calculate and log rates double elapsed = 1.0; // 1 second timer - double lidar_rate = (lidar_msg_count_ - last_lidar_count_) / elapsed; double imu_rate = (imu_msg_count_ - last_imu_count_) / elapsed; double gps_rate = (gps_msg_count_ - last_gps_count_) / elapsed; - last_lidar_count_ = lidar_msg_count_; last_imu_count_ = imu_msg_count_; last_gps_count_ = gps_msg_count_; // Check for degraded rates - double lidar_rate_min = this->get_parameter("lidar_rate_min").as_double(); double imu_rate_min = this->get_parameter("imu_rate_min").as_double(); double gps_rate_min = this->get_parameter("gps_rate_min").as_double(); - if (lidar_rate > 0 && lidar_rate < lidar_rate_min) { - report_fault("lidar_sim", "RATE_DEGRADED", - "LiDAR rate: " + std::to_string(lidar_rate) + " Hz (min: " + - std::to_string(lidar_rate_min) + ")"); - } - if (imu_rate > 0 && imu_rate < imu_rate_min) { report_fault("imu_sim", "RATE_DEGRADED", "IMU rate: " + std::to_string(imu_rate) + " Hz (min: " + @@ -292,8 +249,7 @@ class AnomalyDetectorNode : public rclcpp::Node // This is handled implicitly by the health check timer } - // Subscribers - rclcpp::Subscription::SharedPtr lidar_sub_; + // Subscribers (modern path: IMU, GPS only) rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr gps_sub_; @@ -314,10 +270,8 @@ class AnomalyDetectorNode : public rclcpp::Node // State tracking std::map sensor_timestamps_; std::set active_faults_; - uint64_t lidar_msg_count_{0}; uint64_t imu_msg_count_{0}; uint64_t gps_msg_count_{0}; - uint64_t last_lidar_count_{0}; uint64_t last_imu_count_{0}; uint64_t last_gps_count_{0}; }; diff --git a/demos/sensor_diagnostics/src/camera_sim_node.cpp b/demos/sensor_diagnostics/src/camera_sim_node.cpp index 815cfaa..f573545 100644 --- a/demos/sensor_diagnostics/src/camera_sim_node.cpp +++ b/demos/sensor_diagnostics/src/camera_sim_node.cpp @@ -6,6 +6,8 @@ /// /// Publishes simulated Image and CameraInfo messages /// and supports runtime fault injection via ROS 2 parameters. +/// Diagnostics are published to /diagnostics for the legacy fault reporting path +/// via ros2_medkit_diagnostic_bridge. #include #include @@ -13,6 +15,7 @@ #include #include +#include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/key_value.hpp" #include "rclcpp/rclcpp.hpp" @@ -46,8 +49,9 @@ class CameraSimNode : public rclcpp::Node // Create publishers image_pub_ = this->create_publisher("image_raw", 10); info_pub_ = this->create_publisher("camera_info", 10); - diag_pub_ = this->create_publisher( - "diagnostics", 10); + // Publish to absolute /diagnostics topic for legacy fault reporting via diagnostic_bridge + diag_pub_ = this->create_publisher( + "/diagnostics", 10); // Create timer double rate = this->get_parameter("rate").as_double(); @@ -195,8 +199,12 @@ class CameraSimNode : public rclcpp::Node void publish_diagnostics(const std::string & status, const std::string & message) { + auto diag_array = diagnostic_msgs::msg::DiagnosticArray(); + diag_array.header.stamp = this->now(); + auto diag = diagnostic_msgs::msg::DiagnosticStatus(); diag.name = "camera_sim"; + diag.hardware_id = "camera_sensor"; if (status == "OK") { diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -225,13 +233,14 @@ class CameraSimNode : public rclcpp::Node kv.value = std::to_string(noise_level_); diag.values.push_back(kv); - diag_pub_->publish(diag); + diag_array.status.push_back(diag); + diag_pub_->publish(diag_array); } // Publishers rclcpp::Publisher::SharedPtr image_pub_; rclcpp::Publisher::SharedPtr info_pub_; - rclcpp::Publisher::SharedPtr diag_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/demos/sensor_diagnostics/src/lidar_sim_node.cpp b/demos/sensor_diagnostics/src/lidar_sim_node.cpp index da1da7d..391b2b7 100644 --- a/demos/sensor_diagnostics/src/lidar_sim_node.cpp +++ b/demos/sensor_diagnostics/src/lidar_sim_node.cpp @@ -6,6 +6,8 @@ /// /// This node publishes simulated LaserScan messages with realistic patterns /// and supports runtime fault injection via ROS 2 parameters. +/// Diagnostics are published to /diagnostics for the legacy fault reporting path +/// via ros2_medkit_diagnostic_bridge. #include #include @@ -15,6 +17,7 @@ #include #include +#include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/key_value.hpp" #include "rclcpp/rclcpp.hpp" @@ -51,8 +54,9 @@ class LidarSimNode : public rclcpp::Node // Create publisher scan_pub_ = this->create_publisher("scan", 10); - diag_pub_ = this->create_publisher( - "diagnostics", 10); + // Publish to absolute /diagnostics topic for legacy fault reporting via diagnostic_bridge + diag_pub_ = this->create_publisher( + "/diagnostics", 10); // Calculate publish period from rate double rate = this->get_parameter("scan_rate").as_double(); @@ -213,8 +217,12 @@ class LidarSimNode : public rclcpp::Node void publish_diagnostics(const std::string & status, const std::string & message) { + auto diag_array = diagnostic_msgs::msg::DiagnosticArray(); + diag_array.header.stamp = this->now(); + auto diag = diagnostic_msgs::msg::DiagnosticStatus(); diag.name = "lidar_sim"; + diag.hardware_id = "lidar_sensor"; if (status == "OK") { diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -244,12 +252,13 @@ class LidarSimNode : public rclcpp::Node kv.value = std::to_string(failure_probability_); diag.values.push_back(kv); - diag_pub_->publish(diag); + diag_array.status.push_back(diag); + diag_pub_->publish(diag_array); } // Publishers rclcpp::Publisher::SharedPtr scan_pub_; - rclcpp::Publisher::SharedPtr diag_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; // Timer rclcpp::TimerBase::SharedPtr timer_; From fc395d4bd5423bd2513ebba239b59ac2ce8ed994 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 18:16:53 +0000 Subject: [PATCH 13/19] feat: add fault clearing to restore-normal.sh script --- demos/sensor_diagnostics/restore-normal.sh | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/demos/sensor_diagnostics/restore-normal.sh b/demos/sensor_diagnostics/restore-normal.sh index 7553b73..1a3e981 100755 --- a/demos/sensor_diagnostics/restore-normal.sh +++ b/demos/sensor_diagnostics/restore-normal.sh @@ -48,5 +48,14 @@ curl -s -X PUT "${API_BASE}/apps/camera_sim/configurations/failure_probability" curl -s -X PUT "${API_BASE}/apps/camera_sim/configurations/inject_black_frames" \ -H "Content-Type: application/json" -d '{"value": false}' +# Clear all faults from FaultManager echo "" -echo "✓ Normal operation restored! All fault injections cleared." +echo "Clearing all faults from FaultManager..." +curl -s -X DELETE "${API_BASE}/apps/lidar_sim/faults" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/camera_sim/faults" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/imu_sim/faults" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/gps_sim/faults" > /dev/null 2>&1 + +echo "" +echo "✓ Normal operation restored! All fault injections and faults cleared." +echo " Verify with: curl ${API_BASE}/faults | jq" From 95735a552775a16f35a138f499a1542635181561 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 18:30:58 +0000 Subject: [PATCH 14/19] fix: address PR review comments from Copilot MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add missing include for std::clamp (lidar, camera) - Validate rate parameters to prevent division by zero (all sensors) - Remove unused max_nan_ratio parameter from anomaly_detector - Remove unused lidar_rate_min from config (LiDAR uses legacy path now) - Remove empty clear_passed_faults function and clear_timer_ - Fix DRIFT_DETECTED → DRIFTING in README fault table - Add documentation to simulated_sensor_base.hpp explaining future use --- demos/sensor_diagnostics/README.md | 2 +- .../config/sensor_params.yaml | 7 ++-- .../simulated_sensor_base.hpp | 8 +++++ .../src/anomaly_detector_node.cpp | 36 ++----------------- .../src/camera_sim_node.cpp | 10 +++++- demos/sensor_diagnostics/src/gps_sim_node.cpp | 9 ++++- demos/sensor_diagnostics/src/imu_sim_node.cpp | 9 ++++- .../sensor_diagnostics/src/lidar_sim_node.cpp | 11 +++++- 8 files changed, 50 insertions(+), 42 deletions(-) diff --git a/demos/sensor_diagnostics/README.md b/demos/sensor_diagnostics/README.md index 9f257f3..a679252 100644 --- a/demos/sensor_diagnostics/README.md +++ b/demos/sensor_diagnostics/README.md @@ -176,7 +176,7 @@ curl http://localhost:8080/api/v1/faults | jq | `SENSOR_TIMEOUT` | No messages published | `failure_probability: 1.0` | | `SENSOR_NAN` | Invalid readings | `inject_nan: true` | | `HIGH_NOISE` | Degraded accuracy | `noise_stddev: 0.5` | -| `DRIFT_DETECTED` | Gradual sensor drift | `drift_rate: 0.1` | +| `DRIFTING` | Gradual sensor drift | `drift_rate: 0.1` | | `BLACK_FRAME` | Camera black frames | `inject_black_frames: true` | ## Scripts diff --git a/demos/sensor_diagnostics/config/sensor_params.yaml b/demos/sensor_diagnostics/config/sensor_params.yaml index 4878f14..a7a2c88 100644 --- a/demos/sensor_diagnostics/config/sensor_params.yaml +++ b/demos/sensor_diagnostics/config/sensor_params.yaml @@ -50,7 +50,6 @@ processing: anomaly_detector: ros__parameters: rate_timeout_sec: 5.0 # seconds before timeout fault - max_nan_ratio: 0.1 # max NaN values before fault - lidar_rate_min: 5.0 # Hz - imu_rate_min: 50.0 # Hz - gps_rate_min: 0.5 # Hz + imu_rate_min: 50.0 # Hz (anomaly detector monitors IMU via modern path) + gps_rate_min: 0.5 # Hz (anomaly detector monitors GPS via modern path) + # Note: LiDAR and Camera use legacy path via diagnostic_bridge diff --git a/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp b/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp index 957559e..61a5bcb 100644 --- a/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp +++ b/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp @@ -1,6 +1,14 @@ // Copyright 2025 selfpatch // SPDX-License-Identifier: Apache-2.0 +/// @file simulated_sensor_base.hpp +/// @brief Base class for simulated sensors with fault injection +/// +/// NOTE: This header provides a reusable base class for simulated sensors. +/// Currently, sensor nodes implement fault injection directly for simplicity. +/// This base class is retained for future refactoring when sensors need to +/// share common fault injection logic. + #pragma once #include diff --git a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp index 3e423d4..285ecda 100644 --- a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp +++ b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp @@ -36,12 +36,10 @@ class AnomalyDetectorNode : public rclcpp::Node { // Declare parameters for thresholds this->declare_parameter("rate_timeout_sec", 5.0); - this->declare_parameter("max_nan_ratio", 0.1); this->declare_parameter("imu_rate_min", 50.0); this->declare_parameter("gps_rate_min", 0.5); rate_timeout_sec_ = this->get_parameter("rate_timeout_sec").as_double(); - max_nan_ratio_ = this->get_parameter("max_nan_ratio").as_double(); // Create subscribers for MODERN path sensors (IMU, GPS) // Note: LiDAR and Camera use LEGACY path via /diagnostics → diagnostic_bridge @@ -66,11 +64,6 @@ class AnomalyDetectorNode : public rclcpp::Node std::chrono::seconds(1), std::bind(&AnomalyDetectorNode::check_sensor_health, this)); - // Timer for clearing passed faults (sensors recovered) - clear_timer_ = this->create_wall_timer( - std::chrono::seconds(2), - std::bind(&AnomalyDetectorNode::clear_passed_faults, this)); - RCLCPP_INFO(this->get_logger(), "Anomaly detector started (modern path: IMU, GPS)"); } @@ -225,29 +218,8 @@ class AnomalyDetectorNode : public rclcpp::Node } } - void report_passed(const std::string & source, const std::string & code) - { - std::string fault_key = source + ":" + code; - active_faults_.erase(fault_key); - - if (report_fault_client_->service_is_ready()) { - auto request = std::make_shared(); - request->fault_code = code; - request->event_type = ros2_medkit_msgs::srv::ReportFault::Request::EVENT_PASSED; - request->severity = 0; - request->description = ""; - request->source_id = "/processing/anomaly_detector/" + source; - - report_fault_client_->async_send_request(request); - RCLCPP_INFO(this->get_logger(), "[%s] FAULT CLEARED: %s", source.c_str(), code.c_str()); - } - } - - void clear_passed_faults() - { - // Clear faults for sensors that haven't reported issues recently - // This is handled implicitly by the health check timer - } + // Note: report_passed() can be used to clear faults when sensors recover. + // Currently fault clearing is handled by restore-normal.sh script via REST API. // Subscribers (modern path: IMU, GPS only) rclcpp::Subscription::SharedPtr imu_sub_; @@ -259,13 +231,11 @@ class AnomalyDetectorNode : public rclcpp::Node // Service client for FaultManager rclcpp::Client::SharedPtr report_fault_client_; - // Timers + // Timer rclcpp::TimerBase::SharedPtr timer_; - rclcpp::TimerBase::SharedPtr clear_timer_; // Parameters double rate_timeout_sec_; - double max_nan_ratio_; // State tracking std::map sensor_timestamps_; diff --git a/demos/sensor_diagnostics/src/camera_sim_node.cpp b/demos/sensor_diagnostics/src/camera_sim_node.cpp index f573545..9031395 100644 --- a/demos/sensor_diagnostics/src/camera_sim_node.cpp +++ b/demos/sensor_diagnostics/src/camera_sim_node.cpp @@ -9,6 +9,7 @@ /// Diagnostics are published to /diagnostics for the legacy fault reporting path /// via ros2_medkit_diagnostic_bridge. +#include #include #include #include @@ -53,8 +54,15 @@ class CameraSimNode : public rclcpp::Node diag_pub_ = this->create_publisher( "/diagnostics", 10); - // Create timer + // Create timer (with rate validation) double rate = this->get_parameter("rate").as_double(); + if (rate <= 0.0) { + RCLCPP_WARN( + this->get_logger(), + "Parameter 'rate' must be > 0.0 Hz, but got %.3f. Falling back to 30.0 Hz.", rate); + rate = 30.0; + this->set_parameters({rclcpp::Parameter("rate", rate)}); + } auto period = std::chrono::duration(1.0 / rate); timer_ = this->create_wall_timer( std::chrono::duration_cast(period), diff --git a/demos/sensor_diagnostics/src/gps_sim_node.cpp b/demos/sensor_diagnostics/src/gps_sim_node.cpp index 4859dcf..3adc2f2 100644 --- a/demos/sensor_diagnostics/src/gps_sim_node.cpp +++ b/demos/sensor_diagnostics/src/gps_sim_node.cpp @@ -51,8 +51,15 @@ class GpsSimNode : public rclcpp::Node diag_pub_ = this->create_publisher( "diagnostics", 10); - // Create timer + // Create timer (with rate validation) double rate = this->get_parameter("rate").as_double(); + if (rate <= 0.0) { + RCLCPP_WARN( + this->get_logger(), + "Invalid GPS publish rate (%f Hz) configured; using default of 1.0 Hz instead.", + rate); + rate = 1.0; + } auto period = std::chrono::duration(1.0 / rate); timer_ = this->create_wall_timer( std::chrono::duration_cast(period), diff --git a/demos/sensor_diagnostics/src/imu_sim_node.cpp b/demos/sensor_diagnostics/src/imu_sim_node.cpp index 15619c7..efe398e 100644 --- a/demos/sensor_diagnostics/src/imu_sim_node.cpp +++ b/demos/sensor_diagnostics/src/imu_sim_node.cpp @@ -47,8 +47,15 @@ class ImuSimNode : public rclcpp::Node diag_pub_ = this->create_publisher( "diagnostics", 10); - // Create timer + // Create timer (with rate validation) double rate = this->get_parameter("rate").as_double(); + if (rate <= 0.0) { + RCLCPP_WARN( + this->get_logger(), + "Parameter 'rate' must be positive; using default 100.0 Hz instead of %.3f", + rate); + rate = 100.0; + } auto period = std::chrono::duration(1.0 / rate); timer_ = this->create_wall_timer( std::chrono::duration_cast(period), diff --git a/demos/sensor_diagnostics/src/lidar_sim_node.cpp b/demos/sensor_diagnostics/src/lidar_sim_node.cpp index 391b2b7..8f15856 100644 --- a/demos/sensor_diagnostics/src/lidar_sim_node.cpp +++ b/demos/sensor_diagnostics/src/lidar_sim_node.cpp @@ -9,6 +9,7 @@ /// Diagnostics are published to /diagnostics for the legacy fault reporting path /// via ros2_medkit_diagnostic_bridge. +#include #include #include #include @@ -58,8 +59,16 @@ class LidarSimNode : public rclcpp::Node diag_pub_ = this->create_publisher( "/diagnostics", 10); - // Calculate publish period from rate + // Calculate publish period from rate (with validation) double rate = this->get_parameter("scan_rate").as_double(); + if (rate <= 0.0) { + RCLCPP_WARN( + this->get_logger(), + "Invalid scan_rate parameter value (%f Hz). Using default 10.0 Hz instead.", + rate); + rate = 10.0; + this->set_parameters({rclcpp::Parameter("scan_rate", rate)}); + } auto period = std::chrono::duration(1.0 / rate); // Create timer From f10b56314fcc97afc48bdb933d7ddc846beb1814 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 25 Jan 2026 19:01:12 +0000 Subject: [PATCH 15/19] fix: async_send_request with callback + correct fault clearing paths - Add response callback to async_send_request() in anomaly_detector to ensure service requests are properly processed by the executor - Fix restore-normal.sh to use correct entity paths for fault clearing: - Legacy path faults: /apps/diagnostic_bridge/faults/{fault_code} - Modern path faults: /apps/anomaly_detector/faults/{fault_code} --- demos/sensor_diagnostics/restore-normal.sh | 15 +++++++++++---- .../src/anomaly_detector_node.cpp | 17 +++++++++++++++-- 2 files changed, 26 insertions(+), 6 deletions(-) diff --git a/demos/sensor_diagnostics/restore-normal.sh b/demos/sensor_diagnostics/restore-normal.sh index 1a3e981..48a03a2 100755 --- a/demos/sensor_diagnostics/restore-normal.sh +++ b/demos/sensor_diagnostics/restore-normal.sh @@ -49,12 +49,19 @@ curl -s -X PUT "${API_BASE}/apps/camera_sim/configurations/inject_black_frames" -H "Content-Type: application/json" -d '{"value": false}' # Clear all faults from FaultManager +# Legacy path faults (via diagnostic_bridge) - reported BY diagnostic_bridge +# The fault_code is the name from DiagnosticStatus.name (e.g., LIDAR_SIM, CAMERA_SIM) echo "" echo "Clearing all faults from FaultManager..." -curl -s -X DELETE "${API_BASE}/apps/lidar_sim/faults" > /dev/null 2>&1 -curl -s -X DELETE "${API_BASE}/apps/camera_sim/faults" > /dev/null 2>&1 -curl -s -X DELETE "${API_BASE}/apps/imu_sim/faults" > /dev/null 2>&1 -curl -s -X DELETE "${API_BASE}/apps/gps_sim/faults" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/diagnostic_bridge/faults/LIDAR_SIM" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/diagnostic_bridge/faults/CAMERA_SIM" > /dev/null 2>&1 + +# Modern path faults (via anomaly_detector direct service call) +curl -s -X DELETE "${API_BASE}/apps/anomaly_detector/faults/SENSOR_TIMEOUT" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/anomaly_detector/faults/SENSOR_NAN" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/anomaly_detector/faults/SENSOR_OUT_OF_RANGE" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/anomaly_detector/faults/RATE_DEGRADED" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/anomaly_detector/faults/NO_FIX" > /dev/null 2>&1 echo "" echo "✓ Normal operation restored! All fault injections and faults cleared." diff --git a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp index 285ecda..fb562de 100644 --- a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp +++ b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp @@ -208,8 +208,21 @@ class AnomalyDetectorNode : public rclcpp::Node request->description = message; request->source_id = "/processing/anomaly_detector/" + source; - // Async call - don't block - report_fault_client_->async_send_request(request); + // Async call with callback to ensure request is processed + auto future = report_fault_client_->async_send_request(request, + [this, code, source](rclcpp::Client::SharedFuture response) { + try { + auto result = response.get(); + if (!result->accepted) { + RCLCPP_ERROR(this->get_logger(), "[%s] Fault %s rejected by FaultManager", + source.c_str(), code.c_str()); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "[%s] Service call failed: %s", + source.c_str(), e.what()); + } + }); + (void)future; // Suppress unused warning - callback handles response RCLCPP_WARN(this->get_logger(), "[%s] FAULT REPORTED: %s - %s", source.c_str(), code.c_str(), message.c_str()); } else { From 6e0c47b79fff7517742e6a0fcabbf07025ae1f92 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Mon, 26 Jan 2026 14:43:58 +0000 Subject: [PATCH 16/19] fix: address all PR #14 review comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Fixes from @mfaferek93: - Update copyright year 2025 → 2026 in all source files - Add early return in report_fault() to avoid spamming FaultManager - Remove unused normal_dist_ member from camera_sim_node - Unify diagnostic topic to /diagnostics for all sensors (IMU, GPS, LiDAR, Camera) Fixes from @Copilot: - Add rcl_interfaces to ament_target_dependencies for lidar/camera nodes - Fix docker-compose jq filter: .[] → .items[] - Add exec_depend for ros2_medkit_diagnostic_bridge and fault_manager - Fix anomaly_detector topic subscriptions to match sensor namespaces - Add runtime scan_rate validation in lidar parameter callback - Add num_readings validation with fallback to 360 Additional fixes: - Update restore-normal.sh to clear IMU_SIM and GPS_SIM faults --- demos/sensor_diagnostics/CMakeLists.txt | 2 ++ demos/sensor_diagnostics/docker-compose.yml | 2 +- .../simulated_sensor_base.hpp | 2 +- demos/sensor_diagnostics/package.xml | 2 ++ demos/sensor_diagnostics/restore-normal.sh | 7 ++++--- .../src/anomaly_detector_node.cpp | 12 +++++++---- .../src/camera_sim_node.cpp | 4 +--- demos/sensor_diagnostics/src/gps_sim_node.cpp | 17 ++++++++++----- demos/sensor_diagnostics/src/imu_sim_node.cpp | 17 ++++++++++----- .../sensor_diagnostics/src/lidar_sim_node.cpp | 21 +++++++++++++++++-- 10 files changed, 62 insertions(+), 24 deletions(-) diff --git a/demos/sensor_diagnostics/CMakeLists.txt b/demos/sensor_diagnostics/CMakeLists.txt index cadfea8..09144bc 100644 --- a/demos/sensor_diagnostics/CMakeLists.txt +++ b/demos/sensor_diagnostics/CMakeLists.txt @@ -21,6 +21,7 @@ ament_target_dependencies(lidar_sim_node rclcpp sensor_msgs diagnostic_msgs + rcl_interfaces ) # IMU simulator node @@ -45,6 +46,7 @@ ament_target_dependencies(camera_sim_node rclcpp sensor_msgs diagnostic_msgs + rcl_interfaces ) # Anomaly detector node diff --git a/demos/sensor_diagnostics/docker-compose.yml b/demos/sensor_diagnostics/docker-compose.yml index 23996cc..2bf4f8a 100644 --- a/demos/sensor_diagnostics/docker-compose.yml +++ b/demos/sensor_diagnostics/docker-compose.yml @@ -44,5 +44,5 @@ services: ros2 launch sensor_diagnostics_demo demo.launch.py & sleep 10 && curl -sf http://localhost:8080/api/v1/health && - curl -sf http://localhost:8080/api/v1/apps | jq '.[] | .name' && + curl -sf http://localhost:8080/api/v1/apps | jq '.items[] | .id' && echo 'CI validation passed!'" diff --git a/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp b/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp index 61a5bcb..ce29c89 100644 --- a/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp +++ b/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp @@ -1,4 +1,4 @@ -// Copyright 2025 selfpatch +// Copyright 2026 selfpatch // SPDX-License-Identifier: Apache-2.0 /// @file simulated_sensor_base.hpp diff --git a/demos/sensor_diagnostics/package.xml b/demos/sensor_diagnostics/package.xml index e4a3b69..e6d8e11 100644 --- a/demos/sensor_diagnostics/package.xml +++ b/demos/sensor_diagnostics/package.xml @@ -19,6 +19,8 @@ ros2launch ros2_medkit_gateway + ros2_medkit_diagnostic_bridge + ros2_medkit_fault_manager ament_lint_auto ament_lint_common diff --git a/demos/sensor_diagnostics/restore-normal.sh b/demos/sensor_diagnostics/restore-normal.sh index 48a03a2..075aacd 100755 --- a/demos/sensor_diagnostics/restore-normal.sh +++ b/demos/sensor_diagnostics/restore-normal.sh @@ -49,14 +49,15 @@ curl -s -X PUT "${API_BASE}/apps/camera_sim/configurations/inject_black_frames" -H "Content-Type: application/json" -d '{"value": false}' # Clear all faults from FaultManager -# Legacy path faults (via diagnostic_bridge) - reported BY diagnostic_bridge -# The fault_code is the name from DiagnosticStatus.name (e.g., LIDAR_SIM, CAMERA_SIM) +# All sensors now publish to /diagnostics, so all faults come through diagnostic_bridge echo "" echo "Clearing all faults from FaultManager..." curl -s -X DELETE "${API_BASE}/apps/diagnostic_bridge/faults/LIDAR_SIM" > /dev/null 2>&1 curl -s -X DELETE "${API_BASE}/apps/diagnostic_bridge/faults/CAMERA_SIM" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/diagnostic_bridge/faults/IMU_SIM" > /dev/null 2>&1 +curl -s -X DELETE "${API_BASE}/apps/diagnostic_bridge/faults/GPS_SIM" > /dev/null 2>&1 -# Modern path faults (via anomaly_detector direct service call) +# Faults from anomaly_detector (modern path for anomaly detection) curl -s -X DELETE "${API_BASE}/apps/anomaly_detector/faults/SENSOR_TIMEOUT" > /dev/null 2>&1 curl -s -X DELETE "${API_BASE}/apps/anomaly_detector/faults/SENSOR_NAN" > /dev/null 2>&1 curl -s -X DELETE "${API_BASE}/apps/anomaly_detector/faults/SENSOR_OUT_OF_RANGE" > /dev/null 2>&1 diff --git a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp index fb562de..361e41c 100644 --- a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp +++ b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 selfpatch +// Copyright 2026 selfpatch // SPDX-License-Identifier: Apache-2.0 /// @file anomaly_detector_node.cpp @@ -43,12 +43,13 @@ class AnomalyDetectorNode : public rclcpp::Node // Create subscribers for MODERN path sensors (IMU, GPS) // Note: LiDAR and Camera use LEGACY path via /diagnostics → diagnostic_bridge + // Topics use absolute paths matching sensor namespace (/sensors/imu_sim/imu, /sensors/gps_sim/fix) imu_sub_ = this->create_subscription( - "/sensors/imu", 10, + "/sensors/imu_sim/imu", 10, std::bind(&AnomalyDetectorNode::imu_callback, this, std::placeholders::_1)); gps_sub_ = this->create_subscription( - "/sensors/fix", 10, + "/sensors/gps_sim/fix", 10, std::bind(&AnomalyDetectorNode::gps_callback, this, std::placeholders::_1)); // Create publisher for detected faults (supplementary diagnostic topic) @@ -173,8 +174,11 @@ class AnomalyDetectorNode : public rclcpp::Node const std::string & source, const std::string & code, const std::string & message, uint8_t severity = 2) { - // Track active faults for later clearing + // Skip if already reported (avoid spamming FaultManager) std::string fault_key = source + ":" + code; + if (active_faults_.count(fault_key)) { + return; + } active_faults_.insert(fault_key); // Publish to diagnostic topic (legacy) diff --git a/demos/sensor_diagnostics/src/camera_sim_node.cpp b/demos/sensor_diagnostics/src/camera_sim_node.cpp index 9031395..b4b580c 100644 --- a/demos/sensor_diagnostics/src/camera_sim_node.cpp +++ b/demos/sensor_diagnostics/src/camera_sim_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 selfpatch +// Copyright 2026 selfpatch // SPDX-License-Identifier: Apache-2.0 /// @file camera_sim_node.cpp @@ -33,7 +33,6 @@ class CameraSimNode : public rclcpp::Node CameraSimNode() : Node("camera_sim"), rng_(std::random_device{}()), - normal_dist_(0.0, 1.0), uniform_dist_(0.0, 1.0) { // Declare parameters @@ -258,7 +257,6 @@ class CameraSimNode : public rclcpp::Node // Random number generation std::mt19937 rng_; - std::normal_distribution normal_dist_; std::uniform_real_distribution uniform_dist_; // Parameters diff --git a/demos/sensor_diagnostics/src/gps_sim_node.cpp b/demos/sensor_diagnostics/src/gps_sim_node.cpp index 3adc2f2..e00d705 100644 --- a/demos/sensor_diagnostics/src/gps_sim_node.cpp +++ b/demos/sensor_diagnostics/src/gps_sim_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 selfpatch +// Copyright 2026 selfpatch // SPDX-License-Identifier: Apache-2.0 /// @file gps_sim_node.cpp @@ -13,6 +13,7 @@ #include #include +#include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/key_value.hpp" #include "rclcpp/rclcpp.hpp" @@ -48,8 +49,9 @@ class GpsSimNode : public rclcpp::Node // Create publishers fix_pub_ = this->create_publisher("fix", 10); - diag_pub_ = this->create_publisher( - "diagnostics", 10); + // Publish to absolute /diagnostics topic for legacy fault reporting via diagnostic_bridge + diag_pub_ = this->create_publisher( + "/diagnostics", 10); // Create timer (with rate validation) double rate = this->get_parameter("rate").as_double(); @@ -190,8 +192,12 @@ class GpsSimNode : public rclcpp::Node void publish_diagnostics(const std::string & status, const std::string & message) { + auto diag_array = diagnostic_msgs::msg::DiagnosticArray(); + diag_array.header.stamp = this->now(); + auto diag = diagnostic_msgs::msg::DiagnosticStatus(); diag.name = "gps_sim"; + diag.hardware_id = "gps_sensor"; if (status == "OK") { diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -216,12 +222,13 @@ class GpsSimNode : public rclcpp::Node kv.value = std::to_string(position_noise_stddev_) + "m"; diag.values.push_back(kv); - diag_pub_->publish(diag); + diag_array.status.push_back(diag); + diag_pub_->publish(diag_array); } // Publishers rclcpp::Publisher::SharedPtr fix_pub_; - rclcpp::Publisher::SharedPtr diag_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/demos/sensor_diagnostics/src/imu_sim_node.cpp b/demos/sensor_diagnostics/src/imu_sim_node.cpp index efe398e..5d174e9 100644 --- a/demos/sensor_diagnostics/src/imu_sim_node.cpp +++ b/demos/sensor_diagnostics/src/imu_sim_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 selfpatch +// Copyright 2026 selfpatch // SPDX-License-Identifier: Apache-2.0 /// @file imu_sim_node.cpp @@ -13,6 +13,7 @@ #include #include +#include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" #include "diagnostic_msgs/msg/key_value.hpp" #include "rclcpp/rclcpp.hpp" @@ -44,8 +45,9 @@ class ImuSimNode : public rclcpp::Node // Create publishers imu_pub_ = this->create_publisher("imu", 10); - diag_pub_ = this->create_publisher( - "diagnostics", 10); + // Publish to absolute /diagnostics topic for legacy fault reporting via diagnostic_bridge + diag_pub_ = this->create_publisher( + "/diagnostics", 10); // Create timer (with rate validation) double rate = this->get_parameter("rate").as_double(); @@ -172,8 +174,12 @@ class ImuSimNode : public rclcpp::Node void publish_diagnostics(const std::string & status, const std::string & message) { + auto diag_array = diagnostic_msgs::msg::DiagnosticArray(); + diag_array.header.stamp = this->now(); + auto diag = diagnostic_msgs::msg::DiagnosticStatus(); diag.name = "imu_sim"; + diag.hardware_id = "imu_sensor"; if (status == "OK") { diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -194,12 +200,13 @@ class ImuSimNode : public rclcpp::Node kv.value = std::to_string(msg_count_); diag.values.push_back(kv); - diag_pub_->publish(diag); + diag_array.status.push_back(diag); + diag_pub_->publish(diag_array); } // Publishers rclcpp::Publisher::SharedPtr imu_pub_; - rclcpp::Publisher::SharedPtr diag_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/demos/sensor_diagnostics/src/lidar_sim_node.cpp b/demos/sensor_diagnostics/src/lidar_sim_node.cpp index 8f15856..a428c66 100644 --- a/demos/sensor_diagnostics/src/lidar_sim_node.cpp +++ b/demos/sensor_diagnostics/src/lidar_sim_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2025 selfpatch +// Copyright 2026 selfpatch // SPDX-License-Identifier: Apache-2.0 /// @file lidar_sim_node.cpp @@ -91,6 +91,14 @@ class LidarSimNode : public rclcpp::Node angle_min_ = this->get_parameter("angle_min").as_double(); angle_max_ = this->get_parameter("angle_max").as_double(); num_readings_ = this->get_parameter("num_readings").as_int(); + if (num_readings_ <= 0) { + RCLCPP_WARN( + this->get_logger(), + "Invalid num_readings parameter (%d). Using default 360.", + num_readings_); + num_readings_ = 360; + this->set_parameters({rclcpp::Parameter("num_readings", num_readings_)}); + } noise_stddev_ = this->get_parameter("noise_stddev").as_double(); failure_probability_ = this->get_parameter("failure_probability").as_double(); inject_nan_ = this->get_parameter("inject_nan").as_bool(); @@ -121,8 +129,17 @@ class LidarSimNode : public rclcpp::Node drift_rate_ = param.as_double(); RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f", drift_rate_); } else if (param.get_name() == "scan_rate") { - // Update timer with new rate + // Update timer with new rate (with validation) double rate = param.as_double(); + if (rate <= 0.0) { + RCLCPP_WARN( + this->get_logger(), + "Invalid scan_rate parameter value (%f Hz). Rejecting change.", + rate); + result.successful = false; + result.reason = "scan_rate must be positive"; + return result; + } auto period = std::chrono::duration(1.0 / rate); timer_ = this->create_wall_timer( std::chrono::duration_cast(period), From c606ccba5b456f11468d6d0f875482ca90ec3573 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Mon, 26 Jan 2026 14:59:24 +0000 Subject: [PATCH 17/19] refactor: remove unused simulated_sensor_base.hpp (YAGNI) Per @mfaferek93's review: removed speculative base class that was never used. Can be re-added when/if sensors actually need shared fault injection logic. --- demos/sensor_diagnostics/Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/demos/sensor_diagnostics/Dockerfile b/demos/sensor_diagnostics/Dockerfile index 0c420ba..4aab92b 100644 --- a/demos/sensor_diagnostics/Dockerfile +++ b/demos/sensor_diagnostics/Dockerfile @@ -40,7 +40,6 @@ RUN git clone --depth 1 --recurse-submodules https://github.com/selfpatch/ros2_m # Copy demo package COPY package.xml CMakeLists.txt ${COLCON_WS}/src/sensor_diagnostics_demo/ -COPY include/ ${COLCON_WS}/src/sensor_diagnostics_demo/include/ COPY src/ ${COLCON_WS}/src/sensor_diagnostics_demo/src/ COPY config/ ${COLCON_WS}/src/sensor_diagnostics_demo/config/ COPY launch/ ${COLCON_WS}/src/sensor_diagnostics_demo/launch/ From c4419412ef7d957121092409887591e2441f230e Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Mon, 26 Jan 2026 16:45:10 +0000 Subject: [PATCH 18/19] fix(sensor-demo): Fix fault injection scripts - Fix anomaly_detector topic subscriptions to match actual sensor topics (/sensors/imu instead of /sensors/imu_sim/imu) - Change NaN injection rate from 5% to 100% for clear fault demonstration - Simplify diagnostic levels: all non-OK statuses now ERROR for reliable fault reporting through diagnostic_bridge - Reset drift start_time when drift_rate parameter changes for accurate drift accumulation - Remove unused simulated_sensor_base.hpp (YAGNI) --- .../simulated_sensor_base.hpp | 79 ------------------- .../src/anomaly_detector_node.cpp | 6 +- .../src/camera_sim_node.cpp | 3 +- demos/sensor_diagnostics/src/gps_sim_node.cpp | 11 +-- demos/sensor_diagnostics/src/imu_sim_node.cpp | 11 +-- .../sensor_diagnostics/src/lidar_sim_node.cpp | 11 +-- 6 files changed, 22 insertions(+), 99 deletions(-) delete mode 100644 demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp diff --git a/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp b/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp deleted file mode 100644 index ce29c89..0000000 --- a/demos/sensor_diagnostics/include/sensor_diagnostics/simulated_sensor_base.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright 2026 selfpatch -// SPDX-License-Identifier: Apache-2.0 - -/// @file simulated_sensor_base.hpp -/// @brief Base class for simulated sensors with fault injection -/// -/// NOTE: This header provides a reusable base class for simulated sensors. -/// Currently, sensor nodes implement fault injection directly for simplicity. -/// This base class is retained for future refactoring when sensors need to -/// share common fault injection logic. - -#pragma once - -#include - -namespace sensor_diagnostics -{ - -/// Configuration for fault injection in simulated sensors -struct FaultConfig -{ - double failure_probability{0.0}; ///< Probability of complete failure (0.0-1.0) - bool inject_nan{false}; ///< Inject NaN values - double noise_multiplier{1.0}; ///< Multiplier for noise (>1 = degraded) - double drift_rate{0.0}; ///< Drift rate per second -}; - -/// Base class for simulated sensors with fault injection -class SimulatedSensorBase -{ -public: - SimulatedSensorBase() - : rng_(std::random_device{}()), uniform_dist_(0.0, 1.0), normal_dist_(0.0, 1.0) - { - } - - virtual ~SimulatedSensorBase() = default; - - /// Check if sensor should fail this cycle - bool should_fail() const - { - return uniform_dist_(rng_) < config_.failure_probability; - } - - /// Check if NaN values should be injected - bool should_inject_nan() const { return config_.inject_nan; } - - /// Get noise value scaled by noise_multiplier - double get_noise(double base_stddev) - { - return normal_dist_(rng_) * base_stddev * config_.noise_multiplier; - } - - /// Get accumulated drift value - double get_drift(double elapsed_seconds) const - { - return config_.drift_rate * elapsed_seconds; - } - - /// Generate uniform random value in range [min, max] - double get_uniform(double min, double max) - { - return min + uniform_dist_(rng_) * (max - min); - } - - /// Update fault configuration - void set_fault_config(const FaultConfig & config) { config_ = config; } - - /// Get current fault configuration - const FaultConfig & get_fault_config() const { return config_; } - -protected: - FaultConfig config_; - mutable std::mt19937 rng_; - mutable std::uniform_real_distribution uniform_dist_; - mutable std::normal_distribution normal_dist_; -}; - -} // namespace sensor_diagnostics diff --git a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp index 361e41c..7907299 100644 --- a/demos/sensor_diagnostics/src/anomaly_detector_node.cpp +++ b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp @@ -43,13 +43,13 @@ class AnomalyDetectorNode : public rclcpp::Node // Create subscribers for MODERN path sensors (IMU, GPS) // Note: LiDAR and Camera use LEGACY path via /diagnostics → diagnostic_bridge - // Topics use absolute paths matching sensor namespace (/sensors/imu_sim/imu, /sensors/gps_sim/fix) + // Topics match sensor namespace: /sensors/imu, /sensors/fix imu_sub_ = this->create_subscription( - "/sensors/imu_sim/imu", 10, + "/sensors/imu", 10, std::bind(&AnomalyDetectorNode::imu_callback, this, std::placeholders::_1)); gps_sub_ = this->create_subscription( - "/sensors/gps_sim/fix", 10, + "/sensors/fix", 10, std::bind(&AnomalyDetectorNode::gps_callback, this, std::placeholders::_1)); // Create publisher for detected faults (supplementary diagnostic topic) diff --git a/demos/sensor_diagnostics/src/camera_sim_node.cpp b/demos/sensor_diagnostics/src/camera_sim_node.cpp index b4b580c..6a5e228 100644 --- a/demos/sensor_diagnostics/src/camera_sim_node.cpp +++ b/demos/sensor_diagnostics/src/camera_sim_node.cpp @@ -215,9 +215,8 @@ class CameraSimNode : public rclcpp::Node if (status == "OK") { diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - } else if (status == "HIGH_NOISE" || status == "LOW_BRIGHTNESS" || status == "OVEREXPOSED") { - diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; } else { + // All non-OK statuses are ERROR level for clear fault reporting diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; } diff --git a/demos/sensor_diagnostics/src/gps_sim_node.cpp b/demos/sensor_diagnostics/src/gps_sim_node.cpp index e00d705..0583e9f 100644 --- a/demos/sensor_diagnostics/src/gps_sim_node.cpp +++ b/demos/sensor_diagnostics/src/gps_sim_node.cpp @@ -116,7 +116,9 @@ class GpsSimNode : public rclcpp::Node inject_nan_ ? "enabled" : "disabled"); } else if (param.get_name() == "drift_rate") { drift_rate_ = param.as_double(); - RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f", drift_rate_); + // Reset start time for demo mode - drift accumulates from parameter change + start_time_ = this->now(); + RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f (timer reset)", drift_rate_); } } @@ -174,8 +176,8 @@ class GpsSimNode : public rclcpp::Node msg.position_covariance[4] = position_noise_stddev_ * position_noise_stddev_; msg.position_covariance[8] = altitude_noise_stddev_ * altitude_noise_stddev_; - // Inject NaN if configured - if (inject_nan_ && uniform_dist_(rng_) < 0.05) { + // Inject NaN if configured (100% rate for clear fault demonstration) + if (inject_nan_) { msg.latitude = std::numeric_limits::quiet_NaN(); publish_diagnostics("NAN_VALUES", "Invalid GPS coordinates"); } else if (drift_offset > 5.0) { @@ -201,9 +203,8 @@ class GpsSimNode : public rclcpp::Node if (status == "OK") { diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - } else if (status == "LOW_ACCURACY" || status == "DRIFTING") { - diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; } else { + // All non-OK statuses are ERROR level for clear fault reporting diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; } diff --git a/demos/sensor_diagnostics/src/imu_sim_node.cpp b/demos/sensor_diagnostics/src/imu_sim_node.cpp index 5d174e9..65f122d 100644 --- a/demos/sensor_diagnostics/src/imu_sim_node.cpp +++ b/demos/sensor_diagnostics/src/imu_sim_node.cpp @@ -105,7 +105,9 @@ class ImuSimNode : public rclcpp::Node inject_nan_ ? "enabled" : "disabled"); } else if (param.get_name() == "drift_rate") { drift_rate_ = param.as_double(); - RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f", drift_rate_); + // Reset start time for demo mode - drift accumulates from parameter change + start_time_ = this->now(); + RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f (timer reset)", drift_rate_); } } @@ -157,8 +159,8 @@ class ImuSimNode : public rclcpp::Node (i % 4 == 0) ? accel_noise_stddev_ * accel_noise_stddev_ : 0.0; } - // Inject NaN if configured - if (inject_nan_ && uniform_dist_(rng_) < 0.05) { + // Inject NaN if configured (100% rate for clear fault demonstration) + if (inject_nan_) { msg.linear_acceleration.x = std::numeric_limits::quiet_NaN(); publish_diagnostics("NAN_VALUES", "NaN values detected in acceleration"); } else if (drift_offset > 0.1) { @@ -183,9 +185,8 @@ class ImuSimNode : public rclcpp::Node if (status == "OK") { diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - } else if (status == "HIGH_NOISE" || status == "DRIFTING") { - diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; } else { + // All non-OK statuses are ERROR level for clear fault reporting diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; } diff --git a/demos/sensor_diagnostics/src/lidar_sim_node.cpp b/demos/sensor_diagnostics/src/lidar_sim_node.cpp index a428c66..068ebaf 100644 --- a/demos/sensor_diagnostics/src/lidar_sim_node.cpp +++ b/demos/sensor_diagnostics/src/lidar_sim_node.cpp @@ -127,7 +127,9 @@ class LidarSimNode : public rclcpp::Node inject_nan_ ? "enabled" : "disabled"); } else if (param.get_name() == "drift_rate") { drift_rate_ = param.as_double(); - RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f", drift_rate_); + // Reset start time for demo mode - drift accumulates from parameter change + start_time_ = this->now(); + RCLCPP_INFO(this->get_logger(), "Drift rate changed to %.4f (timer reset)", drift_rate_); } else if (param.get_name() == "scan_rate") { // Update timer with new rate (with validation) double rate = param.as_double(); @@ -197,8 +199,8 @@ class LidarSimNode : public rclcpp::Node // Clamp to valid range range = std::clamp(range, range_min_, range_max_); - // Inject NaN if configured - if (inject_nan_ && uniform_dist_(rng_) < 0.05) { // 5% NaN rate when enabled + // Inject NaN if configured (100% rate for clear fault demonstration) + if (inject_nan_) { range = std::numeric_limits::quiet_NaN(); nan_count++; } @@ -252,9 +254,8 @@ class LidarSimNode : public rclcpp::Node if (status == "OK") { diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - } else if (status == "HIGH_NOISE" || status == "DRIFTING") { - diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; } else { + // All non-OK statuses are ERROR level for clear fault reporting diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; } From 8a13e055766b26a1a9b3ac105c8aca07e3608c5f Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Mon, 26 Jan 2026 16:57:05 +0000 Subject: [PATCH 19/19] docs: note that sensors use ERROR level for all non-OK states --- demos/sensor_diagnostics/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/demos/sensor_diagnostics/README.md b/demos/sensor_diagnostics/README.md index a679252..2e338eb 100644 --- a/demos/sensor_diagnostics/README.md +++ b/demos/sensor_diagnostics/README.md @@ -83,6 +83,8 @@ Sensor Node → publishes DiagnosticArray → /diagnostics topic - `ERROR (2)` → ERROR severity fault - `STALE (3)` → CRITICAL severity fault +> **Note:** This demo's sensors use only `OK` and `ERROR` levels for clear fault demonstration. All non-OK conditions report as `ERROR` to ensure reliable fault detection through the diagnostic bridge. + ### Modern Path (Direct ReportFault Service) Direct ros2_medkit integration used by **IMU** and **GPS** sensors: