|
|
eb61207532
|
feat(perception): dynamic obstacle velocity estimator (Issue #326)
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>
|
2026-03-03 06:53:04 -05:00 |
|
