#[20] fix(turtlebot3-demo): add anomaly_detector for direct fault reporting

README.md

@@ -1,6 +1,9 @@
 # selfpatch_demos
 
 [![CI](https://github.com/selfpatch/selfpatch_demos/actions/workflows/ci.yml/badge.svg)](https://github.com/selfpatch/selfpatch_demos/actions/workflows/ci.yml)
+[![Docs](https://img.shields.io/badge/docs-GitHub%20Pages-blue)](https://selfpatch.github.io/ros2_medkit/)
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](LICENSE)
+[![Discord](https://img.shields.io/badge/Discord-Join%20Us-7289DA?logo=discord&logoColor=white)](https://discord.gg/fEbWKTah)
 
 Demonstration projects showcasing [ros2_medkit](https://github.com/selfpatch/ros2_medkit) integration
 with real ROS 2 systems.

demos/turtlebot3_integration/CMakeLists.txt

@@ -13,4 +13,10 @@ install(DIRECTORY config/
   DESTINATION share/${PROJECT_NAME}/config
 )
 
+# Install scripts
+install(PROGRAMS
+  scripts/anomaly_detector.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_package()

demos/turtlebot3_integration/Dockerfile

@@ -59,6 +59,7 @@ RUN git clone --depth 1 --recurse-submodules https://github.com/selfpatch/ros2_m
 COPY package.xml CMakeLists.txt ${COLCON_WS}/src/turtlebot3_medkit_demo/
 COPY config/ ${COLCON_WS}/src/turtlebot3_medkit_demo/config/
 COPY launch/ ${COLCON_WS}/src/turtlebot3_medkit_demo/launch/
+COPY scripts/ ${COLCON_WS}/src/turtlebot3_medkit_demo/scripts/
 
 # Build ros2_medkit and demo package
 WORKDIR ${COLCON_WS}

demos/turtlebot3_integration/README.md

@@ -271,45 +271,67 @@ TurtleBot3 Demo (manifest-based discovery)
 │   ├── planner-server        → component: nav2-stack
 │   ├── medkit-gateway        → component: gateway
 │   ├── medkit-fault-manager  → component: fault-manager
-│   └── diagnostic-bridge     → component: diagnostic-bridge-unit
+│   ├── diagnostic-bridge     → component: diagnostic-bridge-unit
+│   └── anomaly-detector      → component: diagnostic-bridge-unit
 └── Functions (high-level capabilities)
     ├── autonomous-navigation → hosted by: amcl, bt-navigator, ...
     ├── robot-control         → hosted by: turtlebot3-node, velocity-smoother
-    └── fault-management      → hosted by: gateway, fault-manager, bridge
+    └── fault-management      → hosted by: gateway, fault-manager, bridge, anomaly-detector
 ```
 
 ### Fault Reporting
 
-This demo uses the **legacy fault reporting path** via diagnostic_bridge:
+This demo uses two fault reporting paths:
+
+1. **Direct Fault Reporting** via `anomaly_detector`:
+   - Monitors navigation goal status, AMCL covariance, and robot progress
+   - Reports faults directly to FaultManager via `/fault_manager/report_fault` service
+
+2. **Legacy Path** via `diagnostic_bridge`:
+   - Subscribes to `/diagnostics` topic (DiagnosticArray)
+   - Converts diagnostics to fault reports
 
 ```
-Nav2/TurtleBot3 nodes → /diagnostics topic (DiagnosticArray)
-                              ↓
-        diagnostic_bridge subscribes and converts
-                              ↓
-        FaultManager receives via ReportFault service
-                              ↓
-        Gateway exposes via GET /api/v1/faults
+┌─────────────────────────────────────────────────────────────────────┐
+│                     Fault Reporting Paths                           │
+├─────────────────────────────────────────────────────────────────────┤
+│                                                                     │
+│  anomaly_detector ─────┬──── /fault_manager/report_fault ──────┐   │
+│    (monitors:          │            (direct service call)       │   │
+│     - nav goal status) │                                        │   │
+│     - AMCL covariance) │                                        ▼   │
+│     - robot progress)  │                              ┌──────────────┐
+│                        │                              │ FaultManager │
+│                        │                              └──────┬───────┘
+│  Nav2/TurtleBot3 ──────┼──── /diagnostics topic ──────┐     │        │
+│    (DiagnosticArray)   │                              ▼     │        │
+│                        │                   diagnostic_bridge │        │
+│                        └───────────────────────────────┘     │        │
+│                                                              ▼        │
+│                                           GET /api/v1/faults         │
+└─────────────────────────────────────────────────────────────────────┘
 ```
 
 | Source | Fault Reporter | Example Faults |
 |--------|----------------|----------------|
+| Navigation Goals | anomaly_detector | `NAVIGATION_GOAL_ABORTED`, `NAVIGATION_GOAL_CANCELED` |
+| Localization | anomaly_detector | `LOCALIZATION_UNCERTAINTY` |
+| Robot Progress | anomaly_detector | `NAVIGATION_NO_PROGRESS` |
 | AMCL | diagnostic_bridge | Localization degraded |
 | Nav2 Controller | diagnostic_bridge | Path following errors |
 | TurtleBot3 | diagnostic_bridge | Motor/sensor issues |
 
 ## Fault Injection Scenarios
 
-This demo includes scripts to inject various fault conditions for testing fault management:
+This demo includes scripts to inject various fault conditions for testing fault management.
+Faults are detected by `anomaly_detector` and reported directly to FaultManager.
 
 ### Available Fault Scenarios
 
 | Script | Fault Type | Description | Expected Faults |
 |--------|-----------|-------------|-----------------|
-| `inject-nav-failure.sh` | Navigation | Send goal to unreachable location | BT_NAVIGATOR, PLANNER_SERVER |
-| `inject-localization-failure.sh` | Localization | Reset AMCL with high uncertainty | AMCL |
-| `inject-controller-failure.sh` | Controller | Set very restrictive velocity limits | VELOCITY_SMOOTHER, CONTROLLER_SERVER |
-| `inject-collision.sh` | Collision | Navigate toward obstacles | COLLISION_MONITOR |
+| `inject-nav-failure.sh` | Navigation | Send goal to unreachable location | `NAVIGATION_GOAL_ABORTED` |
+| `inject-localization-failure.sh` | Localization | Reset AMCL with high uncertainty | `LOCALIZATION_UNCERTAINTY` |
 | `restore-normal.sh` | Recovery | Restore defaults and clear faults | - |
 
 ### Fault Injection Examples
@@ -321,7 +343,7 @@ This demo includes scripts to inject various fault conditions for testing fault
 ./inject-nav-failure.sh
 
 # Check resulting faults
-curl http://localhost:8080/api/v1/faults | jq '.items[] | {code, severity, message}'
+curl http://localhost:8080/api/v1/faults | jq '.items[] | {fault_code, severity_label, description}'
 ```
 
 #### 2. Localization Failure
@@ -330,27 +352,7 @@ curl http://localhost:8080/api/v1/faults | jq '.items[] | {code, severity, messa
 # Reinitialize AMCL global localization (causes high uncertainty)
 ./inject-localization-failure.sh
 
-# Watch localization recover
-curl http://localhost:8080/api/v1/apps/amcl/data/particlecloud | jq '.poses | length'
-```
-
-#### 3. Controller Restriction
-
-```bash
-# Set very low velocity limits (robot moves extremely slowly)
-./inject-controller-failure.sh
-
-# Try sending a navigation goal - robot will struggle to follow path
-./send-nav-goal.sh 2.0 0.5
-```
-
-#### 4. Collision Scenario
-
-```bash
-# Trigger collision avoidance
-./inject-collision.sh
-
-# Monitor collision warnings
+# Watch for LOCALIZATION_UNCERTAINTY fault
 curl http://localhost:8080/api/v1/faults | jq
 ```
 
@@ -429,16 +431,16 @@ demos/turtlebot3_integration/
 ├── check-faults.sh             # View active faults
 ├── inject-nav-failure.sh       # Inject navigation failure scenario
 ├── inject-localization-failure.sh  # Inject AMCL localization issues
-├── inject-controller-failure.sh    # Inject controller velocity limits
-├── inject-collision.sh         # Inject collision warning scenario
 ├── restore-normal.sh           # Restore normal operation
 ├── config/
 │   ├── medkit_params.yaml           # ros2_medkit gateway config
 │   ├── turtlebot3_manifest.yaml     # SOVD manifest (entity hierarchy)
 │   ├── nav2_params.yaml             # Nav2 navigation parameters
 │   └── turtlebot3_world.yaml        # Map configuration
-└── launch/
-    └── demo.launch.py          # ROS 2 launch file
+├── launch/
+│   └── demo.launch.py          # ROS 2 launch file
+└── scripts/
+    └── anomaly_detector.py     # Navigation anomaly detector node
 ```
 
 ## Scripts
@@ -452,8 +454,6 @@ demos/turtlebot3_integration/
 | `check-faults.sh` | View active faults from gateway |
 | `inject-nav-failure.sh` | Inject navigation failure (unreachable goal) |
 | `inject-localization-failure.sh` | Inject localization failure (AMCL reset) |
-| `inject-controller-failure.sh` | Inject controller failure (velocity limits) |
-| `inject-collision.sh` | Inject collision warning scenario |
 | `restore-normal.sh` | Restore normal operation and clear faults |
 
 ## Manual Setup (Alternative)

demos/turtlebot3_integration/config/turtlebot3_manifest.yaml

@@ -183,6 +183,18 @@ apps:
       node_name: diagnostic_bridge
       namespace: /bridge
 
+  - id: anomaly-detector
+    name: "Anomaly Detector"
+    category: "diagnostics"
+    is_located_on: diagnostic-bridge-unit
+    description: "Monitors navigation metrics and reports faults directly to FaultManager"
+    depends_on:
+      - amcl
+      - bt-navigator
+    ros_binding:
+      node_name: anomaly_detector
+      namespace: /bridge
+
 # =============================================================================
 # FUNCTIONS - High-level capabilities
 # =============================================================================
@@ -213,3 +225,4 @@ functions:
       - medkit-gateway
       - medkit-fault-manager
       - diagnostic-bridge
+      - anomaly-detector

demos/turtlebot3_integration/inject-localization-failure.sh

@@ -22,14 +22,30 @@ curl -s -X POST "${API_BASE}/apps/amcl/operations/reinitialize_global_localizati
   -d '{}'
 
 echo ""
-echo "✓ Localization failure injected!"
+echo "Waiting for particles to scatter..."
+sleep 2
+
+# Now try to navigate - with scattered particles, localization uncertainty is high
+echo "Sending navigation goal (with high localization uncertainty)..."
+curl -s -X POST "${API_BASE}/apps/bt-navigator/operations/navigate_to_pose/executions" \
+  -H "Content-Type: application/json" \
+  -d '{
+    "goal": {
+      "pose": {
+        "header": {"frame_id": "map"},
+        "pose": {
+          "position": {"x": 2.0, "y": 0.0, "z": 0.0},
+          "orientation": {"w": 1.0}
+        }
+      }
+    }
+  }' | jq '.' 2>/dev/null || true
+
 echo ""
-echo "AMCL will now have high uncertainty until it re-localizes."
-echo "Expected faults (via diagnostic_bridge):"
-echo "  - AMCL: Localization confidence low"
-echo "  - BT_NAVIGATOR: Goal may fail due to uncertain pose"
+echo "✓ Localization failure injected!"
 echo ""
-echo "Watch localization recover with:"
-echo "  curl ${API_BASE}/apps/amcl/data/particlecloud | jq '.poses | length'"
+echo "AMCL has been reinitialized - localization uncertainty is high."
+echo "The anomaly_detector monitors AMCL covariance and will report:"
+echo "  - LOCALIZATION_UNCERTAINTY: High AMCL covariance (uncertainty)"
 echo ""
 echo "Check faults with: curl ${API_BASE}/faults | jq"

demos/turtlebot3_integration/inject-nav-failure.sh

@@ -27,7 +27,7 @@ echo "Sending navigation goal to (100.0, 100.0) - far outside map..."
 RESPONSE=$(curl -s -X POST "${API_BASE}/apps/bt-navigator/operations/navigate_to_pose/executions" \
   -H "Content-Type: application/json" \
   -d '{
-    "request": {
+    "goal": {
       "pose": {
         "header": {"frame_id": "map"},
         "pose": {
@@ -40,11 +40,26 @@ RESPONSE=$(curl -s -X POST "${API_BASE}/apps/bt-navigator/operations/navigate_to
 
 echo "$RESPONSE" | jq '.' 2>/dev/null || echo "$RESPONSE"
 
+# Extract execution ID
+EXEC_ID=$(echo "$RESPONSE" | jq -r '.id' 2>/dev/null)
+
+if [ -n "$EXEC_ID" ] && [ "$EXEC_ID" != "null" ]; then
+    echo ""
+    echo "Waiting for navigation to fail (checking status)..."
+    for _ in {1..10}; do
+        sleep 2
+        STATUS=$(curl -s "${API_BASE}/apps/bt-navigator/operations/navigate_to_pose/executions/${EXEC_ID}" | jq -r '.status' 2>/dev/null)
+        echo "  Status: $STATUS"
+        if [ "$STATUS" = "failed" ] || [ "$STATUS" = "completed" ]; then
+            break
+        fi
+    done
+fi
+
 echo ""
 echo "✓ Navigation failure injected!"
 echo ""
-echo "Expected faults (via diagnostic_bridge):"
-echo "  - BT_NAVIGATOR: Goal rejected or path planning failed"
-echo "  - PLANNER_SERVER: No valid path to goal"
+echo "Expected faults (via anomaly_detector → FaultManager):"
+echo "  - NAVIGATION_GOAL_ABORTED: Navigation goal ABORTED"
 echo ""
 echo "Check faults with: curl ${API_BASE}/faults | jq"

demos/turtlebot3_integration/launch/demo.launch.py

@@ -165,5 +165,19 @@ def generate_launch_description():
                     },
                 ],
             ),
+            # Launch anomaly detector to monitor navigation and publish diagnostics
+            Node(
+                package="turtlebot3_medkit_demo",
+                executable="anomaly_detector.py",
+                name="anomaly_detector",
+                namespace="bridge",
+                output="screen",
+                parameters=[
+                    {"use_sim_time": use_sim_time},
+                    {"covariance_warn_threshold": 0.3},
+                    {"covariance_error_threshold": 1.0},
+                    {"no_progress_timeout_sec": 20.0},
+                ],
+            ),
         ]
     )