feat(control): RGB-only collision guardrail module at ~10 Hz

[pmg5408]

Problem

Direct cmd_vel producers do not currently have a reusable RGB-only guardrail on the motion path. This makes behaviors like person-following vulnerable to blindly forwarding commanded motion without any conservative frontal collision check from the onboard camera stream.

Solution

Adds a reusable RGBCollisionGuardrail module under dimos/control/safety that consumes color_image and incoming_cmd_vel and publishes safe_cmd_vel.

The implementation is split into two parts:

• a module shell that owns stream I/O, freshness checks, worker-thread scheduling, and guarded command publishing
• a policy layer that evaluates optical-flow magnitude in a forward ROI and returns PASS, CLAMP, STOP_LATCHED, or SENSOR_DEGRADED

Key design decisions:

• use a single decision thread with lightweight image/cmd callbacks
• keep latest-value semantics instead of queueing stale commands
• run fail-closed on startup, stale inputs, stale frame pairs, low-texture ROIs, and occluded ROIs
• treat stop-threshold detections conservatively by clamping immediately and requiring repeated evidence before entering STOP_LATCHED
• preserve the module as a generic control-layer guardrail rather than coupling it to a specific upstream behavior

This PR also adds focused unit and integration tests for policy transitions, module fail-closed behavior, and threaded guarded-output behavior.

Breaking Changes

None.

How to Test

uv run pytest dimos/control/safety/test_guardrail_policy.py dimos/control/safety/test_rgb_collision_guardrail.py dimos/control/safety/test_rgb_collision_guardrail_integration.py -v

Contributor License Agreement

• I have read and approved the CLA.

[greptile-apps]

Greptile Summary

This PR adds a new RGBCollisionGuardrail module under dimos/control/safety that subscribes to a camera stream and incoming cmd_vel, evaluates forward-collision risk using Farneback optical flow in a central lower ROI, and publishes a safety-gated safe_cmd_vel. The implementation is cleanly split into a policy layer (OpticalFlowMagnitudeGuardrailPolicy with PASS/CLAMP/STOP_LATCHED/SENSOR_DEGRADED hysteresis) and a threaded module shell that owns stream I/O, freshness checks, and fail-closed publishing.

Confidence Score: 5/5

Safe to merge; all findings are minor style and maintenance issues with no operational impact

The hysteresis state machine is correct and well-covered by tests. The fail-closed design (SENSOR_DEGRADED/INIT on missing/stale inputs) is exercised end-to-end. The only code-level finding is a logging-path data race that is non-crashing in CPython and carries no operational consequence. All remaining findings are P2.

No files require special attention before merging

Important Files Changed

Filename	Overview
dimos/control/safety/guardrail_policy.py	New optical-flow guardrail policy: grayscale+resize → forward ROI crop → quality checks → Farneback flow → PASS/CLAMP/STOP_LATCHED/SENSOR_DEGRADED hysteresis; logic is correct and conservative
dimos/control/safety/rgb_collision_guardrail.py	Threaded guardrail module with condition-variable worker loop; one unlocked read of runtime state in the exception logging path is a minor CPython-safe race
dimos/control/safety/test_guardrail_policy.py	Thorough unit tests covering all state transitions, hysteresis paths, deadband passthrough, and fail-closed quality cases
dimos/control/safety/test_rgb_collision_guardrail.py	Unit and threaded behavioral tests for the guardrail module; FakeTransport and several helpers are duplicated from the integration test file
dimos/control/safety/test_rgb_collision_guardrail_integration.py	Integration tests with real threading, timing assertions, and concurrent-publish stress test; helpers are duplicated from the unit test file

Sequence Diagram

sequenceDiagram
    participant Camera
    participant Guard as RGBCollisionGuardrail
    participant RS as _GuardrailRuntimeState
    participant DL as decision_loop (worker thread)
    participant Policy as OpticalFlowMagnitudePolicy
    participant Out as safe_cmd_vel

    Camera->>Guard: color_image (Image)
    Guard->>RS: shift latest→previous, store image, bump image_generation
    Guard->>DL: condition.notify()

    Note over DL: wakes from condition.wait()
    DL->>RS: _should_recompute_risk_locked(now)?<br/>[new generation AND next_risk_time elapsed]
    RS-->>DL: yes → snapshot _RiskEvaluationInput<br/>(prev_image, cur_image, cmd_vel, health)

    DL->>Policy: evaluate(prev, cur, cmd_vel, health)
    Note over Policy: 1. Health gates (has_previous_frame, image_fresh, frame_pair_fresh)<br/>2. Forward-motion deadband check<br/>3. Grayscale + resize to flow_downsample_width_px<br/>4. Crop central lower forward ROI<br/>5. Quality checks: occlusion + low-texture<br/>6. Farneback optical flow → mean magnitude<br/>7. Hysteresis counter update → next state
    Policy-->>DL: GuardrailDecision(state, cmd_vel, reason, risk_score)

    DL->>RS: _store_decision_locked()<br/>persist decision, set pending_decision_publish on state change
    DL->>RS: _consume_publish_cmd_locked()
    RS-->>DL: resolved Twist to publish
    DL->>Out: publish(cmd_vel)
    Note over Out: PASS → upstream cmd<br/>CLAMP → forward speed capped<br/>STOP_LATCHED → linear.x zeroed<br/>SENSOR_DEGRADED/INIT → Twist.zero()

    Note over Guard,DL: Parallel path — incoming_cmd_vel arrives
    Guard->>RS: store cmd_vel, latest_cmd_time<br/>set pending_cmd_update=True
    Guard->>DL: condition.notify()
    DL->>RS: _consume_publish_cmd_locked()<br/>pending_cmd_update path → _resolved_cmd_for_latest_locked()
    DL->>Out: publish(resolved cmd using last stored decision)

Loading

_{Reviews (1): Last reviewed commit: "test(control): add RGB guardrail policy ..." | Re-trigger Greptile}