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/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 diff --git a/demos/sensor_diagnostics/CMakeLists.txt b/demos/sensor_diagnostics/CMakeLists.txt new file mode 100644 index 0000000..09144bc --- /dev/null +++ b/demos/sensor_diagnostics/CMakeLists.txt @@ -0,0 +1,86 @@ +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) +find_package(ros2_medkit_msgs 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 + rcl_interfaces +) + +# 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 + rcl_interfaces +) + +# Anomaly detector node +add_executable(anomaly_detector_node src/anomaly_detector_node.cpp) +ament_target_dependencies(anomaly_detector_node + rclcpp + sensor_msgs + diagnostic_msgs + ros2_medkit_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/Dockerfile b/demos/sensor_diagnostics/Dockerfile new file mode 100644 index 0000000..4aab92b --- /dev/null +++ b/demos/sensor_diagnostics/Dockerfile @@ -0,0 +1,63 @@ +# 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 \ + ros-jazzy-yaml-cpp-vendor \ + ros-jazzy-example-interfaces \ + python3-colcon-common-extensions \ + python3-requests \ + nlohmann-json3-dev \ + libcpp-httplib-dev \ + libsqlite3-dev \ + git \ + curl \ + jq \ + && rm -rf /var/lib/apt/lists/* + +# Clone ros2_medkit from GitHub (gateway + dependencies) +WORKDIR ${COLCON_WS}/src +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 . && \ + 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 + +# Copy demo package +COPY package.xml CMakeLists.txt ${COLCON_WS}/src/sensor_diagnostics_demo/ +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 (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 \ + --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 +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/README.md b/demos/sensor_diagnostics/README.md new file mode 100644 index 0000000..2e338eb --- /dev/null +++ b/demos/sensor_diagnostics/README.md @@ -0,0 +1,257 @@ +# 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 +- **Dual fault reporting** - Demonstrates both legacy (diagnostics) and modern (direct) paths + +## 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) - 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 (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 + +> **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: + +``` +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 + +```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` | +| `DRIFTING` | 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/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..a7a2c88 --- /dev/null +++ b/demos/sensor_diagnostics/config/sensor_params.yaml @@ -0,0 +1,55 @@ +# 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 + 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/docker-compose.yml b/demos/sensor_diagnostics/docker-compose.yml new file mode 100644 index 0000000..2bf4f8a --- /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 '.items[] | .id' && + echo 'CI validation passed!'" diff --git a/demos/sensor_diagnostics/inject-drift.sh b/demos/sensor_diagnostics/inject-drift.sh new file mode 100755 index 0000000..4b19189 --- /dev/null +++ b/demos/sensor_diagnostics/inject-drift.sh @@ -0,0 +1,25 @@ +#!/bin/bash +# 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 (Legacy path: LiDAR ā†’ diagnostic_bridge)..." +echo "" + +# 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 "" +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 new file mode 100755 index 0000000..8553aff --- /dev/null +++ b/demos/sensor_diagnostics/inject-failure.sh @@ -0,0 +1,20 @@ +#!/bin/bash +# 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 (Modern path: IMU ā†’ anomaly_detector)..." + +# 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 "āœ“ 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 new file mode 100755 index 0000000..f46ae2e --- /dev/null +++ b/demos/sensor_diagnostics/inject-nan.sh @@ -0,0 +1,39 @@ +#!/bin/bash +# 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 (demonstrates both fault reporting paths)..." +echo "" + +# 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 "" + +# 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 "[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 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 new file mode 100755 index 0000000..52849af --- /dev/null +++ b/demos/sensor_diagnostics/inject-noise.sh @@ -0,0 +1,33 @@ +#!/bin/bash +# 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 (Legacy path: LiDAR/Camera ā†’ diagnostic_bridge)..." +echo "" + +# 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 "" + +# 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 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 new file mode 100644 index 0000000..68b6d29 --- /dev/null +++ b/demos/sensor_diagnostics/launch/demo.launch.py @@ -0,0 +1,137 @@ +"""Launch Sensor Diagnostics Demo with ros2_medkit gateway. + +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 + +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) ===== + # Legacy path sensors: publish DiagnosticArray to /diagnostics + 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="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="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}], + ), + # ===== Processing Nodes (under /processing namespace) ===== + # Modern path: anomaly_detector monitors IMU/GPS and calls ReportFault + 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}], + ), + # ===== 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", + 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}, + ], + ), + # ===== 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", + 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 new file mode 100644 index 0000000..e6d8e11 --- /dev/null +++ b/demos/sensor_diagnostics/package.xml @@ -0,0 +1,32 @@ + + + + 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 + ros2_medkit_msgs + + ros2launch + ros2_medkit_gateway + ros2_medkit_diagnostic_bridge + ros2_medkit_fault_manager + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/demos/sensor_diagnostics/restore-normal.sh b/demos/sensor_diagnostics/restore-normal.sh new file mode 100755 index 0000000..075aacd --- /dev/null +++ b/demos/sensor_diagnostics/restore-normal.sh @@ -0,0 +1,69 @@ +#!/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}' + +# Clear all faults from FaultManager +# 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 + +# 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 +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." +echo " Verify with: curl ${API_BASE}/faults | jq" diff --git a/demos/sensor_diagnostics/run-demo.sh b/demos/sensor_diagnostics/run-demo.sh new file mode 100755 index 0000000..c10db28 --- /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 _ 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 '.items[] | {id: .id, name: .name, area: .area}' + +echo_step "4. Listing All Apps (ROS 2 Nodes)" +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..." +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 '.items[] | {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" 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..7907299 --- /dev/null +++ b/demos/sensor_diagnostics/src/anomaly_detector_node.cpp @@ -0,0 +1,274 @@ +// Copyright 2026 selfpatch +// SPDX-License-Identifier: Apache-2.0 + +/// @file anomaly_detector_node.cpp +/// @brief Monitors IMU and GPS sensors and reports detected anomalies as faults +/// +/// 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 +#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/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("imu_rate_min", 50.0); + this->declare_parameter("gps_rate_min", 0.5); + + rate_timeout_sec_ = this->get_parameter("rate_timeout_sec").as_double(); + + // Create subscribers for MODERN path sensors (IMU, GPS) + // Note: LiDAR and Camera use LEGACY path via /diagnostics ā†’ diagnostic_bridge + // Topics match sensor namespace: /sensors/imu, /sensors/fix + imu_sub_ = this->create_subscription( + "/sensors/imu", 10, + std::bind(&AnomalyDetectorNode::imu_callback, this, std::placeholders::_1)); + + gps_sub_ = this->create_subscription( + "/sensors/fix", 10, + std::bind(&AnomalyDetectorNode::gps_callback, this, std::placeholders::_1)); + + // Create publisher for detected faults (supplementary diagnostic topic) + fault_pub_ = this->create_publisher( + "detected_faults", 10); + + // Create service client for FaultManager (MODERN path) + 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)); + + RCLCPP_INFO(this->get_logger(), "Anomaly detector started (modern path: IMU, GPS)"); + } + +private: + 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 (modern path sensors only) + check_timeout("imu", now); + check_timeout("gps", now); + + // Calculate and log rates + double elapsed = 1.0; // 1 second timer + double imu_rate = (imu_msg_count_ - last_imu_count_) / elapsed; + double gps_rate = (gps_msg_count_ - last_gps_count_) / elapsed; + + last_imu_count_ = imu_msg_count_; + last_gps_count_ = gps_msg_count_; + + // Check for degraded rates + 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 (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, uint8_t severity = 2) + { + // 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) + 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); + + // 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 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 { + RCLCPP_WARN(this->get_logger(), "[%s] %s: %s (FaultManager unavailable)", + source.c_str(), code.c_str(), message.c_str()); + } + } + + // 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_; + rclcpp::Subscription::SharedPtr gps_sub_; + + // Publisher + rclcpp::Publisher::SharedPtr fault_pub_; + + // Service client for FaultManager + rclcpp::Client::SharedPtr report_fault_client_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Parameters + double rate_timeout_sec_; + + // State tracking + std::map sensor_timestamps_; + std::set active_faults_; + uint64_t imu_msg_count_{0}; + uint64_t gps_msg_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; +} 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..6a5e228 --- /dev/null +++ b/demos/sensor_diagnostics/src/camera_sim_node.cpp @@ -0,0 +1,281 @@ +// Copyright 2026 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. +/// Diagnostics are published to /diagnostics for the legacy fault reporting path +/// via ros2_medkit_diagnostic_bridge. + +#include +#include +#include +#include +#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" +#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{}()), + 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); + // 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(); + 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), + 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_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; + } else { + // All non-OK statuses are ERROR level for clear fault reporting + 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_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_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Parameter callback + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + // Random number generation + std::mt19937 rng_; + 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..0583e9f --- /dev/null +++ b/demos/sensor_diagnostics/src/gps_sim_node.cpp @@ -0,0 +1,268 @@ +// Copyright 2026 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_array.hpp" +#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); + // 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(); + 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), + 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(); + // 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_); + } + } + + 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 (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) { + 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_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; + } else { + // All non-OK statuses are ERROR level for clear fault reporting + 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_array.status.push_back(diag); + diag_pub_->publish(diag_array); + } + + // 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..65f122d --- /dev/null +++ b/demos/sensor_diagnostics/src/imu_sim_node.cpp @@ -0,0 +1,243 @@ +// Copyright 2026 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_array.hpp" +#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); + // 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(); + 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), + 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(); + // 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_); + } + } + + 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 (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) { + 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_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; + } else { + // All non-OK statuses are ERROR level for clear fault reporting + 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_array.status.push_back(diag); + diag_pub_->publish(diag_array); + } + + // 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; +} 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..068ebaf --- /dev/null +++ b/demos/sensor_diagnostics/src/lidar_sim_node.cpp @@ -0,0 +1,325 @@ +// Copyright 2026 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. +/// Diagnostics are published to /diagnostics for the legacy fault reporting path +/// via ros2_medkit_diagnostic_bridge. + +#include +#include +#include +#include +#include +#include +#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" +#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); + // Publish to absolute /diagnostics topic for legacy fault reporting via diagnostic_bridge + diag_pub_ = this->create_publisher( + "/diagnostics", 10); + + // 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 + 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(); + 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(); + 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(); + // 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(); + 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), + 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 (100% rate for clear fault demonstration) + if (inject_nan_) { + 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_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; + } else { + // All non-OK statuses are ERROR level for clear fault reporting + 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_array.status.push_back(diag); + diag_pub_->publish(diag_array); + } + + // 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; +}