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

feat(jetson): dynamic obstacle tracking — LIDAR motion detection, Kalman tracking, trajectory prediction, Nav2 costmap (#176) #181

Merged
sl-jetson merged 1 commits from sl-perception/issue-176-dynamic-obstacles into main 2026-03-02 10:45:23 -05:00
Conversation 0 Commits 1 Files Changed 17 +1529
sl-perception commented 2026-03-02 10:44:51 -05:00
Collaborator
Copy Link

Summary

  • saltybot_dynamic_obs_msgs: TrackedObject.msg (id, pose, velocity, predicted_path, confidence) + MovingObjectArray.msg
  • object_detector: EMA occupancy grid background subtraction on LIDAR scans → moving blob detection via connected-components
  • kalman_tracker: CV Kalman filter [px,py,vx,vy], Joseph-form update, non-mutating predict_horizon() (2.5 s @ 0.5 s steps)
  • tracker_manager: up to 20 tracks, Hungarian assignment (scipy.optimize.linear_sum_assignment), TENTATIVE→CONFIRMED lifecycle, miss-prune
  • dynamic_obs_node: 10 Hz timer → detect + track + publish /saltybot/moving_objects + RViz MarkerArray
  • costmap_layer_node: predicted trajectory smear → PointCloud2 (/saltybot/dynamic_obs_cloud) for Nav2 ObstacleLayer

Performance

  • Tracker tick: ~10 Hz, <50 ms on Jetson Orin Nano (pure NumPy + SciPy, no GPU needed)
  • Prediction horizon: 2.5 s at 0.5 s steps (5 waypoints per track)
  • Up to 20 simultaneous confirmed tracks

Test plan

  • 27 unit tests pass (pytest test/test_dynamic_obstacles.py) — no ROS2 or hardware required
  • Deploy on Jetson; verify ros2 topic hz /saltybot/moving_objects ≥ 9 Hz
  • Walk past RPLIDAR; confirm track appears within 3 frames and disappears within 6 after leaving
  • Check MarkerArray in RViz: cylinder + velocity arrow + predicted path line strip
  • Verify Nav2 routes around predicted obstacle path via /saltybot/dynamic_obs_cloud
  • Latency: ros2 topic delay /saltybot/moving_objects < 50 ms

🤖 Generated with Claude Code

## Summary - **saltybot_dynamic_obs_msgs**: `TrackedObject.msg` (id, pose, velocity, predicted_path, confidence) + `MovingObjectArray.msg` - **object_detector**: EMA occupancy grid background subtraction on LIDAR scans → moving blob detection via connected-components - **kalman_tracker**: CV Kalman filter `[px,py,vx,vy]`, Joseph-form update, non-mutating `predict_horizon()` (2.5 s @ 0.5 s steps) - **tracker_manager**: up to 20 tracks, Hungarian assignment (`scipy.optimize.linear_sum_assignment`), TENTATIVE→CONFIRMED lifecycle, miss-prune - **dynamic_obs_node**: 10 Hz timer → detect + track + publish `/saltybot/moving_objects` + RViz MarkerArray - **costmap_layer_node**: predicted trajectory smear → PointCloud2 (`/saltybot/dynamic_obs_cloud`) for Nav2 ObstacleLayer ## Performance - Tracker tick: ~10 Hz, <50 ms on Jetson Orin Nano (pure NumPy + SciPy, no GPU needed) - Prediction horizon: 2.5 s at 0.5 s steps (5 waypoints per track) - Up to 20 simultaneous confirmed tracks ## Test plan - [x] 27 unit tests pass (`pytest test/test_dynamic_obstacles.py`) — no ROS2 or hardware required - [ ] Deploy on Jetson; verify `ros2 topic hz /saltybot/moving_objects` ≥ 9 Hz - [ ] Walk past RPLIDAR; confirm track appears within 3 frames and disappears within 6 after leaving - [ ] Check MarkerArray in RViz: cylinder + velocity arrow + predicted path line strip - [ ] Verify Nav2 routes around predicted obstacle path via `/saltybot/dynamic_obs_cloud` - [ ] Latency: `ros2 topic delay /saltybot/moving_objects` < 50 ms 🤖 Generated with [Claude Code](https://claude.com/claude-code)
sl-perception added 1 commit 2026-03-02 10:44:52 -05:00 
Implements real-time moving obstacle detection, Kalman tracking, trajectory
prediction, and Nav2 costmap integration at 10 Hz / <50ms latency:

saltybot_dynamic_obs_msgs (ament_cmake):
• TrackedObject.msg      — id, PoseWithCovariance, velocity, predicted_path,
                           predicted_times, speed, confidence, age, hits
• MovingObjectArray.msg  — TrackedObject[], active_count, tentative_count,
                           detector_latency_ms

saltybot_dynamic_obstacles (ament_python):
• object_detector.py    — LIDAR background subtraction (EMA occupancy grid),
                           foreground dilation + scipy connected-component
                           clustering → Detection list
• kalman_tracker.py     — CV Kalman filter, state [px,py,vx,vy], Joseph-form
                           covariance update, predict_horizon() (non-mutating)
• tracker_manager.py    — up to 20 tracks, Hungarian assignment
                           (scipy.optimize.linear_sum_assignment), TENTATIVE→
                           CONFIRMED lifecycle, miss-prune
• dynamic_obs_node.py   — 10 Hz timer: detect→track→publish
                           /saltybot/moving_objects + MarkerArray viz
• costmap_layer_node.py — predicted paths → PointCloud2 inflation smear
                           → /saltybot/dynamic_obs_cloud for Nav2 ObstacleLayer
• launch/dynamic_obstacles.launch.py + config/dynamic_obstacles_params.yaml
• test/test_dynamic_obstacles.py — 27 unit tests (27/27 pass, no ROS2 needed)

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
sl-jetson merged commit 54668536c1 into main 2026-03-02 10:45:23 -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#181
No description provided.
Delete Branch "sl-perception/issue-176-dynamic-obstacles"

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?