jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/_blur_detector.py

@@ -0,0 +1,105 @@
+"""
+_blur_detector.py — Motion blur detection via Laplacian variance (no ROS2 deps).
+
+Algorithm
+---------
+1. Convert the BGR image to greyscale.
+2. Apply the discrete Laplacian operator (cv2.Laplacian, ksize=3).
+3. Compute the variance of the Laplacian response across the entire image.
+4. A sharp image has high-contrast edges → high variance.
+   A blurred image smears edges → low variance.
+5. Compare the variance against a configurable threshold:
+     blurred = (variance < threshold)
+
+The result is returned as a BlurResult NamedTuple.  All image-free helpers
+(is_blurred, etc.) work on pre-computed variance values so callers can cache
+expensive computation.
+
+Optional ROI
+------------
+If roi (row_start, row_end, col_start, col_end) is given, only that crop is
+analysed.  Useful for ignoring the sky or far-field clutter that would
+artificially inflate variance.
+
+Public API
+----------
+  BlurResult(variance, is_blurred, threshold)
+  laplacian_variance(bgr, roi=None) -> float
+  detect_blur(bgr, threshold=100.0, roi=None) -> BlurResult
+"""
+
+from __future__ import annotations
+
+from typing import NamedTuple, Optional, Tuple
+
+
+# ── Data types ────────────────────────────────────────────────────────────────
+
+class BlurResult(NamedTuple):
+    """Result of a single blur detection evaluation."""
+    variance:   float   # Laplacian variance of the (cropped) image
+    is_blurred: bool    # True when variance < threshold
+    threshold:  float   # threshold used for this evaluation
+
+
+# ── Public API ────────────────────────────────────────────────────────────────
+
+def laplacian_variance(
+    bgr: 'np.ndarray',
+    roi: Optional[Tuple[int, int, int, int]] = None,
+) -> float:
+    """
+    Compute the variance of the Laplacian of a BGR image.
+
+    Parameters
+    ----------
+    bgr : (H, W, 3) or (H, W) uint8 ndarray
+    roi : optional (row_start, row_end, col_start, col_end) — analyse this crop only
+
+    Returns
+    -------
+    float — Laplacian variance (≥ 0); higher = sharper
+    """
+    import cv2
+    import numpy as np
+
+    if bgr.ndim == 3:
+        grey = cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY)
+    else:
+        grey = np.asarray(bgr, dtype=np.uint8)
+
+    if roi is not None:
+        r0, r1, c0, c1 = roi
+        grey = grey[r0:r1, c0:c1]
+
+    if grey.size == 0:
+        return 0.0
+
+    lap = cv2.Laplacian(grey, cv2.CV_64F, ksize=3)
+    return float(lap.var())
+
+
+def detect_blur(
+    bgr:       'np.ndarray',
+    threshold: float = 100.0,
+    roi:       Optional[Tuple[int, int, int, int]] = None,
+) -> BlurResult:
+    """
+    Detect whether an image is motion-blurred.
+
+    Parameters
+    ----------
+    bgr       : (H, W, 3) uint8 BGR ndarray (or greyscale (H, W))
+    threshold : Laplacian variance below which the image is considered blurred
+    roi       : optional (row_start, row_end, col_start, col_end) crop
+
+    Returns
+    -------
+    BlurResult(variance, is_blurred, threshold)
+    """
+    var = laplacian_variance(bgr, roi=roi)
+    return BlurResult(
+        variance=var,
+        is_blurred=var < threshold,
+        threshold=threshold,
+    )

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/blur_detect_node.py

@@ -0,0 +1,105 @@
+"""
+blur_detect_node.py — D435i motion blur detector (Issue #286).
+
+Subscribes to the RealSense colour stream, evaluates the Laplacian variance
+blur metric, and publishes a Bool flag plus a Float32 score.
+
+Intended use: pause or de-weight SLAM keyframe insertion when the robot is
+moving fast and the image is visibly blurred.
+
+Subscribes (BEST_EFFORT):
+  /camera/color/image_raw     sensor_msgs/Image   BGR8
+
+Publishes:
+  /saltybot/image_blurred     std_msgs/Bool       True  = blurred
+  /saltybot/blur_score        std_msgs/Float32    Laplacian variance (higher = sharper)
+
+Parameters
+----------
+threshold   float   100.0   Variance below which image is considered blurred
+roi_frac    float   0.0     Fraction of image height to skip from top (0 = full frame)
+"""
+
+from __future__ import annotations
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+
+from cv_bridge import CvBridge
+
+from sensor_msgs.msg import Image
+from std_msgs.msg import Bool, Float32
+
+from ._blur_detector import detect_blur
+
+
+_SENSOR_QOS = QoSProfile(
+    reliability=ReliabilityPolicy.BEST_EFFORT,
+    history=HistoryPolicy.KEEP_LAST,
+    depth=4,
+)
+
+
+class BlurDetectNode(Node):
+
+    def __init__(self) -> None:
+        super().__init__('blur_detect_node')
+
+        self.declare_parameter('threshold', 100.0)
+        self.declare_parameter('roi_frac',  0.0)
+
+        self._threshold = float(self.get_parameter('threshold').value)
+        self._roi_frac  = float(self.get_parameter('roi_frac').value)
+
+        self._bridge = CvBridge()
+
+        self._sub = self.create_subscription(
+            Image, '/camera/color/image_raw', self._on_image, _SENSOR_QOS)
+        self._pub_flag  = self.create_publisher(Bool,    '/saltybot/image_blurred', 10)
+        self._pub_score = self.create_publisher(Float32, '/saltybot/blur_score',    10)
+
+        self.get_logger().info(
+            f'blur_detect_node ready — threshold={self._threshold} '
+            f'roi_frac={self._roi_frac}'
+        )
+
+    # ── Callback ──────────────────────────────────────────────────────────────
+
+    def _on_image(self, msg: Image) -> None:
+        try:
+            bgr = self._bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        except Exception as exc:
+            self.get_logger().error(
+                f'cv_bridge: {exc}', throttle_duration_sec=5.0)
+            return
+
+        roi = None
+        if self._roi_frac > 0.0:
+            h = bgr.shape[0]
+            r0 = int(h * min(self._roi_frac, 0.9))
+            roi = (r0, h, 0, bgr.shape[1])
+
+        result = detect_blur(bgr, threshold=self._threshold, roi=roi)
+
+        flag_msg = Bool()
+        flag_msg.data = result.is_blurred
+        self._pub_flag.publish(flag_msg)
+
+        score_msg = Float32()
+        score_msg.data = float(result.variance)
+        self._pub_score.publish(score_msg)
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = BlurDetectNode()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

jetson/ros2_ws/src/saltybot_bringup/setup.py

@@ -41,6 +41,8 @@ setup(
             'depth_hole_fill         = saltybot_bringup.depth_hole_fill_node:main',
             # HSV color object segmenter (Issue #274)
             'color_segmenter         = saltybot_bringup.color_segment_node:main',
+            # Motion blur detector (Issue #286)
+            'blur_detector           = saltybot_bringup.blur_detect_node:main',
         ],
     },
 )

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

@@ -0,0 +1,257 @@
+"""
+test_blur_detector.py — Unit tests for blur detection helpers (no ROS2 required).
+
+Covers:
+  BlurResult:
+    - fields accessible by name
+    - is_blurred True when variance < threshold
+    - is_blurred False when variance >= threshold
+
+  laplacian_variance:
+    - sharp synthetic image has higher variance than blurred version
+    - solid (zero-texture) image returns near-zero variance
+    - greyscale input accepted without error
+    - ROI crop returns variance of cropped region only
+    - empty ROI returns 0.0
+    - returns float
+    - variance is non-negative
+
+  detect_blur — output contract:
+    - returns BlurResult
+    - threshold stored in result matches input
+    - variance matches laplacian_variance independently computed
+    - is_blurred consistent with variance vs threshold
+
+  detect_blur — sharp vs blurred:
+    - sharp checkerboard → not blurred at default threshold
+    - heavily Gaussian-blurred image → blurred at default threshold
+    - sharp image variance > blurred image variance
+    - very low threshold → sharp image reported blurred
+    - very high threshold → blurred image still reported blurred
+
+  detect_blur — ROI:
+    - sharp ROI on blurred image → ROI variant has higher variance
+    - blurred ROI on sharp image → ROI variant has lower variance
+    - roi_frac=0 equivalent to no ROI
+"""
+
+import sys
+import os
+
+import numpy as np
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
+
+from saltybot_bringup._blur_detector import (
+    BlurResult,
+    laplacian_variance,
+    detect_blur,
+)
+
+
+# ── Image factories ───────────────────────────────────────────────────────────
+
+def _checkerboard(h=64, w=64, tile=8) -> np.ndarray:
+    """High-frequency checkerboard — very sharp, high Laplacian variance."""
+    img = np.zeros((h, w), dtype=np.uint8)
+    for r in range(h):
+        for c in range(w):
+            if ((r // tile) + (c // tile)) % 2 == 0:
+                img[r, c] = 255
+    bgr = np.stack([img, img, img], axis=-1)
+    return bgr
+
+
+def _blurred(bgr, ksize=21) -> np.ndarray:
+    """Gaussian-blur a BGR image."""
+    import cv2
+    k = ksize if ksize % 2 == 1 else ksize + 1
+    return cv2.GaussianBlur(bgr, (k, k), 0)
+
+
+def _solid(val=128, h=64, w=64) -> np.ndarray:
+    """Solid grey BGR image — zero texture."""
+    return np.full((h, w, 3), val, dtype=np.uint8)
+
+
+def _gradient(h=64, w=64) -> np.ndarray:
+    """Horizontal linear gradient — mild texture."""
+    row = np.linspace(0, 255, w, dtype=np.uint8)
+    img = np.tile(row, (h, 1))
+    return np.stack([img, img, img], axis=-1)
+
+
+# ── BlurResult ────────────────────────────────────────────────────────────────
+
+class TestBlurResult:
+
+    def test_fields_accessible(self):
+        r = BlurResult(variance=50.0, is_blurred=True, threshold=100.0)
+        assert r.variance == pytest.approx(50.0)
+        assert r.is_blurred is True
+        assert r.threshold == pytest.approx(100.0)
+
+    def test_is_blurred_true_when_variance_below_threshold(self):
+        r = BlurResult(variance=30.0, is_blurred=30.0 < 100.0, threshold=100.0)
+        assert r.is_blurred is True
+
+    def test_is_blurred_false_when_variance_above_threshold(self):
+        r = BlurResult(variance=200.0, is_blurred=200.0 < 100.0, threshold=100.0)
+        assert r.is_blurred is False
+
+
+# ── laplacian_variance ────────────────────────────────────────────────────────
+
+class TestLaplacianVariance:
+
+    def test_returns_float(self):
+        v = laplacian_variance(_checkerboard())
+        assert isinstance(v, float)
+
+    def test_variance_non_negative(self):
+        for img in [_checkerboard(), _solid(), _blurred(_checkerboard())]:
+            assert laplacian_variance(img) >= 0.0
+
+    def test_sharp_higher_than_blurred(self):
+        sharp = _checkerboard()
+        blurred = _blurred(sharp, ksize=21)
+        assert laplacian_variance(sharp) > laplacian_variance(blurred)
+
+    def test_solid_image_near_zero(self):
+        v = laplacian_variance(_solid(128))
+        assert v < 1.0, f'solid image variance should be near 0, got {v}'
+
+    def test_greyscale_input_accepted(self):
+        grey = np.zeros((32, 32), dtype=np.uint8)
+        grey[::4, ::4] = 255
+        v = laplacian_variance(grey)
+        assert isinstance(v, float)
+        assert v >= 0.0
+
+    def test_roi_returns_crop_variance(self):
+        """Left half: solid black. Right half: checkerboard.
+        ROI over right half should have high variance."""
+        h, w = 64, 64
+        img = np.zeros((h, w, 3), dtype=np.uint8)
+        cb = _checkerboard(h, w // 2, tile=4)
+        img[:, w // 2:] = cb
+        var_full = laplacian_variance(img)
+        var_roi  = laplacian_variance(img, roi=(0, h, w // 2, w))
+        assert var_roi > var_full, (
+            f'ROI over checkerboard side should have higher variance: '
+            f'roi={var_roi:.1f} full={var_full:.1f}'
+        )
+
+    def test_empty_roi_returns_zero(self):
+        img = _checkerboard()
+        v = laplacian_variance(img, roi=(10, 10, 5, 5))  # zero-size crop
+        assert v == pytest.approx(0.0)
+
+    def test_checkerboard_has_high_variance(self):
+        v = laplacian_variance(_checkerboard(64, 64, tile=4))
+        assert v > 500.0, f'checkerboard should have high variance, got {v:.1f}'
+
+
+# ── detect_blur — output contract ─────────────────────────────────────────────
+
+class TestDetectBlurContract:
+
+    def test_returns_blur_result(self):
+        r = detect_blur(_checkerboard())
+        assert isinstance(r, BlurResult)
+
+    def test_threshold_stored_in_result(self):
+        r = detect_blur(_checkerboard(), threshold=77.5)
+        assert r.threshold == pytest.approx(77.5)
+
+    def test_variance_matches_independent_computation(self):
+        img = _checkerboard()
+        r = detect_blur(img, threshold=100.0)
+        expected = laplacian_variance(img)
+        assert r.variance == pytest.approx(expected, rel=1e-5)
+
+    def test_is_blurred_consistent_with_variance(self):
+        img = _checkerboard()
+        r = detect_blur(img, threshold=100.0)
+        assert r.is_blurred == (r.variance < r.threshold)
+
+
+# ── detect_blur — sharp vs blurred ────────────────────────────────────────────
+
+class TestDetectBlurSharpVsBlurred:
+
+    def test_checkerboard_not_blurred_at_default_threshold(self):
+        r = detect_blur(_checkerboard(), threshold=100.0)
+        assert not r.is_blurred, (
+            f'sharp checkerboard should not be blurred, variance={r.variance:.1f}')
+
+    def test_heavily_blurred_image_is_blurred(self):
+        blurred = _blurred(_checkerboard(), ksize=31)
+        r = detect_blur(blurred, threshold=100.0)
+        assert r.is_blurred, (
+            f'heavily blurred image should be flagged, variance={r.variance:.1f}')
+
+    def test_sharp_variance_greater_than_blurred(self):
+        sharp   = _checkerboard()
+        blurred = _blurred(sharp, ksize=21)
+        r_sharp   = detect_blur(sharp)
+        r_blurred = detect_blur(blurred)
+        assert r_sharp.variance > r_blurred.variance
+
+    def test_very_high_threshold_flags_sharp_as_blurred(self):
+        """If threshold is set above any realistic variance, even sharp images are flagged."""
+        r = detect_blur(_checkerboard(), threshold=1e9)
+        assert r.is_blurred
+
+    def test_very_high_threshold_flags_blurred_as_blurred(self):
+        blurred = _blurred(_checkerboard(), ksize=31)
+        r = detect_blur(blurred, threshold=1e9)
+        assert r.is_blurred
+
+    def test_solid_image_is_blurred_at_default_threshold(self):
+        r = detect_blur(_solid(128), threshold=100.0)
+        assert r.is_blurred, (
+            f'featureless solid should be flagged, variance={r.variance:.1f}')
+
+    def test_gradient_variance_positive(self):
+        """A linear gradient has some edges → variance > 0."""
+        r = detect_blur(_gradient())
+        assert r.variance > 0.0
+
+
+# ── detect_blur — ROI ────────────────────────────────────────────────────────
+
+class TestDetectBlurROI:
+
+    def test_sharp_roi_on_blurred_image_has_higher_variance(self):
+        """Create an image where one half is sharp and the other blurred."""
+        h, w = 64, 128
+        sharp_half   = _checkerboard(h, w // 2, tile=4)
+        blurred_half = _blurred(sharp_half, ksize=21)
+        img = np.concatenate([sharp_half, blurred_half], axis=1)
+
+        var_sharp_roi   = laplacian_variance(img, roi=(0, h, 0, w // 2))
+        var_blurred_roi = laplacian_variance(img, roi=(0, h, w // 2, w))
+        assert var_sharp_roi > var_blurred_roi, (
+            f'sharp ROI ({var_sharp_roi:.1f}) should exceed blurred ROI ({var_blurred_roi:.1f})'
+        )
+
+    def test_detect_blur_roi_param_passed_through(self):
+        """detect_blur with roi gives same variance as laplacian_variance with same roi."""
+        img = _checkerboard()
+        roi = (0, 32, 0, 32)
+        r   = detect_blur(img, threshold=100.0, roi=roi)
+        expected = laplacian_variance(img, roi=roi)
+        assert r.variance == pytest.approx(expected, rel=1e-5)
+
+    def test_no_roi_uses_full_frame(self):
+        img = _checkerboard()
+        r_no_roi   = detect_blur(img, threshold=100.0)
+        r_full_roi = detect_blur(img, threshold=100.0,
+                                 roi=(0, img.shape[0], 0, img.shape[1]))
+        assert r_no_roi.variance == pytest.approx(r_full_roi.variance, rel=1e-5)
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])