seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

feat(#169): emergency behavior system — obstacle stop, fall prevention, stuck detection, recovery FSM #179

Merged
sl-jetson merged 1 commits from sl-controls/issue-169-emergency into main 2026-03-02 10:44:50 -05:00
Conversation 0 Commits 1 Files Changed 17 +2095
sl-webui commented 2026-03-02 10:40:16 -05:00
Collaborator
Copy Link

Summary

  • saltybot_emergency_msgs: EmergencyEvent.msg (state, threat_type, severity, cmd_override) + RecoveryAction.msg
  • saltybot_emergency: threat detectors (obstacle, fall, stuck, bump), alert manager with escalation, recovery sequencer (reverse+turn+retry), master emergency FSM, 20 Hz ROS2 node

Behaviour

Threat Threshold Level Response
Obstacle <30 cm at speed MAJOR stop + recover
Obstacle <10 cm CRITICAL stop → ESCALATED
Tilt 0.20 rad MINOR beep
Tilt 0.35 rad / floor drop 15 cm MAJOR stop + recover
Tilt 0.52 rad (~30°) CRITICAL ESCALATED
Stuck 3 s stall MAJOR reverse + 90° turn
Jerk >8 m/s³ MAJOR stop + recover
Jerk >25 m/s³ CRITICAL ESCALATED
MAJOR × 3 in 10 s — escalated to CRITICAL —

Topics

  • Publishes: /saltybot/emergency (EmergencyEvent), /saltybot/recovery_action (RecoveryAction), /saltybot/e_stop (Bool), /saltybot/alert_beep, /saltybot/alert_flash (Empty), /saltybot/critical_alert (String JSON)
  • Subscribes: /scan (LaserScan), /saltybot/imu (Imu), odom, /cmd_vel, /saltybot/emergency_ack (Bool)

Test plan

  • 59/59 unit tests passing (pure Python, no ROS2 dependency)
  • ObstacleDetector: proximity thresholds, speed gate
  • FallDetector: tilt levels + floor drop
  • StuckDetector: timeout accumulation, motion-resume reset
  • BumpDetector: jerk thresholds, first-call safety, gravity removal
  • AlertManager: severity mapping, suppression, escalation
  • RecoverySequencer: full sequence, retry count, GAVE_UP
  • EmergencyFSM: all state transitions, cmd_override immediacy on entry

Closes #169

🤖 Generated with Claude Code

## Summary - `saltybot_emergency_msgs`: `EmergencyEvent.msg` (state, threat_type, severity, cmd_override) + `RecoveryAction.msg` - `saltybot_emergency`: threat detectors (obstacle, fall, stuck, bump), alert manager with escalation, recovery sequencer (reverse+turn+retry), master emergency FSM, 20 Hz ROS2 node ## Behaviour | Threat | Threshold | Level | Response | |--------|-----------|-------|-----------| | Obstacle | <30 cm at speed | MAJOR | stop + recover | | Obstacle | <10 cm | CRITICAL | stop → ESCALATED | | Tilt | 0.20 rad | MINOR | beep | | Tilt | 0.35 rad / floor drop 15 cm | MAJOR | stop + recover | | Tilt | 0.52 rad (~30°) | CRITICAL | ESCALATED | | Stuck | 3 s stall | MAJOR | reverse + 90° turn | | Jerk | >8 m/s³ | MAJOR | stop + recover | | Jerk | >25 m/s³ | CRITICAL | ESCALATED | | MAJOR × 3 in 10 s | — | escalated to CRITICAL | — | ## Topics - Publishes: `/saltybot/emergency` (EmergencyEvent), `/saltybot/recovery_action` (RecoveryAction), `/saltybot/e_stop` (Bool), `/saltybot/alert_beep`, `/saltybot/alert_flash` (Empty), `/saltybot/critical_alert` (String JSON) - Subscribes: `/scan` (LaserScan), `/saltybot/imu` (Imu), odom, `/cmd_vel`, `/saltybot/emergency_ack` (Bool) ## Test plan - [x] 59/59 unit tests passing (pure Python, no ROS2 dependency) - [x] ObstacleDetector: proximity thresholds, speed gate - [x] FallDetector: tilt levels + floor drop - [x] StuckDetector: timeout accumulation, motion-resume reset - [x] BumpDetector: jerk thresholds, first-call safety, gravity removal - [x] AlertManager: severity mapping, suppression, escalation - [x] RecoverySequencer: full sequence, retry count, GAVE_UP - [x] EmergencyFSM: all state transitions, cmd_override immediacy on entry Closes #169 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-webui added 1 commit 2026-03-02 10:40:17 -05:00 
Two new packages:
- saltybot_emergency_msgs: EmergencyEvent.msg, RecoveryAction.msg
- saltybot_emergency: threat_detector, alert_manager, recovery_sequencer, emergency_fsm, emergency_node

Implements:
- ObstacleDetector: MAJOR <30cm, CRITICAL <10cm; suppressed when not moving
- FallDetector: MINOR/MAJOR/CRITICAL tilt thresholds; floor-drop edge detection
- StuckDetector: MAJOR after 3s wheel stall (cmd>threshold, actual~0)
- BumpDetector: jerk = |Δ|a||/dt with gravity removal; MAJOR/CRITICAL thresholds
- AlertManager: per-(type,level) suppression; MAJOR×N within window → CRITICAL escalation
- RecoverySequencer: REVERSING→TURNING→RETRYING FSM; max_retries before GAVE_UP
- EmergencyFSM: NOMINAL→STOPPING→RECOVERING→ESCALATED; acknowledge to clear
- EmergencyNode: 20Hz ROS2 node; /saltybot/emergency, /saltybot/e_stop, cmd_vel mux

59/59 tests passing.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit c4bf8c371f into main 2026-03-02 10:44:50 -05:00
Sign in to join this conversation.
Reviewers
No Reviewers
Labels
No items
No Label
Milestone
No items
No Milestone
Projects
Clear projects
No project
Assignees
Clear assignees
shaytac sl-controls sl-firmware sl-jetson sl-mechanical sl-perception sl-uwb sl-webui seb
No Assignees
1 Participants
Notifications
Due Date
No due date set.
Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#179
No description provided.
Delete Branch "sl-controls/issue-169-emergency"

Deleting a branch is permanent. Although the deleted branch may continue to exist for a short time before it actually gets removed, it CANNOT be undone in most cases. Continue?