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())

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()

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',
         ],
     },
 )

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'])

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
 )
 

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

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