saltylab-firmware/.claude/plans/floofy-forging-hellman.md

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:

{
  "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

# 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