diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_obstacle_velocity.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_obstacle_velocity.py new file mode 100644 index 0000000..fc43ceb --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_obstacle_velocity.py @@ -0,0 +1,375 @@ +""" +_obstacle_velocity.py — Dynamic obstacle velocity estimation via Kalman +filtering of LIDAR cluster centroids (no ROS2 deps). + +Algorithm +--------- +Each detected cluster centroid is tracked by a constant-velocity 2-D Kalman +filter: + + State : [x, y, vx, vy]^T + Obs : [x, y]^T (centroid position) + + Predict : x_pred = F(dt) @ x; P_pred = F @ P @ F^T + Q(dt) + Update : S = H @ P_pred @ H^T + R + K = P_pred @ H^T @ inv(S) + x = x_pred + K @ (z - H @ x_pred) + P = (I - K @ H) @ P_pred + +Process noise Q uses the white-noise-acceleration approximation: + Q = diag([q_pos·dt², q_pos·dt², q_vel·dt, q_vel·dt]) + +Data association uses a greedy nearest-centroid approach: scan all +(track, cluster) pairs in order of Euclidean distance and accept matches +below `max_association_dist_m`, one-to-one. + +Track lifecycle +--------------- + created : first appearance of a cluster with no nearby track + confident : after `n_init_frames` consecutive updates (confidence=1.0) + coasting : cluster not matched for up to `max_coasting_frames` calls + deleted : coasting count exceeds `max_coasting_frames` + +Public API +---------- + KalmanTrack one tracked obstacle + .predict(dt) advance state by dt seconds + .update(centroid, width, depth, n) correct with new measurement + .position → np.ndarray (2,) + .velocity → np.ndarray (2,) + .speed → float (m/s) + .confidence → float 0–1 + .alive → bool + + associate(positions, centroids, max_dist) + → (matches, unmatched_t, unmatched_c) + + ObstacleTracker + .update(centroids, timestamp, widths, depths, point_counts) + → List[KalmanTrack] + .tracks → Dict[int, KalmanTrack] +""" + +from __future__ import annotations + +from typing import Dict, List, Optional, Tuple + +import numpy as np + + +# ── Kalman filter constants ─────────────────────────────────────────────────── + +_H = np.array([[1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0]], dtype=np.float64) + +_I4 = np.eye(4, dtype=np.float64) + + +# ── KalmanTrack ─────────────────────────────────────────────────────────────── + +class KalmanTrack: + """ + Constant-velocity 2-D Kalman filter for one LIDAR cluster centroid. + + Parameters + ---------- + track_id : unique integer ID + centroid : initial (x, y) position in metres + q_pos : position process noise density (m²/s²) + q_vel : velocity process noise density (m²/s³) + r_pos : measurement noise std-dev (metres) + n_init_frames : consecutive updates needed for confidence=1 + max_coasting : missed updates before track is deleted + """ + + def __init__( + self, + track_id: int, + centroid: np.ndarray, + q_pos: float = 0.05, + q_vel: float = 0.50, + r_pos: float = 0.10, + n_init_frames: int = 3, + max_coasting: int = 5, + ) -> None: + self._id = track_id + self._n_init = max(n_init_frames, 1) + self._max_coast = max_coasting + self._q_pos = float(q_pos) + self._q_vel = float(q_vel) + self._R = np.diag([r_pos ** 2, r_pos ** 2]) + self._age = 0 # successful updates + self._coasting = 0 # consecutive missed updates + self._alive = True + + # State: [x, y, vx, vy]^T — initial velocity is zero + self._x = np.array( + [float(centroid[0]), float(centroid[1]), 0.0, 0.0], + dtype=np.float64, + ) + # High initial velocity uncertainty, low position uncertainty + self._P = np.diag([r_pos ** 2, r_pos ** 2, 10.0, 10.0]) + + # Last-known cluster metadata (stored, not filtered) + self.last_width: float = 0.0 + self.last_depth: float = 0.0 + self.last_point_count: int = 0 + + # ── Properties ──────────────────────────────────────────────────────────── + + @property + def track_id(self) -> int: + return self._id + + @property + def position(self) -> np.ndarray: + return self._x[:2].copy() + + @property + def velocity(self) -> np.ndarray: + return self._x[2:].copy() + + @property + def speed(self) -> float: + return float(np.linalg.norm(self._x[2:])) + + @property + def confidence(self) -> float: + return min(1.0, self._age / self._n_init) + + @property + def alive(self) -> bool: + return self._alive + + @property + def age(self) -> int: + return self._age + + @property + def coasting(self) -> int: + return self._coasting + + # ── Kalman steps ────────────────────────────────────────────────────────── + + def predict(self, dt: float) -> None: + """ + Advance the filter state by `dt` seconds (constant-velocity model). + + dt must be ≥ 0; negative values are clamped to 0. + Increments the coasting counter; marks the track dead when the + counter exceeds max_coasting_frames. + """ + dt = max(0.0, float(dt)) + + F = np.array([ + [1.0, 0.0, dt, 0.0], + [0.0, 1.0, 0.0, dt], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ], dtype=np.float64) + + Q = np.diag([ + self._q_pos * dt * dt, + self._q_pos * dt * dt, + self._q_vel * dt, + self._q_vel * dt, + ]) + + self._x = F @ self._x + self._P = F @ self._P @ F.T + Q + + self._coasting += 1 + if self._coasting > self._max_coast: + self._alive = False + + def update( + self, + centroid: np.ndarray, + width: float = 0.0, + depth: float = 0.0, + point_count: int = 0, + ) -> None: + """ + Correct state with a new centroid measurement. + + Resets the coasting counter and increments the age counter. + """ + z = np.asarray(centroid, dtype=np.float64)[:2] + + S = _H @ self._P @ _H.T + self._R # innovation covariance + K = self._P @ _H.T @ np.linalg.inv(S) # Kalman gain + + self._x = self._x + K @ (z - _H @ self._x) + self._P = (_I4 - K @ _H) @ self._P + + self._age += 1 + self._coasting = 0 + + self.last_width = float(width) + self.last_depth = float(depth) + self.last_point_count = int(point_count) + + +# ── Data association ────────────────────────────────────────────────────────── + +def associate( + positions: np.ndarray, # (N, 2) predicted track positions + centroids: np.ndarray, # (M, 2) new cluster centroids + max_dist: float, +) -> Tuple[List[Tuple[int, int]], List[int], List[int]]: + """ + Greedy nearest-centroid data association. + + Iteratively matches the closest (track, cluster) pair, one-to-one, as + long as the distance is strictly less than `max_dist`. + + Returns + ------- + matches : list of (track_idx, cluster_idx) pairs + unmatched_tracks : track indices with no assigned cluster + unmatched_clusters: cluster indices with no assigned track + """ + N = len(positions) + M = len(centroids) + + if N == 0 or M == 0: + return [], list(range(N)), list(range(M)) + + # (N, M) pairwise Euclidean distance matrix + cost = np.linalg.norm( + positions[:, None, :] - centroids[None, :, :], axis=2 + ).astype(np.float64) # (N, M) + + matched_t: set = set() + matched_c: set = set() + matches: List[Tuple[int, int]] = [] + + for _ in range(min(N, M)): + if cost.min() >= max_dist: + break + t_idx, c_idx = np.unravel_index(int(cost.argmin()), cost.shape) + matches.append((int(t_idx), int(c_idx))) + matched_t.add(t_idx) + matched_c.add(c_idx) + cost[t_idx, :] = np.inf + cost[:, c_idx] = np.inf + + unmatched_t = [i for i in range(N) if i not in matched_t] + unmatched_c = [i for i in range(M) if i not in matched_c] + return matches, unmatched_t, unmatched_c + + +# ── ObstacleTracker ─────────────────────────────────────────────────────────── + +class ObstacleTracker: + """ + Multi-obstacle Kalman tracker operating on LIDAR cluster centroids. + + Call `update()` once per LIDAR scan with the list of cluster centroids + (and optional bbox / point_count metadata). Returns the current list + of alive KalmanTrack objects. + """ + + def __init__( + self, + max_association_dist_m: float = 0.50, + max_coasting_frames: int = 5, + n_init_frames: int = 3, + q_pos: float = 0.05, + q_vel: float = 0.50, + r_pos: float = 0.10, + ) -> None: + self._max_dist = float(max_association_dist_m) + self._max_coast = int(max_coasting_frames) + self._n_init = int(n_init_frames) + self._q_pos = float(q_pos) + self._q_vel = float(q_vel) + self._r_pos = float(r_pos) + + self._tracks: Dict[int, KalmanTrack] = {} + self._next_id: int = 1 + self._last_t: Optional[float] = None + + @property + def tracks(self) -> Dict[int, KalmanTrack]: + return self._tracks + + def update( + self, + centroids: List[np.ndarray], + timestamp: float, + widths: Optional[List[float]] = None, + depths: Optional[List[float]] = None, + point_counts: Optional[List[int]] = None, + ) -> List[KalmanTrack]: + """ + Process one frame of cluster centroids. + + Parameters + ---------- + centroids : list of (2,) arrays — cluster centroid positions + timestamp : wall-clock time in seconds (time.time()) + widths : optional per-cluster bbox widths (metres) + depths : optional per-cluster bbox depths (metres) + point_counts : optional per-cluster LIDAR point counts + + Returns + ------- + List of all alive KalmanTrack objects after this update. + """ + dt = max(0.0, timestamp - self._last_t) if self._last_t is not None else 0.0 + self._last_t = timestamp + + # 1. Predict all existing tracks forward by dt + for track in self._tracks.values(): + track.predict(dt) + + # 2. Build arrays for association (alive tracks only) + alive = [t for t in self._tracks.values() if t.alive] + if alive: + pred_pos = np.array([t.position for t in alive]) # (N, 2) + else: + pred_pos = np.empty((0, 2), dtype=np.float64) + + if centroids: + cent_arr = np.array([c[:2] for c in centroids], dtype=np.float64) # (M, 2) + else: + cent_arr = np.empty((0, 2), dtype=np.float64) + + # 3. Associate + matches, _, unmatched_c = associate(pred_pos, cent_arr, self._max_dist) + + # 4. Update matched tracks + for ti, ci in matches: + track = alive[ti] + track.update( + cent_arr[ci], + width = widths[ci] if widths else 0.0, + depth = depths[ci] if depths else 0.0, + point_count = point_counts[ci] if point_counts else 0, + ) + + # 5. Spawn new tracks for unmatched clusters + for ci in unmatched_c: + track = KalmanTrack( + track_id = self._next_id, + centroid = cent_arr[ci], + q_pos = self._q_pos, + q_vel = self._q_vel, + r_pos = self._r_pos, + n_init_frames = self._n_init, + max_coasting = self._max_coast, + ) + track.update( + cent_arr[ci], + width = widths[ci] if widths else 0.0, + depth = depths[ci] if depths else 0.0, + point_count = point_counts[ci] if point_counts else 0, + ) + self._tracks[self._next_id] = track + self._next_id += 1 + + # 6. Prune dead tracks + self._tracks = {tid: t for tid, t in self._tracks.items() if t.alive} + + return list(self._tracks.values()) diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/obstacle_velocity_node.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/obstacle_velocity_node.py new file mode 100644 index 0000000..c37baba --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/obstacle_velocity_node.py @@ -0,0 +1,172 @@ +""" +obstacle_velocity_node.py — Dynamic obstacle velocity estimator (Issue #326). + +Subscribes to the 2-D LIDAR scan, clusters points using the existing +gap-segmentation helper, tracks cluster centroids with per-obstacle +constant-velocity Kalman filters, and publishes estimated velocity +vectors on /saltybot/obstacle_velocities. + +Subscribes: + /scan sensor_msgs/LaserScan (BEST_EFFORT) + +Publishes: + /saltybot/obstacle_velocities saltybot_scene_msgs/ObstacleVelocityArray + +Parameters +---------- +distance_threshold_m float 0.20 Max gap between consecutive scan points (m) +min_points int 3 Minimum LIDAR points per valid cluster +range_min_m float 0.05 Discard ranges below this (m) +range_max_m float 12.0 Discard ranges above this (m) +max_association_dist_m float 0.50 Max centroid distance for track-cluster match +max_coasting_frames int 5 Missed frames before a track is deleted +n_init_frames int 3 Updates required to reach confidence=1.0 +q_pos float 0.05 Process noise — position (m²/s²) +q_vel float 0.50 Process noise — velocity (m²/s³) +r_pos float 0.10 Measurement noise std-dev (m) +static_speed_threshold float 0.10 Speed (m/s) below which obstacle is static +""" + +from __future__ import annotations + +import time +from typing import List + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy + +from sensor_msgs.msg import LaserScan +from saltybot_scene_msgs.msg import ObstacleVelocity, ObstacleVelocityArray + +from ._lidar_clustering import scan_to_cartesian, cluster_points +from ._obstacle_velocity import ObstacleTracker + + +_SENSOR_QOS = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=4, +) + + +class ObstacleVelocityNode(Node): + + def __init__(self) -> None: + super().__init__('obstacle_velocity_node') + + self.declare_parameter('distance_threshold_m', 0.20) + self.declare_parameter('min_points', 3) + self.declare_parameter('range_min_m', 0.05) + self.declare_parameter('range_max_m', 12.0) + self.declare_parameter('max_association_dist_m', 0.50) + self.declare_parameter('max_coasting_frames', 5) + self.declare_parameter('n_init_frames', 3) + self.declare_parameter('q_pos', 0.05) + self.declare_parameter('q_vel', 0.50) + self.declare_parameter('r_pos', 0.10) + self.declare_parameter('static_speed_threshold', 0.10) + + self._dist_thresh = float(self.get_parameter('distance_threshold_m').value) + self._min_pts = int(self.get_parameter('min_points').value) + self._range_min = float(self.get_parameter('range_min_m').value) + self._range_max = float(self.get_parameter('range_max_m').value) + self._static_thresh = float(self.get_parameter('static_speed_threshold').value) + + self._tracker = ObstacleTracker( + max_association_dist_m = float(self.get_parameter('max_association_dist_m').value), + max_coasting_frames = int(self.get_parameter('max_coasting_frames').value), + n_init_frames = int(self.get_parameter('n_init_frames').value), + q_pos = float(self.get_parameter('q_pos').value), + q_vel = float(self.get_parameter('q_vel').value), + r_pos = float(self.get_parameter('r_pos').value), + ) + + self._sub = self.create_subscription( + LaserScan, '/scan', self._on_scan, _SENSOR_QOS) + self._pub = self.create_publisher( + ObstacleVelocityArray, '/saltybot/obstacle_velocities', 10) + + self.get_logger().info( + f'obstacle_velocity_node ready — ' + f'dist_thresh={self._dist_thresh}m ' + f'static_thresh={self._static_thresh}m/s' + ) + + # ── Scan callback ───────────────────────────────────────────────────────── + + def _on_scan(self, msg: LaserScan) -> None: + points = scan_to_cartesian( + list(msg.ranges), + msg.angle_min, + msg.angle_increment, + range_min=self._range_min, + range_max=self._range_max, + ) + clusters = cluster_points( + points, + distance_threshold_m=self._dist_thresh, + min_points=self._min_pts, + ) + + centroids = [c.centroid for c in clusters] + widths = [c.width_m for c in clusters] + depths = [c.depth_m for c in clusters] + point_counts = [len(c.points) for c in clusters] + + # Use scan timestamp if available, fall back to wall clock + stamp_secs = ( + msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 + if msg.header.stamp.sec > 0 + else time.time() + ) + + alive_tracks = self._tracker.update( + centroids, + timestamp = stamp_secs, + widths = widths, + depths = depths, + point_counts = point_counts, + ) + + # ── Build and publish message ───────────────────────────────────────── + out = ObstacleVelocityArray() + out.header.stamp = msg.header.stamp + out.header.frame_id = msg.header.frame_id or 'laser' + + for track in alive_tracks: + pos = track.position + vel = track.velocity + + obs = ObstacleVelocity() + obs.header = out.header + obs.obstacle_id = track.track_id + obs.centroid.x = float(pos[0]) + obs.centroid.y = float(pos[1]) + obs.centroid.z = 0.0 + obs.velocity.x = float(vel[0]) + obs.velocity.y = float(vel[1]) + obs.velocity.z = 0.0 + obs.speed_mps = float(track.speed) + obs.width_m = float(track.last_width) + obs.depth_m = float(track.last_depth) + obs.point_count = int(track.last_point_count) + obs.confidence = float(track.confidence) + obs.is_static = track.speed < self._static_thresh + out.obstacles.append(obs) + + self._pub.publish(out) + + +def main(args=None) -> None: + rclpy.init(args=args) + node = ObstacleVelocityNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/jetson/ros2_ws/src/saltybot_bringup/setup.py b/jetson/ros2_ws/src/saltybot_bringup/setup.py index 25a76a5..b4aface 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/setup.py +++ b/jetson/ros2_ws/src/saltybot_bringup/setup.py @@ -51,6 +51,8 @@ setup( 'wheel_odom = saltybot_bringup.wheel_odom_node:main', # Appearance-based person re-identification (Issue #322) 'person_reid = saltybot_bringup.person_reid_node:main', + # Dynamic obstacle velocity estimator (Issue #326) + 'obstacle_velocity = saltybot_bringup.obstacle_velocity_node:main', ], }, ) diff --git a/jetson/ros2_ws/src/saltybot_bringup/test/test_obstacle_velocity.py b/jetson/ros2_ws/src/saltybot_bringup/test/test_obstacle_velocity.py new file mode 100644 index 0000000..86b26ab --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/test/test_obstacle_velocity.py @@ -0,0 +1,467 @@ +""" +test_obstacle_velocity.py — Unit tests for obstacle velocity estimation (no ROS2). + +Covers: + KalmanTrack — construction: + - initial position matches centroid + - initial velocity is zero + - initial confidence is 0.0 + - initial alive=True, coasting=0, age=0 + + KalmanTrack — predict(): + - predict(dt=0) leaves position unchanged + - predict(dt=1) with zero velocity leaves position unchanged + - predict() increments coasting by 1 + - alive stays True until coasting > max_coasting + - alive becomes False exactly when coasting > max_coasting + + KalmanTrack — update(): + - update() resets coasting to 0 + - update() increments age + - update() shifts position toward measurement + - update() reduces position uncertainty (trace of P decreases) + - confidence increases with age, caps at 1.0 + - confidence never exceeds 1.0 + - metadata stored (width, depth, point_count) + + KalmanTrack — velocity convergence: + - constant-velocity target: vx converges toward true velocity after N steps + - stationary target: speed stays near zero + + associate(): + - empty tracks → all clusters unmatched + - empty clusters → all tracks unmatched + - both empty → empty matches + - single perfect match below max_dist + - single pair above max_dist → no match + - two tracks two clusters: diagonal nearest wins + - more tracks than clusters: extra tracks unmatched + - more clusters than tracks: extra clusters unmatched + + ObstacleTracker: + - empty centroids → no tracks created + - single cluster → one track, confidence=0 initially + - same position twice → track maintained, confidence increases + - cluster disappears: coasts then deleted after max_coasting+1 frames + - new cluster after deletion gets new track_id + - two clusters → two tracks, correct IDs + - moving cluster: velocity direction correct after convergence + - track_id is monotonically increasing + - metadata (width, depth, point_count) stored on track +""" + +import sys +import os +import math + +import numpy as np +import pytest + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) + +from saltybot_bringup._obstacle_velocity import ( + KalmanTrack, + ObstacleTracker, + associate, +) + + +# ── Helpers ─────────────────────────────────────────────────────────────────── + +def _c(x: float, y: float) -> np.ndarray: + return np.array([x, y], dtype=np.float64) + + +def _track(x: float = 0.0, y: float = 0.0, **kw) -> KalmanTrack: + return KalmanTrack(1, _c(x, y), **kw) + + +# ── KalmanTrack — construction ──────────────────────────────────────────────── + +class TestKalmanTrackInit: + + def test_initial_position(self): + t = _track(3.0, -1.5) + assert t.position == pytest.approx([3.0, -1.5], abs=1e-9) + + def test_initial_velocity_zero(self): + t = _track(1.0, 2.0) + assert t.velocity == pytest.approx([0.0, 0.0], abs=1e-9) + + def test_initial_speed_zero(self): + t = _track(1.0, 0.0) + assert t.speed == pytest.approx(0.0, abs=1e-9) + + def test_initial_confidence_zero(self): + t = KalmanTrack(1, _c(0, 0), n_init_frames=3) + assert t.confidence == pytest.approx(0.0, abs=1e-9) + + def test_initial_alive(self): + assert _track().alive is True + + def test_initial_coasting_zero(self): + assert _track().coasting == 0 + + def test_initial_age_zero(self): + assert _track().age == 0 + + +# ── KalmanTrack — predict ───────────────────────────────────────────────────── + +class TestKalmanTrackPredict: + + def test_predict_dt_zero_no_position_change(self): + t = _track(2.0, 3.0) + t.predict(0.0) + assert t.position == pytest.approx([2.0, 3.0], abs=1e-9) + + def test_predict_dt_positive_zero_velocity_no_position_change(self): + t = _track(1.0, 1.0) + t.predict(1.0) + assert t.position == pytest.approx([1.0, 1.0], abs=1e-6) + + def test_predict_negative_dt_clamped_to_zero(self): + t = _track(1.0, 0.0) + t.predict(-5.0) + assert t.position == pytest.approx([1.0, 0.0], abs=1e-9) + + def test_predict_increments_coasting(self): + t = _track() + assert t.coasting == 0 + t.predict(0.1) + assert t.coasting == 1 + t.predict(0.1) + assert t.coasting == 2 + + def test_alive_before_max_coasting(self): + t = KalmanTrack(1, _c(0, 0), max_coasting=3) + for _ in range(3): + t.predict(0.1) + assert t.alive is True # coast=3, 3 > 3 is False → still alive + + def test_alive_false_after_exceeding_max_coasting(self): + t = KalmanTrack(1, _c(0, 0), max_coasting=3) + for _ in range(4): + t.predict(0.1) + assert t.alive is False # coast=4, 4 > 3 → dead + + def test_predict_advances_position_when_velocity_set(self): + """After seeding velocity via update, predict advances x.""" + t = KalmanTrack(1, _c(0.0, 0.0), r_pos=0.001, n_init_frames=1) + # Drive velocity to ~(1,0) by updating with advancing centroids + for i in range(1, 8): + t.predict(1.0) + t.update(_c(float(i), 0.0)) + # Now predict one more step — position should advance in +x + x0 = t.position[0] + t.predict(1.0) + assert t.position[0] > x0 + + +# ── KalmanTrack — update ────────────────────────────────────────────────────── + +class TestKalmanTrackUpdate: + + def test_update_resets_coasting(self): + t = _track() + t.predict(0.1) # coast=1 + t.predict(0.1) # coast=2 + t.update(_c(0, 0)) + assert t.coasting == 0 + + def test_update_increments_age(self): + t = _track() + t.update(_c(1, 0)) + assert t.age == 1 + t.update(_c(2, 0)) + assert t.age == 2 + + def test_update_shifts_position_toward_measurement(self): + t = _track(0.0, 0.0) + t.update(_c(5.0, 0.0)) + # Position should have moved in +x direction + assert t.position[0] > 0.0 + + def test_update_reduces_position_covariance(self): + t = KalmanTrack(1, _c(0, 0)) + p_before = float(t._P[0, 0]) + t.update(_c(0, 0)) + p_after = float(t._P[0, 0]) + assert p_after < p_before + + def test_confidence_increases_with_age(self): + t = KalmanTrack(1, _c(0, 0), n_init_frames=4) + assert t.confidence == pytest.approx(0.0, abs=1e-9) + t.update(_c(0, 0)) + assert t.confidence == pytest.approx(0.25) + t.update(_c(0, 0)) + assert t.confidence == pytest.approx(0.50) + t.update(_c(0, 0)) + assert t.confidence == pytest.approx(0.75) + t.update(_c(0, 0)) + assert t.confidence == pytest.approx(1.0) + + def test_confidence_caps_at_one(self): + t = KalmanTrack(1, _c(0, 0), n_init_frames=2) + for _ in range(10): + t.update(_c(0, 0)) + assert t.confidence == pytest.approx(1.0) + + def test_update_stores_metadata(self): + t = _track() + t.update(_c(1, 1), width=0.3, depth=0.5, point_count=7) + assert t.last_width == pytest.approx(0.3) + assert t.last_depth == pytest.approx(0.5) + assert t.last_point_count == 7 + + +# ── KalmanTrack — velocity convergence ─────────────────────────────────────── + +class TestKalmanTrackVelocityConvergence: + + def test_constant_velocity_converges(self): + """ + Target at vx=1 m/s: observations at x=0,1,2,... + After 15 predict+update cycles with low noise, vx should be near 1.0. + """ + t = KalmanTrack(1, _c(0.0, 0.0), r_pos=0.01, q_pos=0.001, q_vel=0.1) + for i in range(1, 16): + t.predict(1.0) + t.update(_c(float(i), 0.0)) + assert t.velocity[0] == pytest.approx(1.0, abs=0.15) + assert t.velocity[1] == pytest.approx(0.0, abs=0.10) + + def test_diagonal_velocity_converges(self): + """Target moving at (vx=1, vy=1) m/s.""" + t = KalmanTrack(1, _c(0.0, 0.0), r_pos=0.01, q_pos=0.001, q_vel=0.1) + for i in range(1, 16): + t.predict(1.0) + t.update(_c(float(i), float(i))) + assert t.velocity[0] == pytest.approx(1.0, abs=0.15) + assert t.velocity[1] == pytest.approx(1.0, abs=0.15) + + def test_stationary_target_speed_near_zero(self): + """Target at fixed position: estimated speed should stay small.""" + t = KalmanTrack(1, _c(2.0, 3.0), r_pos=0.001) + for _ in range(15): + t.predict(0.1) + t.update(_c(2.0, 3.0)) + assert t.speed < 0.05 + + +# ── associate ───────────────────────────────────────────────────────────────── + +class TestAssociate: + + def test_empty_tracks(self): + pos = np.empty((0, 2)) + cent = np.array([[1.0, 0.0]]) + m, ut, uc = associate(pos, cent, 1.0) + assert m == [] + assert ut == [] + assert uc == [0] + + def test_empty_clusters(self): + pos = np.array([[1.0, 0.0]]) + cent = np.empty((0, 2)) + m, ut, uc = associate(pos, cent, 1.0) + assert m == [] + assert ut == [0] + assert uc == [] + + def test_both_empty(self): + m, ut, uc = associate(np.empty((0, 2)), np.empty((0, 2)), 1.0) + assert m == [] + assert ut == [] + assert uc == [] + + def test_single_match_below_threshold(self): + pos = np.array([[0.0, 0.0]]) + cent = np.array([[0.1, 0.0]]) + m, ut, uc = associate(pos, cent, 0.5) + assert m == [(0, 0)] + assert ut == [] + assert uc == [] + + def test_single_pair_above_threshold_no_match(self): + pos = np.array([[0.0, 0.0]]) + cent = np.array([[2.0, 0.0]]) + m, ut, uc = associate(pos, cent, 0.5) + assert m == [] + assert ut == [0] + assert uc == [0] + + def test_two_tracks_two_clusters_diagonal(self): + pos = np.array([[0.0, 0.0], [3.0, 0.0]]) + cent = np.array([[0.1, 0.0], [3.1, 0.0]]) + m, ut, uc = associate(pos, cent, 0.5) + assert (0, 0) in m + assert (1, 1) in m + assert ut == [] + assert uc == [] + + def test_more_tracks_than_clusters(self): + pos = np.array([[0.0, 0.0], [5.0, 0.0]]) + cent = np.array([[0.1, 0.0]]) + m, ut, uc = associate(pos, cent, 0.5) + assert len(m) == 1 + assert len(ut) == 1 # one track unmatched + assert uc == [] + + def test_more_clusters_than_tracks(self): + pos = np.array([[0.0, 0.0]]) + cent = np.array([[0.1, 0.0], [5.0, 0.0]]) + m, ut, uc = associate(pos, cent, 0.5) + assert len(m) == 1 + assert ut == [] + assert len(uc) == 1 # one cluster unmatched + + def test_nearest_wins(self): + """Track 0 is closest to cluster 0; ensure it's matched to cluster 0.""" + pos = np.array([[0.0, 0.0], [10.0, 0.0]]) + cent = np.array([[0.05, 0.0], [10.2, 0.0]]) + m, ut, uc = associate(pos, cent, 1.0) + assert (0, 0) in m + assert (1, 1) in m + + def test_threshold_strictly_less_than(self): + """Distance exactly equal to max_dist should NOT match.""" + pos = np.array([[0.0, 0.0]]) + cent = np.array([[0.5, 0.0]]) + m, ut, uc = associate(pos, cent, 0.5) # dist=0.5, max_dist=0.5 + assert m == [] + + +# ── ObstacleTracker ─────────────────────────────────────────────────────────── + +class TestObstacleTracker: + + def test_empty_input_no_tracks(self): + tracker = ObstacleTracker() + result = tracker.update([], timestamp=0.0) + assert result == [] + + def test_single_cluster_creates_track(self): + tracker = ObstacleTracker() + tracks = tracker.update([_c(1, 0)], timestamp=0.0) + assert len(tracks) == 1 + + def test_track_id_starts_at_one(self): + tracker = ObstacleTracker() + tracks = tracker.update([_c(0, 0)], timestamp=0.0) + assert tracks[0].track_id == 1 + + def test_same_position_twice_single_track(self): + tracker = ObstacleTracker() + t1 = tracker.update([_c(1, 0)], timestamp=0.0) + t2 = tracker.update([_c(1, 0)], timestamp=0.1) + assert len(t2) == 1 + assert t2[0].track_id == t1[0].track_id + + def test_confidence_increases_with_updates(self): + tracker = ObstacleTracker(n_init_frames=3) + tracks = tracker.update([_c(0, 0)], timestamp=0.0) + c0 = tracks[0].confidence + tracks = tracker.update([_c(0, 0)], timestamp=0.1) + assert tracks[0].confidence > c0 + + def test_track_coasts_then_dies(self): + tracker = ObstacleTracker(max_coasting_frames=2) + tracker.update([_c(0, 0)], timestamp=0.0) # create + t1 = tracker.update([], timestamp=0.1) # coast 1 + assert len(t1) == 1 + t2 = tracker.update([], timestamp=0.2) # coast 2 + assert len(t2) == 1 + t3 = tracker.update([], timestamp=0.3) # coast 3 > 2 → dead + assert len(t3) == 0 + + def test_new_cluster_after_deletion_new_id(self): + tracker = ObstacleTracker(max_coasting_frames=1) + t0 = tracker.update([_c(0, 0)], timestamp=0.0) + old_id = t0[0].track_id + tracker.update([], timestamp=0.1) # coast 1 + tracker.update([], timestamp=0.2) # coast 2 > 1 → dead + t1 = tracker.update([_c(0, 0)], timestamp=0.3) + assert len(t1) == 1 + assert t1[0].track_id != old_id + + def test_two_clusters_two_tracks(self): + tracker = ObstacleTracker() + tracks = tracker.update([_c(0, 0), _c(5, 0)], timestamp=0.0) + assert len(tracks) == 2 + ids = {t.track_id for t in tracks} + assert len(ids) == 2 + + def test_track_ids_monotonically_increasing(self): + tracker = ObstacleTracker() + tracker.update([_c(0, 0)], timestamp=0.0) + tracker.update([_c(10, 10)], timestamp=0.1) # far → new track + all_ids = [t.track_id for t in tracker.tracks.values()] + assert all_ids == sorted(all_ids) + + def test_moving_cluster_velocity_direction(self): + """ + Cluster moves in +x at 1 m/s; after convergence vx should be positive. + Use low noise and many steps for reliable convergence. + """ + tracker = ObstacleTracker( + n_init_frames=1, + r_pos=0.01, + q_pos=0.001, + q_vel=0.1, + ) + for i in range(20): + tracker.update([_c(float(i) * 0.1, 0.0)], timestamp=float(i) * 0.1) + tracks = list(tracker.tracks.values()) + assert len(tracks) == 1 + assert tracks[0].velocity[0] > 0.05 + + def test_metadata_stored_on_track(self): + tracker = ObstacleTracker() + tracker.update( + [_c(1, 1)], + timestamp = 0.0, + widths = [0.4], + depths = [0.6], + point_counts = [9], + ) + t = list(tracker.tracks.values())[0] + assert t.last_width == pytest.approx(0.4) + assert t.last_depth == pytest.approx(0.6) + assert t.last_point_count == 9 + + def test_far_cluster_creates_new_track(self): + """Cluster beyond max_association_dist creates a second track.""" + tracker = ObstacleTracker(max_association_dist_m=0.5) + tracker.update([_c(0, 0)], timestamp=0.0) + tracks = tracker.update([_c(10, 0)], timestamp=0.1) + # Original track coasts, new track spawned for far cluster + assert len(tracks) == 2 + + def test_empty_to_single_and_back(self): + tracker = ObstacleTracker(max_coasting_frames=0) + t1 = tracker.update([_c(1, 0)], timestamp=0.0) + assert len(t1) == 1 + t2 = tracker.update([], timestamp=0.1) # coast=1 > 0 → dead + assert len(t2) == 0 + t3 = tracker.update([_c(1, 0)], timestamp=0.2) + assert len(t3) == 1 + + def test_constant_velocity_estimate(self): + """ + Target moves at vx=0.1 m/s (0.1 m per 1-second step). + Each step is well within the default max_association_dist_m=0.5 m, + so the track is continuously matched. After many updates, estimated + speed should be close to 0.1 m/s. + """ + tracker = ObstacleTracker( + n_init_frames=1, r_pos=0.001, q_pos=0.0001, q_vel=0.05 + ) + for i in range(30): + tracker.update([_c(float(i) * 0.1, 0.0)], timestamp=float(i)) + t = list(tracker.tracks.values())[0] + assert t.speed == pytest.approx(0.1, abs=0.03) + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) diff --git a/jetson/ros2_ws/src/saltybot_scene_msgs/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_scene_msgs/CMakeLists.txt index 180bf09..ea476f9 100644 --- a/jetson/ros2_ws/src/saltybot_scene_msgs/CMakeLists.txt +++ b/jetson/ros2_ws/src/saltybot_scene_msgs/CMakeLists.txt @@ -22,6 +22,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} # Issue #322 — cross-camera person re-identification "msg/PersonTrack.msg" "msg/PersonTrackArray.msg" + # Issue #326 — dynamic obstacle velocity estimator + "msg/ObstacleVelocity.msg" + "msg/ObstacleVelocityArray.msg" DEPENDENCIES std_msgs geometry_msgs vision_msgs builtin_interfaces ) diff --git a/jetson/ros2_ws/src/saltybot_scene_msgs/msg/ObstacleVelocity.msg b/jetson/ros2_ws/src/saltybot_scene_msgs/msg/ObstacleVelocity.msg new file mode 100644 index 0000000..0764a08 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_scene_msgs/msg/ObstacleVelocity.msg @@ -0,0 +1,22 @@ +# ObstacleVelocity.msg — tracked obstacle with estimated velocity (Issue #326) +# +# obstacle_id : stable track ID, monotonically increasing, 1-based +# centroid : estimated position in the LIDAR sensor frame (z=0) +# velocity : estimated velocity vector, m/s, in the LIDAR sensor frame +# speed_mps : |velocity| magnitude (m/s) +# width_m : cluster bounding-box width (metres) +# depth_m : cluster bounding-box depth (metres) +# point_count : number of LIDAR returns in the cluster this frame +# confidence : Kalman track age confidence, 0–1 (1.0 after n_init frames) +# is_static : true when speed_mps < static_speed_threshold parameter +# +std_msgs/Header header +uint32 obstacle_id +geometry_msgs/Point centroid +geometry_msgs/Vector3 velocity +float32 speed_mps +float32 width_m +float32 depth_m +uint32 point_count +float32 confidence +bool is_static diff --git a/jetson/ros2_ws/src/saltybot_scene_msgs/msg/ObstacleVelocityArray.msg b/jetson/ros2_ws/src/saltybot_scene_msgs/msg/ObstacleVelocityArray.msg new file mode 100644 index 0000000..44f0d2f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_scene_msgs/msg/ObstacleVelocityArray.msg @@ -0,0 +1,4 @@ +# ObstacleVelocityArray.msg — all tracked obstacles with velocities (Issue #326) +# +std_msgs/Header header +ObstacleVelocity[] obstacles