feat(perception): dynamic obstacle velocity estimator (Issue #326) #336

sl-perception · 2026-03-03T06:53:35-05:00

sl-perception commented

2026-03-03 06:53:35 -05:00

Summary

Closes #326

New ObstacleVelocity.msg / ObstacleVelocityArray.msg in saltybot_scene_msgs — carries stable obstacle_id, Kalman-filtered centroid + velocity vector, speed_mps, cluster width_m/depth_m/point_count, track confidence (0–1), and is_static flag
_obstacle_velocity.py — pure-Python tracker (no ROS2 deps):
- KalmanTrack — constant-velocity 2-D Kalman filter: state [x, y, vx, vy], white-noise-acceleration process noise, predict(dt) / update(centroid), coasting → alive lifecycle, confidence = min(1, age/n_init)
- associate() — greedy nearest-centroid matching O(N·M), strict < threshold
- ObstacleTracker — predict-all → associate → update-matched / spawn-new → prune-dead cycle
obstacle_velocity_node.py — subscribes /scan (BEST_EFFORT); reuses _lidar_clustering.scan_to_cartesian + cluster_points; publishes ObstacleVelocityArray on /saltybot/obstacle_velocities each scan

Algorithm

Each scan:
  1. scan → Cartesian points (range filter)
  2. gap-segmentation clustering → centroids + bbox
  3. predict all Kalman tracks forward by dt
  4. greedy nearest-centroid association (max_association_dist_m)
  5. update matched tracks; spawn new tracks for unmatched clusters
  6. prune dead tracks (coasting > max_coasting_frames)
  7. publish ObstacleVelocityArray

Parameters

Param	Default	Description
`distance_threshold_m`	`0.20`	Cluster gap threshold (m)
`min_points`	`3`	Min LIDAR points per cluster
`range_min_m` / `range_max_m`	`0.05` / `12.0`	Range filter (m)
`max_association_dist_m`	`0.50`	Max centroid distance for track match (m)
`max_coasting_frames`	`5`	Frames before track deletion
`n_init_frames`	`3`	Frames to reach confidence=1.0
`q_pos` / `q_vel`	`0.05` / `0.50`	Process noise densities
`r_pos`	`0.10`	Measurement noise std-dev (m)
`static_speed_threshold`	`0.10`	Speed (m/s) below which obstacle is static

Test plan

test/test_obstacle_velocity.py — 48 tests, 0 failures (pure Python, no ROS2)
KalmanTrack init: position, velocity, confidence, alive, coasting, age
KalmanTrack predict: dt=0 no change, dt>0 zero-vel no change, negative clamped, coasting inc, alive boundary
KalmanTrack update: resets coasting, increments age, shifts position, reduces covariance, confidence growth/cap, metadata stored
Velocity convergence: constant-velocity (x and diagonal), stationary target
associate: empty cases, single match, above-threshold reject, 2×2 diagonal, M>N, N>M, nearest-wins, strict-less-than boundary
ObstacleTracker: empty input, single cluster, ID sequence, same-position continuity, confidence growth, coast→die, deletion→new ID, two clusters, monotonic IDs, velocity direction, metadata, far cluster, empty-to-single-and-back, speed estimate

🤖 Generated with Claude Code

## Summary Closes #326 - New `ObstacleVelocity.msg` / `ObstacleVelocityArray.msg` in `saltybot_scene_msgs` — carries stable `obstacle_id`, Kalman-filtered `centroid` + `velocity` vector, `speed_mps`, cluster `width_m`/`depth_m`/`point_count`, track `confidence` (0–1), and `is_static` flag - `_obstacle_velocity.py` — pure-Python tracker (no ROS2 deps): - `KalmanTrack` — constant-velocity 2-D Kalman filter: state `[x, y, vx, vy]`, white-noise-acceleration process noise, `predict(dt)` / `update(centroid)`, coasting → alive lifecycle, `confidence = min(1, age/n_init)` - `associate()` — greedy nearest-centroid matching O(N·M), strict `<` threshold - `ObstacleTracker` — predict-all → associate → update-matched / spawn-new → prune-dead cycle - `obstacle_velocity_node.py` — subscribes `/scan` (BEST_EFFORT); reuses `_lidar_clustering.scan_to_cartesian` + `cluster_points`; publishes `ObstacleVelocityArray` on `/saltybot/obstacle_velocities` each scan ## Algorithm ``` Each scan: 1. scan → Cartesian points (range filter) 2. gap-segmentation clustering → centroids + bbox 3. predict all Kalman tracks forward by dt 4. greedy nearest-centroid association (max_association_dist_m) 5. update matched tracks; spawn new tracks for unmatched clusters 6. prune dead tracks (coasting > max_coasting_frames) 7. publish ObstacleVelocityArray ``` ## Parameters | Param | Default | Description | |---|---|---| | `distance_threshold_m` | `0.20` | Cluster gap threshold (m) | | `min_points` | `3` | Min LIDAR points per cluster | | `range_min_m` / `range_max_m` | `0.05` / `12.0` | Range filter (m) | | `max_association_dist_m` | `0.50` | Max centroid distance for track match (m) | | `max_coasting_frames` | `5` | Frames before track deletion | | `n_init_frames` | `3` | Frames to reach confidence=1.0 | | `q_pos` / `q_vel` | `0.05` / `0.50` | Process noise densities | | `r_pos` | `0.10` | Measurement noise std-dev (m) | | `static_speed_threshold` | `0.10` | Speed (m/s) below which obstacle is static | ## Test plan - [x] `test/test_obstacle_velocity.py` — 48 tests, 0 failures (pure Python, no ROS2) - [x] `KalmanTrack` init: position, velocity, confidence, alive, coasting, age - [x] `KalmanTrack` predict: dt=0 no change, dt>0 zero-vel no change, negative clamped, coasting inc, alive boundary - [x] `KalmanTrack` update: resets coasting, increments age, shifts position, reduces covariance, confidence growth/cap, metadata stored - [x] Velocity convergence: constant-velocity (x and diagonal), stationary target - [x] `associate`: empty cases, single match, above-threshold reject, 2×2 diagonal, M>N, N>M, nearest-wins, strict-less-than boundary - [x] `ObstacleTracker`: empty input, single cluster, ID sequence, same-position continuity, confidence growth, coast→die, deletion→new ID, two clusters, monotonic IDs, velocity direction, metadata, far cluster, empty-to-single-and-back, speed estimate 🤖 Generated with [Claude Code](https://claude.com/claude-code)

sl-perception added 1 commit 2026-03-03 06:53:35 -05:00

feat(perception): dynamic obstacle velocity estimator (Issue #326 ) eb61207532

Adds ObstacleVelocity/ObstacleVelocityArray msgs and an
ObstacleVelocityNode that clusters /scan points, tracks each centroid
with a constant-velocity Kalman filter, and publishes velocity vectors
on /saltybot/obstacle_velocities.

New messages (saltybot_scene_msgs):
  msg/ObstacleVelocity.msg      — obstacle_id, centroid, velocity,
                                  speed_mps, width_m, depth_m,
                                  point_count, confidence, is_static
  msg/ObstacleVelocityArray.msg — array wrapper with header

New files (saltybot_bringup):
  saltybot_bringup/_obstacle_velocity.py   — pure helpers (no ROS2 deps)
    KalmanTrack   constant-velocity 2-D KF: predict(dt) / update(centroid)
                  coasting counter → alive flag; confidence = age/n_init
    associate()   greedy nearest-centroid matching (O(N·M), strict <)
    ObstacleTracker  predict-all → associate → update/spawn → prune cycle
  saltybot_bringup/obstacle_velocity_node.py
    Subscribes /scan (BEST_EFFORT); reuses _lidar_clustering helpers;
    publishes ObstacleVelocityArray on /saltybot/obstacle_velocities
    Parameters: distance_threshold_m=0.20, min_points=3, range 0.05–12m,
                max_association_dist_m=0.50, max_coasting_frames=5,
                n_init_frames=3, q_pos=0.05, q_vel=0.50, r_pos=0.10,
                static_speed_threshold=0.10
  test/test_obstacle_velocity.py — 48 tests, all passing

Modified:
  saltybot_scene_msgs/CMakeLists.txt  — register new msgs
  saltybot_bringup/setup.py           — add obstacle_velocity console_script

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

sl-jetson merged commit 2f76d1d0d5 into main

2026-03-03 11:22:22 -05:00

sl-jetson referenced this issue from a commit

2026-03-03 11:22:22 -05:00

Merge pull request 'feat(perception): dynamic obstacle velocity estimator (Issue #326)' (#336) from sl-perception/issue-326-obstacle-velocity into main

Sign in to join this conversation.

1 Participants

Notifications

Due Date

No due date set.

Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#336