# Issue #194 — Speed Limiter Node Implementation Plan

## Context

The SaltyBot control stack currently lacks a dedicated speed limiting aggregator. Individual subsystems publish speed scale factors:
- **TerrainAdaptationNode** → `/saltybot/terrain_speed_scale` (Float32, 0.15–1.0)
- **EmergencyNode** → `/saltybot/emergency` (EmergencyEvent with severity level)
- Hypothetical external source → `/saltybot/speed_limit` (to be determined)

These must be unified into a single authoritative speed scale and applied to `/cmd_vel` before forwarding to the drive system. Currently, each subsystem is independent, creating potential conflicts and inconsistent behavior.

## Goal

Create `saltybot_speed_limiter` ROS2 package that:
1. Subscribes to terrain_speed_scale, speed_limit, and emergency signals
2. Computes effective minimum speed scale from all three inputs
3. Applies scale to /cmd_vel messages → /saltybot/cmd_vel_limited
4. Publishes diagnostic JSON on /saltybot/speed_limit_info
5. Operates at 50 Hz with proper message synchronization

## Architecture

### Subscriptions

| Topic | Type | Source | Frequency | Use |
|-------|------|--------|-----------|-----|
| `/saltybot/terrain_speed_scale` | Float32 | TerrainAdaptationNode | 50Hz | Terrain-based scaling (0.15–1.0) |
| `/saltybot/speed_limit` | Float32 | External (TBD) | ≥10Hz | Global speed limit (0.0–1.0) |
| `/saltybot/emergency` | EmergencyEvent | EmergencyNode | 20Hz | Emergency severity level |
| `/cmd_vel` | Twist | SpeedControllerNode | ≥20Hz | Raw commanded velocity |

### Outputs

| Topic | Type | Frequency | Content |
|-------|------|-----------|---------|
| `/saltybot/cmd_vel_limited` | Twist | 50Hz | Scaled cmd_vel |
| `/saltybot/speed_limit_info` | String (JSON) | 50Hz | Diagnostic state |

### Message Types

**EmergencyEvent** (from saltybot_emergency_msgs):
```
string severity  # "CLEAR"|"MINOR"|"MAJOR"|"CRITICAL"
```

**speed_limit_info JSON**:
```json
{
  "terrain_scale": 0.638,
  "speed_limit": 1.0,
  "emergency_scale": 1.0,
  "effective_scale": 0.638,
  "cmd_vel_input": {"linear": 1.0, "angular": 0.5},
  "cmd_vel_limited": {"linear": 0.638, "angular": 0.319},
  "timestamp": 1740000000.123456
}
```

### Control Logic

```python
# Emergency severity → speed scale mapping
emergency_scale = {
    "CLEAR": 1.0,
    "MINOR": 0.95,      # Yellow alert: slight caution
    "MAJOR": 0.7,       # Orange alert: significant slowdown
    "CRITICAL": 0.3,    # Red alert: severe limitation
}

# Effective scale = minimum of all sources
effective_scale = min(
    terrain_scale,           # From /saltybot/terrain_speed_scale
    speed_limit,             # From /saltybot/speed_limit
    emergency_scale,         # From /saltybot/emergency severity
)

# Apply to all cmd_vel components
cmd_vel_limited.linear.x *= effective_scale
cmd_vel_limited.linear.y *= effective_scale
cmd_vel_limited.linear.z *= effective_scale
cmd_vel_limited.angular.x *= effective_scale
cmd_vel_limited.angular.y *= effective_scale
cmd_vel_limited.angular.z *= effective_scale
```

## Implementation Details

### Package Structure

```
saltybot_speed_limiter/
├── CMakeLists.txt
├── package.xml
├── setup.py
├── README.md
├── launch/
│   └── speed_limiter.launch.py
├── resource/
│   └── saltybot_speed_limiter
├── saltybot_speed_limiter/
│   ├── __init__.py
│   └── speed_limiter_node.py
├── config/
│   └── speed_limiter_params.yaml
└── test/
    ├── __init__.py
    └── test_speed_limiter.py
```

### Key Implementation Points

1. **Message Synchronization**: Use `message_filters.ApproximateTimeSynchronizer` to align terrain_scale, speed_limit, emergency, and cmd_vel with small slop tolerance (50–100ms @ 50Hz)

2. **State Management**:
   - Store latest values for each input (timeout handling if stale)
   - Fallback to safe defaults if messages stop arriving:
     - terrain_scale → 1.0 (no scaling)
     - speed_limit → 1.0 (no limit)
     - emergency → "CLEAR" (no threat)

3. **Parameters**:
   - `publish_hz`: 50.0 (fixed, not tunable)
   - `sync_slop_ms`: 100 (ApproximateTimeSynchronizer slop)
   - `timeout_s`: 2.0 (signal timeout)
   - `frame_id`: 'base_link'
   - Emergency scale map (tunable per severity)

4. **Performance**:
   - Small package, minimal computation (3 float comparisons + geometry scaling)
   - No heavy libraries (just numpy for Twist scaling)

### Unit Tests

Test file: `test/test_speed_limiter.py`

Coverage:
- Min-of-three logic (all combinations)
- Twist scaling on all 6 components
- Emergency severity mapping (CLEAR, MINOR, MAJOR, CRITICAL)
- Timeout/stale handling (graceful fallback)
- JSON output structure and values
- Message synchronization edge cases

### Integration Points

**Upstream** (inputs):
- SpeedControllerNode → `/cmd_vel`
- TerrainAdaptationNode → `/saltybot/terrain_speed_scale`
- EmergencyNode → `/saltybot/emergency`

**Downstream** (outputs):
- CmdVelBridgeNode ← `/saltybot/cmd_vel_limited` (replaces `/cmd_vel`)

**Migration Path**:
1. CmdVelBridgeNode currently reads `/cmd_vel`
2. After speed_limiter is ready: modify CmdVelBridgeNode to read `/saltybot/cmd_vel_limited` instead
3. Or: use ROS2 topic remapping in launch files during transition

## Files to Create

1. **saltybot_speed_limiter/speed_limiter_node.py** (~200 lines)
   - SpeedLimiterNode class
   - Message synchronization with fallback logic
   - Scale computation and Twist application
   - JSON diagnostic publishing

2. **test/test_speed_limiter.py** (~150 lines)
   - Min-of-three logic tests
   - Twist scaling verification
   - Emergency map tests
   - Timeout/stale handling tests

3. **launch/speed_limiter.launch.py** (~40 lines)
   - Configurable parameters
   - QoS setup

4. **config/speed_limiter_params.yaml** (~20 lines)
   - Emergency severity scales
   - Timeout and sync parameters

5. **package.xml, setup.py, CMakeLists.txt, README.md, resource/** (~150 lines total)
   - Standard ROS2 package boilerplate

## Verification Plan

1. **Unit Tests**: Run pytest on test_speed_limiter.py (expect 15+ tests, all pass)
2. **Package Build**: `colcon build --packages-select saltybot_speed_limiter`
3. **Node Launch**: `ros2 launch saltybot_speed_limiter speed_limiter.launch.py`
4. **Manual Testing** (hardware):
   - Subscribe to /saltybot/cmd_vel_limited and verify scaling
   - Inject different terrain_scale, speed_limit, emergency values
   - Confirm effective_scale = min(all three)
   - Verify JSON diagnostic content

## Branch & Commit Strategy

1. Branch: `sl-controls/issue-194-speed-limiter` from main
2. Commit: Single commit with all 10 files
3. PR: To main branch with detailed description
4. MQTT Report: Send status to max after merge

## Key Decisions

- **Min-of-three logic** (not weighted average): Ensures safety — most conservative limit wins
- **Separate topic output** (`/saltybot/cmd_vel_limited`): Preserves original /cmd_vel for debugging
- **Emergency severity levels** (CLEAR/MINOR/MAJOR/CRITICAL): Map to scales 1.0/0.95/0.7/0.3 (can tune)
- **ApproximateTimeSynchronizer**: Handles async inputs at different rates
- **JSON string output** (not typed message): Easy to parse in logging/MQTT system