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

feat: RPLIDAR safety zone detector (Issue #575) #580

Merged
sl-jetson merged 1 commits from sl-perception/issue-575-safety-zone into main 2026-03-14 12:14:01 -04:00
Conversation 0 Commits 1 Files Changed 8 +490
sl-perception commented 2026-03-14 11:55:23 -04:00
Collaborator
Copy Link

Summary

Implements saltybot_safety_zone — a ROS2 Python node that processes the RPLIDAR A1M8 /scan into three concentric 360° safety zones, latches an e-stop when DANGER is detected in the forward arc, and overrides /cmd_vel to zero while latched.

Zone thresholds (configurable)

Zone Condition Default
DANGER min range in sector < threshold 0.30 m
WARN min range in sector < threshold 1.00 m
CLEAR otherwise —

Sector grid

  • 36 sectors × 10° = 360° coverage
  • Sector 0 = robot forward (0°), increasing counter-clockwise (ROS convention)
  • Per-sector data: {sector, angle_deg, zone, min_range_m, forward}

E-stop behaviour

  1. Trigger: DANGER in forward arc (±30° default) for ≥ 2 consecutive scans
  2. While latched: zero Twist published to /cmd_vel on every scan + every cmd_vel_input message
  3. Clear: via service /saltybot/safety_zone/clear_estop (std_srvs/Trigger) — only succeeds when no DANGER sectors remain
  4. All-arcs mode: estop_all_arcs: true triggers on DANGER anywhere in 360°

Topics

Topic Direction Type Rate
/scan sub LaserScan RPLIDAR ~5.5 Hz
/cmd_vel_input sub Twist upstream velocity
/saltybot/safety_zone pub String (JSON) every scan
/saltybot/safety_zone/status pub String (JSON) 10 Hz
/cmd_vel pub Twist zero when e-stopped

/saltybot/safety_zone JSON (every scan)

{
  "sectors": [{"sector": 0, "angle_deg": 0.0, "zone": "WARN", "min_range_m": 0.82, "forward": true}, ...],
  "estop_active": false,
  "estop_reason": "",
  "danger_sectors": [],
  "warn_sectors": [0, 1, 35]
}

/saltybot/safety_zone/status JSON (10 Hz)

{
  "estop_active": false,
  "forward_zone": "WARN",
  "danger_sector_count": 0,
  "warn_sector_count": 3,
  "closest_obstacle_m": 0.82,
  "scan_count": 142,
  "forward_sector_ids": [0, 1, 2, 33, 34, 35]
}

cmd_vel intercept chain

cmd_vel_mux → /cmd_vel_input → [safety_zone node] → /cmd_vel → STM32 bridge

When e-stopped, the safety zone node blocks /cmd_vel_input and publishes zero to /cmd_vel.

Test plan

  • colcon build --packages-select saltybot_safety_zone succeeds
  • Node starts: ros2 run saltybot_safety_zone safety_zone
  • /saltybot/safety_zone publishes 36 sectors on every /scan message
  • Sector angle mapping: sector 0 = 0°, sector 18 = 180°
  • WARN detected when obstacle at 0.5 m (< 1.0 m, ≥ 0.3 m)
  • DANGER detected when obstacle at 0.2 m (< 0.3 m)
  • E-stop latches after 2 consecutive DANGER scans in forward ±30° arc
  • /cmd_vel zeroed immediately on latch
  • Upstream /cmd_vel_input blocked while latched
  • Clear service fails with obstacle present; succeeds when DANGER gone
  • estop_all_arcs: true triggers on side/rear DANGER too
  • Obstacle behind robot (180°) does NOT trigger e-stop in default mode

Closes #575

## Summary Implements `saltybot_safety_zone` — a ROS2 Python node that processes the RPLIDAR A1M8 `/scan` into three concentric 360° safety zones, latches an e-stop when DANGER is detected in the forward arc, and overrides `/cmd_vel` to zero while latched. ### Zone thresholds (configurable) | Zone | Condition | Default | |------|-----------|---------| | DANGER | min range in sector < threshold | 0.30 m | | WARN | min range in sector < threshold | 1.00 m | | CLEAR | otherwise | — | ### Sector grid - 36 sectors × 10° = 360° coverage - Sector 0 = robot forward (0°), increasing counter-clockwise (ROS convention) - Per-sector data: `{sector, angle_deg, zone, min_range_m, forward}` ### E-stop behaviour 1. **Trigger**: DANGER in forward arc (±30° default) for ≥ 2 consecutive scans 2. **While latched**: zero `Twist` published to `/cmd_vel` on every scan + every `cmd_vel_input` message 3. **Clear**: via service `/saltybot/safety_zone/clear_estop` (`std_srvs/Trigger`) — only succeeds when no DANGER sectors remain 4. **All-arcs mode**: `estop_all_arcs: true` triggers on DANGER anywhere in 360° ### Topics | Topic | Direction | Type | Rate | |-------|-----------|------|------| | `/scan` | sub | LaserScan | RPLIDAR ~5.5 Hz | | `/cmd_vel_input` | sub | Twist | upstream velocity | | `/saltybot/safety_zone` | pub | String (JSON) | every scan | | `/saltybot/safety_zone/status` | pub | String (JSON) | 10 Hz | | `/cmd_vel` | pub | Twist | zero when e-stopped | ### `/saltybot/safety_zone` JSON (every scan) ```json { "sectors": [{"sector": 0, "angle_deg": 0.0, "zone": "WARN", "min_range_m": 0.82, "forward": true}, ...], "estop_active": false, "estop_reason": "", "danger_sectors": [], "warn_sectors": [0, 1, 35] } ``` ### `/saltybot/safety_zone/status` JSON (10 Hz) ```json { "estop_active": false, "forward_zone": "WARN", "danger_sector_count": 0, "warn_sector_count": 3, "closest_obstacle_m": 0.82, "scan_count": 142, "forward_sector_ids": [0, 1, 2, 33, 34, 35] } ``` ### cmd_vel intercept chain ``` cmd_vel_mux → /cmd_vel_input → [safety_zone node] → /cmd_vel → STM32 bridge ``` When e-stopped, the safety zone node blocks `/cmd_vel_input` and publishes zero to `/cmd_vel`. ## Test plan - [ ] `colcon build --packages-select saltybot_safety_zone` succeeds - [ ] Node starts: `ros2 run saltybot_safety_zone safety_zone` - [ ] `/saltybot/safety_zone` publishes 36 sectors on every `/scan` message - [ ] Sector angle mapping: sector 0 = 0°, sector 18 = 180° - [ ] WARN detected when obstacle at 0.5 m (< 1.0 m, ≥ 0.3 m) - [ ] DANGER detected when obstacle at 0.2 m (< 0.3 m) - [ ] E-stop latches after 2 consecutive DANGER scans in forward ±30° arc - [ ] `/cmd_vel` zeroed immediately on latch - [ ] Upstream `/cmd_vel_input` blocked while latched - [ ] Clear service fails with obstacle present; succeeds when DANGER gone - [ ] `estop_all_arcs: true` triggers on side/rear DANGER too - [ ] Obstacle behind robot (180°) does NOT trigger e-stop in default mode Closes #575
sl-perception added 1 commit 2026-03-14 11:55:24 -04:00 
Add saltybot_safety_zone — ROS2 Python node that processes the RPLIDAR
A1M8 /scan into three concentric 360° safety zones, latches an e-stop
when DANGER is detected in the forward arc, and overrides /cmd_vel to
zero while the latch is active.

Zone thresholds (default):
  DANGER  < 0.30 m — latching e-stop in forward arc
  WARN    < 1.00 m — advisory (published in sector data)
  CLEAR   otherwise

Sector grid:
  36 sectors of 10° each (sector 0 = robot forward, CCW positive).
  Per-sector: angle_deg, zone, min_range_m, in_forward_arc flag.

E-stop behaviour:
  - Latches after estop_debounce_frames (2) consecutive DANGER scans
    in the forward arc (configurable ±30°, or all-arcs mode).
  - While latched: zero Twist published to /cmd_vel every scan + every
    incoming /cmd_vel_input message is blocked.
  - Clear only via service (obstacle must be gone):
    /saltybot/safety_zone/clear_estop  (std_srvs/Trigger)

Published topics:
  /saltybot/safety_zone          String/JSON  every scan
    — per-sector {sector, angle_deg, zone, min_range_m, forward}
    — estop_active, estop_reason, danger_sectors[], warn_sectors[]
  /saltybot/safety_zone/status   String/JSON  10 Hz
    — forward_zone, closest_obstacle_m, danger/warn counts
  /cmd_vel                       Twist        zero when e-stopped

Subscribed topics:
  /scan           LaserScan  — RPLIDAR A1M8
  /cmd_vel_input  Twist      — upstream velocity (pass-through / block)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 1726558a7a into main 2026-03-14 12:14:01 -04: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#580
No description provided.
Delete Branch "sl-perception/issue-575-safety-zone"

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?