jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_lidar_clustering.py

@@ -0,0 +1,131 @@
+"""
+_lidar_clustering.py — Euclidean clustering of 2-D LIDAR scan points (no ROS2 deps).
+
+Algorithm
+---------
+1. Convert LaserScan ranges to Cartesian (x, y) points, dropping inf / NaN.
+2. Walk through the points in angular order.  When the Euclidean distance
+   between consecutive scan points exceeds `distance_threshold_m`, a new
+   cluster begins (gap-based segmentation — O(N), standard for dense 360°
+   LIDAR scans at short range).
+3. Discard clusters with fewer than `min_points` points.
+
+Each cluster is returned as a Cluster namedtuple with:
+  points      — (N, 2) float64 ndarray of (x, y) in metres
+  centroid    — (2,)   float64 ndarray, mean of points
+  bbox_min    — (2,)   min (x, y) corner of axis-aligned bounding box
+  bbox_max    — (2,)   max (x, y) corner of axis-aligned bounding box
+  width_m     — bbox X extent (metres)
+  depth_m     — bbox Y extent (metres)
+
+Coordinate frame: 2-D projection of the LIDAR optical frame (+X forward, +Y left).
+"""
+
+from __future__ import annotations
+
+import math
+from typing import List, NamedTuple
+
+import numpy as np
+
+
+class Cluster(NamedTuple):
+    points:   np.ndarray   # (N, 2) float64
+    centroid: np.ndarray   # (2,)   float64
+    bbox_min: np.ndarray   # (2,)   float64
+    bbox_max: np.ndarray   # (2,)   float64
+    width_m:  float
+    depth_m:  float
+
+
+def scan_to_cartesian(
+    ranges:          List[float],
+    angle_min:       float,
+    angle_increment: float,
+    range_min:       float = 0.05,
+    range_max:       float = 12.0,
+) -> np.ndarray:
+    """
+    Convert LaserScan ranges to a (M, 2) float64 array of valid (x, y) points.
+
+    Parameters
+    ----------
+    ranges          : raw range values from LaserScan.ranges
+    angle_min       : start angle of the scan (radians)
+    angle_increment : angular step between consecutive readings (radians)
+    range_min       : readings below this are discarded (m)
+    range_max       : readings above this are discarded (m)
+
+    Returns
+    -------
+    (M, 2) ndarray of valid Cartesian points, in scan order.
+    """
+    angles = angle_min + np.arange(len(ranges)) * angle_increment
+    r      = np.asarray(ranges, dtype=np.float64)
+
+    # Mask out invalid readings
+    valid  = np.isfinite(r) & (r >= range_min) & (r <= range_max)
+    r      = r[valid]
+    a      = angles[valid]
+
+    x = r * np.cos(a)
+    y = r * np.sin(a)
+    return np.column_stack((x, y))   # (M, 2)
+
+
+def cluster_points(
+    points:              np.ndarray,
+    distance_threshold_m: float = 0.20,
+    min_points:          int   = 3,
+) -> List[Cluster]:
+    """
+    Cluster a (M, 2) array of scan-order Cartesian points.
+
+    Parameters
+    ----------
+    points               : output of scan_to_cartesian()
+    distance_threshold_m : max distance between consecutive points in the same cluster
+    min_points           : discard clusters with fewer than this many points
+
+    Returns
+    -------
+    List of Cluster objects, sorted by distance from the origin (nearest first).
+    """
+    if len(points) == 0:
+        return []
+
+    clusters: List[Cluster] = []
+    current: List[np.ndarray] = [points[0]]
+
+    for i in range(1, len(points)):
+        gap = np.linalg.norm(points[i] - points[i - 1])
+        if gap <= distance_threshold_m:
+            current.append(points[i])
+        else:
+            _flush(current, clusters, min_points)
+            current = [points[i]]
+
+    _flush(current, clusters, min_points)
+
+    # Sort by distance of centroid from origin (nearest first)
+    clusters.sort(key=lambda c: float(np.linalg.norm(c.centroid)))
+    return clusters
+
+
+def _flush(pts: List[np.ndarray], out: List[Cluster], min_pts: int) -> None:
+    """Convert accumulated points list into a Cluster and append to out."""
+    if len(pts) < min_pts:
+        return
+    arr     = np.stack(pts)              # (N, 2)
+    centroid = arr.mean(axis=0)
+    bmin     = arr.min(axis=0)
+    bmax     = arr.max(axis=0)
+    span     = bmax - bmin
+    out.append(Cluster(
+        points   = arr,
+        centroid = centroid,
+        bbox_min = bmin,
+        bbox_max = bmax,
+        width_m  = float(span[0]),
+        depth_m  = float(span[1]),
+    ))

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/lidar_clustering_node.py

@@ -0,0 +1,214 @@
+"""
+lidar_clustering_node.py — LIDAR object clustering and RViz visualisation (Issue #239).
+
+Subscribes to the 2-D LIDAR scan, segments nearby points into objects using
+Euclidean distance threshold clustering, and publishes a MarkerArray so each
+cluster is visible as a labelled bounding-box cube in RViz.
+
+Subscribes:
+  /scan                    sensor_msgs/LaserScan   (RPLIDAR A1M8)
+
+Publishes:
+  /saltybot/lidar_clusters visualization_msgs/MarkerArray
+
+Algorithm
+---------
+1. Convert LaserScan ranges to Cartesian (x, y) points.
+2. Walk scan-order points — start a new cluster when the gap between
+   consecutive points exceeds `distance_threshold_m`.
+3. Discard clusters below `min_points`.
+4. Publish one CUBE marker per cluster (centroid + bbox dims) plus one TEXT
+   marker showing cluster index and point count.
+
+Parameters
+----------
+distance_threshold_m  float  0.20   Max gap between consecutive scan points (m)
+min_points            int    3      Min points for a valid cluster
+range_min_m           float  0.05   Discard ranges below this (m)
+range_max_m           float  12.0   Discard ranges above this (m)
+marker_lifetime_s     float  0.15   Marker lifetime — auto-expire if scan stalls
+marker_z              float  0.0    Z position of cube markers in the LIDAR frame
+marker_height         float  0.3    Fixed height of cube markers (m)
+"""
+
+from __future__ import annotations
+
+from typing import List
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+from rclpy.duration import Duration
+
+from sensor_msgs.msg import LaserScan
+from visualization_msgs.msg import Marker, MarkerArray
+from geometry_msgs.msg import Point
+from std_msgs.msg import ColorRGBA
+from builtin_interfaces.msg import Duration as DurationMsg
+
+from ._lidar_clustering import scan_to_cartesian, cluster_points, Cluster
+
+
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=4,
+)
+
+# Distinct hue palette — up to 12 clusters get unique colours; wraps for more
+_PALETTE: List[tuple] = [
+    (0.90, 0.20, 0.20),  # red
+    (0.20, 0.70, 0.20),  # green
+    (0.20, 0.50, 0.90),  # blue
+    (0.90, 0.70, 0.10),  # yellow
+    (0.80, 0.30, 0.90),  # purple
+    (0.20, 0.85, 0.85),  # cyan
+    (0.95, 0.50, 0.10),  # orange
+    (0.50, 0.90, 0.30),  # lime
+    (0.90, 0.20, 0.70),  # pink
+    (0.40, 0.20, 0.90),  # indigo
+    (0.10, 0.70, 0.50),  # teal
+    (0.70, 0.50, 0.20),  # brown
+]
+
+
+class LidarClusteringNode(Node):
+
+    def __init__(self) -> None:
+        super().__init__('lidar_clustering_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('marker_lifetime_s',    0.15)
+        self.declare_parameter('marker_z',             0.0)
+        self.declare_parameter('marker_height',        0.3)
+
+        self._dist_thresh  = self.get_parameter('distance_threshold_m').value
+        self._min_pts      = self.get_parameter('min_points').value
+        self._range_min    = self.get_parameter('range_min_m').value
+        self._range_max    = self.get_parameter('range_max_m').value
+        self._lifetime_s   = self.get_parameter('marker_lifetime_s').value
+        self._marker_z     = self.get_parameter('marker_z').value
+        self._marker_h     = self.get_parameter('marker_height').value
+
+        self._sub = self.create_subscription(
+            LaserScan, '/scan', self._on_scan, _SENSOR_QOS)
+        self._pub = self.create_publisher(
+            MarkerArray, '/saltybot/lidar_clusters', 10)
+
+        # Track how many markers were published last cycle so we can DELETE extras
+        self._prev_count = 0
+
+        self.get_logger().info(
+            f'lidar_clustering_node ready — '
+            f'dist={self._dist_thresh}m min_pts={self._min_pts}'
+        )
+
+    # ── 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,
+        )
+
+        markers: List[Marker] = []
+        lifetime = _duration_msg(self._lifetime_s)
+        frame_id = msg.header.frame_id or 'laser'
+        stamp    = msg.header.stamp
+
+        for idx, cluster in enumerate(clusters):
+            color = _PALETTE[idx % len(_PALETTE)]
+
+            # ── CUBE marker (bounding box) ────────────────────────────────────
+            cube = Marker()
+            cube.header.frame_id = frame_id
+            cube.header.stamp    = stamp
+            cube.ns              = 'lidar_clusters'
+            cube.id              = idx * 2
+            cube.type            = Marker.CUBE
+            cube.action          = Marker.ADD
+            cube.pose.position.x = float(cluster.centroid[0])
+            cube.pose.position.y = float(cluster.centroid[1])
+            cube.pose.position.z = self._marker_z
+            cube.pose.orientation.w = 1.0
+            # Minimum visible size of 0.1 m per axis
+            cube.scale.x = max(cluster.width_m, 0.10)
+            cube.scale.y = max(cluster.depth_m, 0.10)
+            cube.scale.z = self._marker_h
+            cube.color   = _rgba(*color, a=0.45)
+            cube.lifetime = lifetime
+            markers.append(cube)
+
+            # ── TEXT marker (id + count) ──────────────────────────────────────
+            label = Marker()
+            label.header.frame_id = frame_id
+            label.header.stamp    = stamp
+            label.ns              = 'lidar_clusters'
+            label.id              = idx * 2 + 1
+            label.type            = Marker.TEXT_VIEW_FACING
+            label.action          = Marker.ADD
+            label.pose.position.x = float(cluster.centroid[0])
+            label.pose.position.y = float(cluster.centroid[1])
+            label.pose.position.z = self._marker_z + self._marker_h * 0.5 + 0.05
+            label.pose.orientation.w = 1.0
+            label.scale.z         = 0.12   # text height (m)
+            label.color           = _rgba(1.0, 1.0, 1.0, a=0.9)
+            label.text            = f'#{idx}  n={len(cluster.points)}'
+            label.lifetime        = lifetime
+            markers.append(label)
+
+        # DELETE markers from the previous cycle that no longer have a cluster
+        new_count = len(clusters)
+        for idx in range(new_count, self._prev_count):
+            for offset in (0, 1):
+                del_m = Marker()
+                del_m.ns     = 'lidar_clusters'
+                del_m.id     = idx * 2 + offset
+                del_m.action = Marker.DELETE
+                markers.append(del_m)
+
+        self._prev_count = new_count
+
+        ma = MarkerArray()
+        ma.markers = markers
+        self._pub.publish(ma)
+
+
+# ── Helpers ───────────────────────────────────────────────────────────────────
+
+def _rgba(r: float, g: float, b: float, a: float) -> ColorRGBA:
+    c = ColorRGBA()
+    c.r = r; c.g = g; c.b = b; c.a = a
+    return c
+
+
+def _duration_msg(seconds: float) -> DurationMsg:
+    d = DurationMsg()
+    d.sec     = int(seconds)
+    d.nanosec = int((seconds - int(seconds)) * 1e9)
+    return d
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = LidarClusteringNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_bringup/setup.py

@@ -31,6 +31,8 @@ setup(
             'depth_confidence_filter = saltybot_bringup.depth_confidence_filter_node:main',
             'camera_health_monitor   = saltybot_bringup.camera_health_node:main',
             'scan_height_filter      = saltybot_bringup.scan_height_filter_node:main',
+            # LIDAR object clustering + RViz visualisation (Issue #239)
+            'lidar_clustering        = saltybot_bringup.lidar_clustering_node:main',
         ],
     },
 )

jetson/ros2_ws/src/saltybot_bringup/test/test_lidar_clustering.py

@@ -0,0 +1,191 @@
+"""
+test_lidar_clustering.py — Unit tests for LIDAR clustering helpers (no ROS2 required).
+
+Covers:
+  scan_to_cartesian:
+    - principal ray (angle=0) maps to (+range, 0)
+    - 90° ray maps to (0, +range)
+    - inf / NaN ranges are discarded
+    - ranges outside [range_min, range_max] are discarded
+    - all-invalid scan returns empty array
+    - output shape and dtype
+
+  cluster_points:
+    - two well-separated groups → two clusters
+    - one tight group → one cluster
+    - consecutive points just within threshold stay together
+    - consecutive points just over threshold split
+    - fewer than min_points dropped
+    - empty input returns empty list
+    - clusters sorted nearest-first
+    - cluster centroid is mean of points
+    - cluster bbox correct
+"""
+
+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._lidar_clustering import (
+    scan_to_cartesian,
+    cluster_points,
+    Cluster,
+)
+
+
+# ── scan_to_cartesian ─────────────────────────────────────────────────────────
+
+class TestScanToCartesian:
+
+    def test_forward_ray(self):
+        """angle=0 → (range, 0)"""
+        pts = scan_to_cartesian([2.0], angle_min=0.0, angle_increment=0.0)
+        assert pts.shape == (1, 2)
+        assert pts[0, 0] == pytest.approx(2.0, abs=1e-6)
+        assert pts[0, 1] == pytest.approx(0.0, abs=1e-6)
+
+    def test_left_ray(self):
+        """angle=π/2 → (0, range)"""
+        pts = scan_to_cartesian([3.0], angle_min=math.pi / 2, angle_increment=0.0)
+        assert pts[0, 0] == pytest.approx(0.0,  abs=1e-6)
+        assert pts[0, 1] == pytest.approx(3.0,  abs=1e-6)
+
+    def test_inf_discarded(self):
+        pts = scan_to_cartesian([float('inf'), 1.0], angle_min=0.0, angle_increment=0.1)
+        assert pts.shape == (1, 2)   # only the valid reading
+
+    def test_nan_discarded(self):
+        pts = scan_to_cartesian([float('nan'), 1.0], angle_min=0.0, angle_increment=0.1)
+        assert pts.shape == (1, 2)
+
+    def test_below_range_min_discarded(self):
+        pts = scan_to_cartesian([0.01, 1.0], angle_min=0.0,
+                                angle_increment=0.1, range_min=0.05)
+        assert pts.shape == (1, 2)
+
+    def test_above_range_max_discarded(self):
+        pts = scan_to_cartesian([1.0, 50.0], angle_min=0.0,
+                                angle_increment=0.1, range_max=12.0)
+        assert pts.shape == (1, 2)
+
+    def test_all_invalid_returns_empty(self):
+        pts = scan_to_cartesian([float('inf')] * 10, angle_min=0.0, angle_increment=0.1)
+        assert pts.shape[0] == 0
+
+    def test_output_dtype_float64(self):
+        pts = scan_to_cartesian([1.0, 2.0], angle_min=0.0, angle_increment=0.1)
+        assert pts.dtype == np.float64
+
+    def test_output_shape_n_by_2(self):
+        pts = scan_to_cartesian([1.0, 2.0, 3.0], angle_min=0.0, angle_increment=0.1)
+        assert pts.shape == (3, 2)
+
+    def test_negative_angle(self):
+        """-π/2 → (0, -range)"""
+        pts = scan_to_cartesian([2.5], angle_min=-math.pi / 2, angle_increment=0.0)
+        assert pts[0, 0] == pytest.approx(0.0,  abs=1e-6)
+        assert pts[0, 1] == pytest.approx(-2.5, abs=1e-6)
+
+
+# ── cluster_points ────────────────────────────────────────────────────────────
+
+def _pts(*xy_pairs) -> np.ndarray:
+    """Convenience: _pts(0,0, 1,0, 2,0) → (3,2) array."""
+    return np.array(xy_pairs, dtype=np.float64).reshape(-1, 2)
+
+
+class TestClusterPoints:
+
+    def test_empty_input_returns_empty_list(self):
+        result = cluster_points(np.empty((0, 2)))
+        assert result == []
+
+    def test_two_separated_groups(self):
+        """Points at x≈0 and x≈10 should form two clusters."""
+        group_a = _pts(0.0, 0.0,  0.1, 0.0,  0.2, 0.0)  # 3 pts near origin
+        group_b = _pts(10.0, 0.0, 10.1, 0.0, 10.2, 0.0) # 3 pts far away
+        pts = np.vstack([group_a, group_b])
+        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=2)
+        assert len(clusters) == 2
+
+    def test_one_tight_group(self):
+        pts = _pts(0.0, 0.0,  0.05, 0.0,  0.10, 0.0,  0.15, 0.0)
+        clusters = cluster_points(pts, distance_threshold_m=0.20, min_points=3)
+        assert len(clusters) == 1
+
+    def test_gap_just_within_threshold_stays_together(self):
+        # gap = exactly threshold → same cluster
+        pts = _pts(0.0, 0.0,  0.20, 0.0,  0.40, 0.0)
+        clusters = cluster_points(pts, distance_threshold_m=0.20, min_points=2)
+        assert len(clusters) == 1
+
+    def test_gap_just_over_threshold_splits(self):
+        # gap = threshold + epsilon → different clusters
+        pts = _pts(0.0, 0.0,  0.21, 0.0,  0.42, 0.0)
+        clusters = cluster_points(pts, distance_threshold_m=0.20, min_points=1)
+        assert len(clusters) == 3   # every point is its own cluster
+
+    def test_fewer_than_min_points_dropped(self):
+        pts = _pts(0.0, 0.0,  0.05, 0.0)   # only 2 pts
+        clusters = cluster_points(pts, distance_threshold_m=0.20, min_points=3)
+        assert clusters == []
+
+    def test_sorted_nearest_first(self):
+        far_pts  = _pts(8.0, 0.0,  8.1, 0.0,  8.2, 0.0)
+        near_pts = _pts(1.0, 0.0,  1.1, 0.0,  1.2, 0.0)
+        pts = np.vstack([far_pts, near_pts])  # far group listed first
+        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=2)
+        assert len(clusters) == 2
+        dist0 = np.linalg.norm(clusters[0].centroid)
+        dist1 = np.linalg.norm(clusters[1].centroid)
+        assert dist0 < dist1
+
+    def test_centroid_is_mean(self):
+        pts = _pts(0.0, 0.0,  2.0, 0.0)   # gap=2 > threshold, but with min_pts=1
+        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=1)
+        # two separate clusters; first cluster centroid = (0,0)
+        c0 = [c for c in clusters if np.linalg.norm(c.centroid) < 0.5][0]
+        np.testing.assert_allclose(c0.centroid, [0.0, 0.0])
+
+    def test_bbox_min_max_correct(self):
+        pts = _pts(1.0, 2.0,  3.0, 4.0,  2.0, 3.0)
+        clusters = cluster_points(pts, distance_threshold_m=5.0, min_points=2)
+        assert len(clusters) == 1
+        c = clusters[0]
+        np.testing.assert_allclose(c.bbox_min, [1.0, 2.0])
+        np.testing.assert_allclose(c.bbox_max, [3.0, 4.0])
+
+    def test_width_depth_correct(self):
+        pts = _pts(0.0, 0.0,  4.0, 0.0,  0.0, 2.0,  4.0, 2.0)
+        clusters = cluster_points(pts, distance_threshold_m=10.0, min_points=4)
+        assert len(clusters) == 1
+        c = clusters[0]
+        assert c.width_m == pytest.approx(4.0, abs=1e-9)
+        assert c.depth_m == pytest.approx(2.0, abs=1e-9)
+
+    def test_single_point_cluster_with_min_pts_1(self):
+        pts = _pts(5.0, 5.0)
+        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=1)
+        assert len(clusters) == 1
+        np.testing.assert_allclose(clusters[0].centroid, [5.0, 5.0])
+
+    def test_cluster_fields_are_namedtuple(self):
+        pts = _pts(0.0, 0.0,  0.1, 0.0,  0.2, 0.0)
+        clusters = cluster_points(pts, distance_threshold_m=0.5, min_points=3)
+        assert len(clusters) == 1
+        c = clusters[0]
+        assert hasattr(c, 'points')
+        assert hasattr(c, 'centroid')
+        assert hasattr(c, 'bbox_min')
+        assert hasattr(c, 'bbox_max')
+        assert hasattr(c, 'width_m')
+        assert hasattr(c, 'depth_m')
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])