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

feat(bringup): obstacle height filter with IMU pitch compensation (Issue #211) #219

Merged
sl-jetson merged 1 commits from sl-perception/issue-211-height-filter into main 2026-03-02 11:51:38 -05:00
Conversation 0 Commits 1 Files Changed 4 +499
sl-perception commented 2026-03-02 11:51:15 -05:00
Collaborator
Copy Link

Summary

  • Add _scan_height_filter.py (pure Python, no rclpy): filter_scan_by_height() projects each LIDAR ray to world-frame height using z = lidar_h + r·cos(θ)·sin(pitch) − r·sin(θ)·sin(roll); pitch_roll_from_accel() uses convention-agnostic atan2(ay, sqrt(ax²+az²)) (works for both z-up and z-down IMU frames); AttitudeEstimator low-pass filters accelerometer attitude
  • Add scan_height_filter_node.py: subscribes /scan + /camera/imu, publishes /scan_filtered (LaserScan) for Nav2 at source scan rate (designed for up to 20 Hz); BEST_EFFORT QoS
  • Add scan_height_filter entry point to setup.py
  • 18/18 unit tests pass

Closes #211

Test plan

  • python3 -m pytest test/test_scan_height_filter.py -v — 18 passed
  • Launch with sensors + verify /scan_filtered topic appears
  • Tilt robot on slope, verify ground returns removed from /scan_filtered
  • Configure Nav2 scan_topic: /scan_filtered

🤖 Generated with Claude Code

## Summary - Add `_scan_height_filter.py` (pure Python, no rclpy): `filter_scan_by_height()` projects each LIDAR ray to world-frame height using `z = lidar_h + r·cos(θ)·sin(pitch) − r·sin(θ)·sin(roll)`; `pitch_roll_from_accel()` uses convention-agnostic `atan2(ay, sqrt(ax²+az²))` (works for both z-up and z-down IMU frames); `AttitudeEstimator` low-pass filters accelerometer attitude - Add `scan_height_filter_node.py`: subscribes `/scan` + `/camera/imu`, publishes `/scan_filtered` (LaserScan) for Nav2 at source scan rate (designed for up to 20 Hz); BEST_EFFORT QoS - Add `scan_height_filter` entry point to `setup.py` - 18/18 unit tests pass Closes #211 ## Test plan - [ ] `python3 -m pytest test/test_scan_height_filter.py -v` — 18 passed - [ ] Launch with sensors + verify `/scan_filtered` topic appears - [ ] Tilt robot on slope, verify ground returns removed from `/scan_filtered` - [ ] Configure Nav2 `scan_topic: /scan_filtered` 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-perception added 1 commit 2026-03-02 11:51:16 -05:00 
Two files added to saltybot_bringup:
- _scan_height_filter.py: pure-Python helpers (no rclpy) —
  filter_scan_by_height() projects each LIDAR ray to world-frame height
  using pitch/roll from the IMU and filters ground/ceiling returns;
  pitch_roll_from_accel() uses convention-agnostic atan2 formula;
  AttitudeEstimator low-pass filters the accelerometer attitude.
- scan_height_filter_node.py: subscribes /scan + /camera/imu, publishes
  /scan_filtered (LaserScan) for Nav2 at source rate (up to 20 Hz).

setup.py: adds scan_height_filter entry point.
18/18 unit tests pass.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 3a34ec84e0 into main 2026-03-02 11:51:38 -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#219
No description provided.
Delete Branch "sl-perception/issue-211-height-filter"

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?