Compare commits
No commits in common. "cb8f6c82a44d68ad398b68f6ecf0d0febaef1cfd" and "de1166058cce469364946d7f441e7f8f2bce93e9" have entirely different histories.
cb8f6c82a4
...
de1166058c
@ -25,8 +25,6 @@
|
|||||||
<exec_depend>saltybot_follower</exec_depend>
|
<exec_depend>saltybot_follower</exec_depend>
|
||||||
<exec_depend>saltybot_outdoor</exec_depend>
|
<exec_depend>saltybot_outdoor</exec_depend>
|
||||||
<exec_depend>saltybot_perception</exec_depend>
|
<exec_depend>saltybot_perception</exec_depend>
|
||||||
<!-- HSV color segmentation messages (Issue #274) -->
|
|
||||||
<exec_depend>saltybot_scene_msgs</exec_depend>
|
|
||||||
<exec_depend>saltybot_uwb</exec_depend>
|
<exec_depend>saltybot_uwb</exec_depend>
|
||||||
|
|
||||||
<buildtool_depend>ament_python</buildtool_depend>
|
<buildtool_depend>ament_python</buildtool_depend>
|
||||||
|
|||||||
@ -1,184 +0,0 @@
|
|||||||
"""
|
|
||||||
_color_segmenter.py — HSV color segmentation helpers (no ROS2 deps).
|
|
||||||
|
|
||||||
Algorithm
|
|
||||||
---------
|
|
||||||
For each requested color:
|
|
||||||
1. Convert BGR → HSV (OpenCV: H∈[0,180], S∈[0,255], V∈[0,255])
|
|
||||||
2. Build a binary mask via cv2.inRange using the color's HSV bounds.
|
|
||||||
Red wraps around H=0/180 so two ranges are OR-combined.
|
|
||||||
3. Morphological open (3×3) to remove noise.
|
|
||||||
4. Find external contours; filter by min_area_px.
|
|
||||||
5. Return ColorBlob NamedTuples — one per surviving contour.
|
|
||||||
|
|
||||||
confidence is the contour area divided by the bounding-rectangle area
|
|
||||||
(how "filled" the bounding box is), clamped to [0, 1].
|
|
||||||
|
|
||||||
Public API
|
|
||||||
----------
|
|
||||||
HsvRange(h_lo, h_hi, s_lo, s_hi, v_lo, v_hi)
|
|
||||||
ColorBlob(color_name, confidence, cx, cy, w, h, area_px, contour_id)
|
|
||||||
COLOR_RANGES : Dict[str, List[HsvRange]] — default per-color HSV ranges
|
|
||||||
mask_for_color(hsv, color_name) -> np.ndarray — uint8 binary mask
|
|
||||||
find_color_blobs(bgr, active_colors, min_area_px, max_blobs_per_color) -> List[ColorBlob]
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
from typing import Dict, List, NamedTuple
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
# ── Data types ────────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
class HsvRange(NamedTuple):
|
|
||||||
"""Single HSV band (OpenCV: H∈[0,180], S/V∈[0,255])."""
|
|
||||||
h_lo: int
|
|
||||||
h_hi: int
|
|
||||||
s_lo: int
|
|
||||||
s_hi: int
|
|
||||||
v_lo: int
|
|
||||||
v_hi: int
|
|
||||||
|
|
||||||
|
|
||||||
class ColorBlob(NamedTuple):
|
|
||||||
"""One detected color object in image coordinates."""
|
|
||||||
color_name: str
|
|
||||||
confidence: float # contour_area / bbox_area (0–1)
|
|
||||||
cx: float # bbox centre x (pixels)
|
|
||||||
cy: float # bbox centre y (pixels)
|
|
||||||
w: float # bbox width (pixels)
|
|
||||||
h: float # bbox height (pixels)
|
|
||||||
area_px: float # contour area (pixels²)
|
|
||||||
contour_id: int # 0-based index within this color in this frame
|
|
||||||
|
|
||||||
|
|
||||||
# ── Default per-color HSV ranges ──────────────────────────────────────────────
|
|
||||||
# Two ranges are used for red (wraps at 0/180).
|
|
||||||
# S_lo=60, V_lo=50 to ignore desaturated / near-black pixels.
|
|
||||||
|
|
||||||
COLOR_RANGES: Dict[str, List[HsvRange]] = {
|
|
||||||
'red': [
|
|
||||||
HsvRange(h_lo=0, h_hi=10, s_lo=60, s_hi=255, v_lo=50, v_hi=255),
|
|
||||||
HsvRange(h_lo=170, h_hi=180, s_lo=60, s_hi=255, v_lo=50, v_hi=255),
|
|
||||||
],
|
|
||||||
'green': [
|
|
||||||
HsvRange(h_lo=35, h_hi=85, s_lo=60, s_hi=255, v_lo=50, v_hi=255),
|
|
||||||
],
|
|
||||||
'blue': [
|
|
||||||
HsvRange(h_lo=90, h_hi=130, s_lo=60, s_hi=255, v_lo=50, v_hi=255),
|
|
||||||
],
|
|
||||||
'yellow': [
|
|
||||||
HsvRange(h_lo=18, h_hi=38, s_lo=60, s_hi=255, v_lo=80, v_hi=255),
|
|
||||||
],
|
|
||||||
'orange': [
|
|
||||||
HsvRange(h_lo=8, h_hi=20, s_lo=80, s_hi=255, v_lo=80, v_hi=255),
|
|
||||||
],
|
|
||||||
}
|
|
||||||
|
|
||||||
# Structuring element for morphological open (noise removal)
|
|
||||||
_MORPH_KERNEL = None
|
|
||||||
|
|
||||||
|
|
||||||
def _get_morph_kernel():
|
|
||||||
import cv2
|
|
||||||
global _MORPH_KERNEL
|
|
||||||
if _MORPH_KERNEL is None:
|
|
||||||
_MORPH_KERNEL = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3, 3))
|
|
||||||
return _MORPH_KERNEL
|
|
||||||
|
|
||||||
|
|
||||||
# ── Public helpers ─────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def mask_for_color(hsv: np.ndarray, color_name: str) -> np.ndarray:
|
|
||||||
"""
|
|
||||||
Return a uint8 binary mask (255=foreground) for *color_name* in the HSV image.
|
|
||||||
|
|
||||||
Parameters
|
|
||||||
----------
|
|
||||||
hsv : (H, W, 3) uint8 ndarray in OpenCV HSV format (H∈[0,180])
|
|
||||||
color_name : one of COLOR_RANGES keys
|
|
||||||
|
|
||||||
Returns
|
|
||||||
-------
|
|
||||||
(H, W) uint8 ndarray
|
|
||||||
"""
|
|
||||||
import cv2
|
|
||||||
|
|
||||||
ranges = COLOR_RANGES.get(color_name)
|
|
||||||
if not ranges:
|
|
||||||
raise ValueError(f'Unknown color: {color_name!r}. Known: {list(COLOR_RANGES)}')
|
|
||||||
|
|
||||||
mask = np.zeros(hsv.shape[:2], dtype=np.uint8)
|
|
||||||
for r in ranges:
|
|
||||||
lo = np.array([r.h_lo, r.s_lo, r.v_lo], dtype=np.uint8)
|
|
||||||
hi = np.array([r.h_hi, r.s_hi, r.v_hi], dtype=np.uint8)
|
|
||||||
mask |= cv2.inRange(hsv, lo, hi)
|
|
||||||
|
|
||||||
return cv2.morphologyEx(mask, cv2.MORPH_OPEN, _get_morph_kernel())
|
|
||||||
|
|
||||||
|
|
||||||
def find_color_blobs(
|
|
||||||
bgr: np.ndarray,
|
|
||||||
active_colors: List[str] | None = None,
|
|
||||||
min_area_px: float = 200.0,
|
|
||||||
max_blobs_per_color: int = 10,
|
|
||||||
) -> List[ColorBlob]:
|
|
||||||
"""
|
|
||||||
Detect HSV-segmented color blobs in a BGR image.
|
|
||||||
|
|
||||||
Parameters
|
|
||||||
----------
|
|
||||||
bgr : (H, W, 3) uint8 BGR ndarray
|
|
||||||
active_colors : color names to detect; None → all COLOR_RANGES keys
|
|
||||||
min_area_px : minimum contour area to report (pixels²)
|
|
||||||
max_blobs_per_color : keep at most this many blobs per color (largest first)
|
|
||||||
|
|
||||||
Returns
|
|
||||||
-------
|
|
||||||
List[ColorBlob] — may be empty; contour_id is 0-based within each color
|
|
||||||
"""
|
|
||||||
import cv2
|
|
||||||
|
|
||||||
if active_colors is None:
|
|
||||||
active_colors = list(COLOR_RANGES.keys())
|
|
||||||
|
|
||||||
hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
|
|
||||||
blobs: List[ColorBlob] = []
|
|
||||||
|
|
||||||
for color_name in active_colors:
|
|
||||||
mask = mask_for_color(hsv, color_name)
|
|
||||||
contours, _ = cv2.findContours(
|
|
||||||
mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
|
||||||
|
|
||||||
# Sort largest first so max_blobs_per_color keeps the significant ones
|
|
||||||
contours = sorted(contours, key=cv2.contourArea, reverse=True)
|
|
||||||
|
|
||||||
blob_idx = 0
|
|
||||||
for cnt in contours:
|
|
||||||
if blob_idx >= max_blobs_per_color:
|
|
||||||
break
|
|
||||||
|
|
||||||
area = cv2.contourArea(cnt)
|
|
||||||
if area < min_area_px:
|
|
||||||
break # already sorted, no need to continue
|
|
||||||
|
|
||||||
x, y, bw, bh = cv2.boundingRect(cnt)
|
|
||||||
bbox_area = float(bw * bh)
|
|
||||||
confidence = float(area / bbox_area) if bbox_area > 0 else 0.0
|
|
||||||
confidence = min(1.0, max(0.0, confidence))
|
|
||||||
|
|
||||||
blobs.append(ColorBlob(
|
|
||||||
color_name=color_name,
|
|
||||||
confidence=confidence,
|
|
||||||
cx=float(x + bw / 2.0),
|
|
||||||
cy=float(y + bh / 2.0),
|
|
||||||
w=float(bw),
|
|
||||||
h=float(bh),
|
|
||||||
area_px=float(area),
|
|
||||||
contour_id=blob_idx,
|
|
||||||
))
|
|
||||||
blob_idx += 1
|
|
||||||
|
|
||||||
return blobs
|
|
||||||
@ -1,127 +0,0 @@
|
|||||||
"""
|
|
||||||
color_segment_node.py — D435i HSV color object segmenter (Issue #274).
|
|
||||||
|
|
||||||
Subscribes to the RealSense colour stream, applies per-color HSV thresholding,
|
|
||||||
extracts contours, and publishes detected blobs as ColorDetectionArray.
|
|
||||||
|
|
||||||
Subscribes (BEST_EFFORT):
|
|
||||||
/camera/color/image_raw sensor_msgs/Image BGR8 (or rgb8)
|
|
||||||
|
|
||||||
Publishes:
|
|
||||||
/saltybot/color_objects saltybot_scene_msgs/ColorDetectionArray
|
|
||||||
|
|
||||||
Parameters
|
|
||||||
----------
|
|
||||||
active_colors str "red,green,blue,yellow,orange" Comma-separated list
|
|
||||||
min_area_px float 200.0 Minimum contour area (pixels²)
|
|
||||||
max_blobs_per_color int 10 Max detections per color per frame
|
|
||||||
"""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import rclpy
|
|
||||||
from rclpy.node import Node
|
|
||||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
from cv_bridge import CvBridge
|
|
||||||
|
|
||||||
from sensor_msgs.msg import Image
|
|
||||||
from std_msgs.msg import Header
|
|
||||||
|
|
||||||
from saltybot_scene_msgs.msg import ColorDetection, ColorDetectionArray
|
|
||||||
from vision_msgs.msg import BoundingBox2D
|
|
||||||
from geometry_msgs.msg import Pose2D
|
|
||||||
|
|
||||||
from ._color_segmenter import find_color_blobs
|
|
||||||
|
|
||||||
|
|
||||||
_SENSOR_QOS = QoSProfile(
|
|
||||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
|
||||||
history=HistoryPolicy.KEEP_LAST,
|
|
||||||
depth=4,
|
|
||||||
)
|
|
||||||
|
|
||||||
_DEFAULT_COLORS = 'red,green,blue,yellow,orange'
|
|
||||||
|
|
||||||
|
|
||||||
class ColorSegmentNode(Node):
|
|
||||||
|
|
||||||
def __init__(self) -> None:
|
|
||||||
super().__init__('color_segment_node')
|
|
||||||
|
|
||||||
self.declare_parameter('active_colors', _DEFAULT_COLORS)
|
|
||||||
self.declare_parameter('min_area_px', 200.0)
|
|
||||||
self.declare_parameter('max_blobs_per_color', 10)
|
|
||||||
|
|
||||||
colors_str = self.get_parameter('active_colors').value
|
|
||||||
self._active_colors = [c.strip() for c in colors_str.split(',') if c.strip()]
|
|
||||||
self._min_area = float(self.get_parameter('min_area_px').value)
|
|
||||||
self._max_blobs = int(self.get_parameter('max_blobs_per_color').value)
|
|
||||||
|
|
||||||
self._bridge = CvBridge()
|
|
||||||
|
|
||||||
self._sub = self.create_subscription(
|
|
||||||
Image, '/camera/color/image_raw', self._on_image, _SENSOR_QOS)
|
|
||||||
self._pub = self.create_publisher(
|
|
||||||
ColorDetectionArray, '/saltybot/color_objects', 10)
|
|
||||||
|
|
||||||
self.get_logger().info(
|
|
||||||
f'color_segment_node ready — colors={self._active_colors} '
|
|
||||||
f'min_area={self._min_area}px² max_blobs={self._max_blobs}'
|
|
||||||
)
|
|
||||||
|
|
||||||
# ── 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
|
|
||||||
|
|
||||||
blobs = find_color_blobs(
|
|
||||||
bgr,
|
|
||||||
active_colors=self._active_colors,
|
|
||||||
min_area_px=self._min_area,
|
|
||||||
max_blobs_per_color=self._max_blobs,
|
|
||||||
)
|
|
||||||
|
|
||||||
arr = ColorDetectionArray()
|
|
||||||
arr.header = msg.header
|
|
||||||
|
|
||||||
for blob in blobs:
|
|
||||||
det = ColorDetection()
|
|
||||||
det.header = msg.header
|
|
||||||
det.color_name = blob.color_name
|
|
||||||
det.confidence = blob.confidence
|
|
||||||
det.area_px = blob.area_px
|
|
||||||
det.contour_id = blob.contour_id
|
|
||||||
|
|
||||||
bbox = BoundingBox2D()
|
|
||||||
center = Pose2D()
|
|
||||||
center.x = blob.cx
|
|
||||||
center.y = blob.cy
|
|
||||||
bbox.center = center
|
|
||||||
bbox.size_x = blob.w
|
|
||||||
bbox.size_y = blob.h
|
|
||||||
det.bbox = bbox
|
|
||||||
|
|
||||||
arr.detections.append(det)
|
|
||||||
|
|
||||||
self._pub.publish(arr)
|
|
||||||
|
|
||||||
|
|
||||||
def main(args=None) -> None:
|
|
||||||
rclpy.init(args=args)
|
|
||||||
node = ColorSegmentNode()
|
|
||||||
try:
|
|
||||||
rclpy.spin(node)
|
|
||||||
finally:
|
|
||||||
node.destroy_node()
|
|
||||||
rclpy.shutdown()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
main()
|
|
||||||
@ -39,8 +39,6 @@ setup(
|
|||||||
'vo_drift_detector = saltybot_bringup.vo_drift_node:main',
|
'vo_drift_detector = saltybot_bringup.vo_drift_node:main',
|
||||||
# Depth image hole filler (Issue #268)
|
# Depth image hole filler (Issue #268)
|
||||||
'depth_hole_fill = saltybot_bringup.depth_hole_fill_node:main',
|
'depth_hole_fill = saltybot_bringup.depth_hole_fill_node:main',
|
||||||
# HSV color object segmenter (Issue #274)
|
|
||||||
'color_segmenter = saltybot_bringup.color_segment_node:main',
|
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
)
|
)
|
||||||
|
|||||||
@ -1,361 +0,0 @@
|
|||||||
"""
|
|
||||||
test_color_segmenter.py — Unit tests for HSV color segmentation helpers (no ROS2 required).
|
|
||||||
|
|
||||||
Covers:
|
|
||||||
HsvRange / ColorBlob:
|
|
||||||
- NamedTuple fields accessible by name
|
|
||||||
- confidence clamped to [0,1]
|
|
||||||
|
|
||||||
mask_for_color:
|
|
||||||
- pure red image → red mask fully white
|
|
||||||
- pure red image → green mask fully black
|
|
||||||
- pure green image → green mask fully white
|
|
||||||
- pure blue image → blue mask fully white
|
|
||||||
- pure yellow image → yellow mask non-empty
|
|
||||||
- pure orange image → orange mask non-empty
|
|
||||||
- red hue wrap-around detected from both HSV bands
|
|
||||||
- unknown color name raises ValueError
|
|
||||||
- mask is uint8
|
|
||||||
- mask shape matches input
|
|
||||||
|
|
||||||
find_color_blobs — output contract:
|
|
||||||
- returns list
|
|
||||||
- empty list on blank (no-color) image
|
|
||||||
- empty list when min_area_px larger than any contour
|
|
||||||
|
|
||||||
find_color_blobs — detection:
|
|
||||||
- large red rectangle detected as red blob
|
|
||||||
- large green rectangle detected as green blob
|
|
||||||
- large blue rectangle detected as blue blob
|
|
||||||
- detected blob color_name matches requested color
|
|
||||||
- contour_id is 0 for first blob
|
|
||||||
- confidence in [0, 1]
|
|
||||||
- cx, cy within image bounds
|
|
||||||
- w, h > 0 for detected blob
|
|
||||||
- area_px > 0 for detected blob
|
|
||||||
|
|
||||||
find_color_blobs — filtering:
|
|
||||||
- active_colors=None detects all colors when present
|
|
||||||
- only requested colors returned when active_colors restricted
|
|
||||||
- max_blobs_per_color limits output count
|
|
||||||
- two separate red blobs both detected when max_blobs=2
|
|
||||||
- smaller blob filtered when min_area_px high
|
|
||||||
|
|
||||||
find_color_blobs — multi-color:
|
|
||||||
- image with red + green regions → both detected
|
|
||||||
"""
|
|
||||||
|
|
||||||
import sys
|
|
||||||
import os
|
|
||||||
|
|
||||||
import numpy as np
|
|
||||||
import pytest
|
|
||||||
|
|
||||||
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
|
|
||||||
|
|
||||||
from saltybot_bringup._color_segmenter import (
|
|
||||||
HsvRange,
|
|
||||||
ColorBlob,
|
|
||||||
COLOR_RANGES,
|
|
||||||
mask_for_color,
|
|
||||||
find_color_blobs,
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
# ── Image factories ───────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
def _solid_bgr(b, g, r, h=64, w=64) -> np.ndarray:
|
|
||||||
"""Solid BGR image."""
|
|
||||||
img = np.zeros((h, w, 3), dtype=np.uint8)
|
|
||||||
img[:, :] = (b, g, r)
|
|
||||||
return img
|
|
||||||
|
|
||||||
|
|
||||||
def _blank(h=64, w=64) -> np.ndarray:
|
|
||||||
"""All-black image (nothing to detect)."""
|
|
||||||
return np.zeros((h, w, 3), dtype=np.uint8)
|
|
||||||
|
|
||||||
|
|
||||||
def _image_with_rect(bg_bgr, rect_bgr, rect_slice_r, rect_slice_c, h=128, w=128) -> np.ndarray:
|
|
||||||
"""Background colour with a filled rectangle."""
|
|
||||||
img = np.zeros((h, w, 3), dtype=np.uint8)
|
|
||||||
img[:, :] = bg_bgr
|
|
||||||
img[rect_slice_r, rect_slice_c] = rect_bgr
|
|
||||||
return img
|
|
||||||
|
|
||||||
|
|
||||||
# Canonical solid color BGR values (saturated, in-range for HSV thresholds)
|
|
||||||
_RED_BGR = (0, 0, 200) # BGR pure red
|
|
||||||
_GREEN_BGR = (0, 200, 0 ) # BGR pure green
|
|
||||||
_BLUE_BGR = (200, 0, 0 ) # BGR pure blue
|
|
||||||
_YELLOW_BGR = (0, 220, 220) # BGR yellow
|
|
||||||
_ORANGE_BGR = (0, 140, 220) # BGR orange
|
|
||||||
|
|
||||||
|
|
||||||
# ── HsvRange / ColorBlob types ────────────────────────────────────────────────
|
|
||||||
|
|
||||||
class TestTypes:
|
|
||||||
|
|
||||||
def test_hsv_range_fields(self):
|
|
||||||
r = HsvRange(0, 10, 60, 255, 50, 255)
|
|
||||||
assert r.h_lo == 0 and r.h_hi == 10
|
|
||||||
assert r.s_lo == 60 and r.s_hi == 255
|
|
||||||
assert r.v_lo == 50 and r.v_hi == 255
|
|
||||||
|
|
||||||
def test_color_blob_fields(self):
|
|
||||||
b = ColorBlob('red', 0.8, 32.0, 32.0, 20.0, 20.0, 300.0, 0)
|
|
||||||
assert b.color_name == 'red'
|
|
||||||
assert b.confidence == pytest.approx(0.8)
|
|
||||||
assert b.contour_id == 0
|
|
||||||
|
|
||||||
def test_color_ranges_contains_all_defaults(self):
|
|
||||||
for color in ('red', 'green', 'blue', 'yellow', 'orange'):
|
|
||||||
assert color in COLOR_RANGES
|
|
||||||
assert len(COLOR_RANGES[color]) >= 1
|
|
||||||
|
|
||||||
|
|
||||||
# ── mask_for_color ────────────────────────────────────────────────────────────
|
|
||||||
|
|
||||||
class TestMaskForColor:
|
|
||||||
|
|
||||||
def test_mask_is_uint8(self):
|
|
||||||
import cv2
|
|
||||||
hsv = cv2.cvtColor(_solid_bgr(*_RED_BGR), cv2.COLOR_BGR2HSV)
|
|
||||||
m = mask_for_color(hsv, 'red')
|
|
||||||
assert m.dtype == np.uint8
|
|
||||||
|
|
||||||
def test_mask_shape_matches_input(self):
|
|
||||||
import cv2
|
|
||||||
bgr = _solid_bgr(*_RED_BGR, h=48, w=80)
|
|
||||||
hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
|
|
||||||
m = mask_for_color(hsv, 'red')
|
|
||||||
assert m.shape == (48, 80)
|
|
||||||
|
|
||||||
def test_pure_red_gives_red_mask_nonzero(self):
|
|
||||||
import cv2
|
|
||||||
hsv = cv2.cvtColor(_solid_bgr(*_RED_BGR), cv2.COLOR_BGR2HSV)
|
|
||||||
m = mask_for_color(hsv, 'red')
|
|
||||||
assert m.any(), 'red mask should be non-empty for red image'
|
|
||||||
|
|
||||||
def test_pure_red_gives_green_mask_empty(self):
|
|
||||||
import cv2
|
|
||||||
hsv = cv2.cvtColor(_solid_bgr(*_RED_BGR), cv2.COLOR_BGR2HSV)
|
|
||||||
m = mask_for_color(hsv, 'green')
|
|
||||||
assert not m.any(), 'green mask should be empty for red image'
|
|
||||||
|
|
||||||
def test_pure_green_gives_green_mask_nonzero(self):
|
|
||||||
import cv2
|
|
||||||
hsv = cv2.cvtColor(_solid_bgr(*_GREEN_BGR), cv2.COLOR_BGR2HSV)
|
|
||||||
m = mask_for_color(hsv, 'green')
|
|
||||||
assert m.any()
|
|
||||||
|
|
||||||
def test_pure_blue_gives_blue_mask_nonzero(self):
|
|
||||||
import cv2
|
|
||||||
hsv = cv2.cvtColor(_solid_bgr(*_BLUE_BGR), cv2.COLOR_BGR2HSV)
|
|
||||||
m = mask_for_color(hsv, 'blue')
|
|
||||||
assert m.any()
|
|
||||||
|
|
||||||
def test_pure_yellow_gives_yellow_mask_nonzero(self):
|
|
||||||
import cv2
|
|
||||||
hsv = cv2.cvtColor(_solid_bgr(*_YELLOW_BGR), cv2.COLOR_BGR2HSV)
|
|
||||||
m = mask_for_color(hsv, 'yellow')
|
|
||||||
assert m.any()
|
|
||||||
|
|
||||||
def test_pure_orange_gives_orange_mask_nonzero(self):
|
|
||||||
import cv2
|
|
||||||
hsv = cv2.cvtColor(_solid_bgr(*_ORANGE_BGR), cv2.COLOR_BGR2HSV)
|
|
||||||
m = mask_for_color(hsv, 'orange')
|
|
||||||
assert m.any()
|
|
||||||
|
|
||||||
def test_unknown_color_raises(self):
|
|
||||||
import cv2
|
|
||||||
hsv = cv2.cvtColor(_blank(), cv2.COLOR_BGR2HSV)
|
|
||||||
with pytest.raises(ValueError, match='Unknown color'):
|
|
||||||
mask_for_color(hsv, 'purple')
|
|
||||||
|
|
||||||
def test_red_detected_in_high_hue_band(self):
|
|
||||||
"""A near-180-hue red pixel should still trigger the red mask."""
|
|
||||||
import cv2
|
|
||||||
# HSV (175, 200, 200) = high-hue red (wrap-around band)
|
|
||||||
hsv = np.full((32, 32, 3), (175, 200, 200), dtype=np.uint8)
|
|
||||||
m = mask_for_color(hsv, 'red')
|
|
||||||
assert m.any(), 'high-hue red not detected'
|
|
||||||
|
|
||||||
|
|
||||||
# ── find_color_blobs — output contract ───────────────────────────────────────
|
|
||||||
|
|
||||||
class TestFindColorBlobsContract:
|
|
||||||
|
|
||||||
def test_returns_list(self):
|
|
||||||
result = find_color_blobs(_blank())
|
|
||||||
assert isinstance(result, list)
|
|
||||||
|
|
||||||
def test_blank_image_returns_empty(self):
|
|
||||||
result = find_color_blobs(_blank())
|
|
||||||
assert result == []
|
|
||||||
|
|
||||||
def test_min_area_filter_removes_all(self):
|
|
||||||
"""Request a min area larger than the entire image → no blobs."""
|
|
||||||
bgr = _solid_bgr(*_RED_BGR, h=32, w=32)
|
|
||||||
result = find_color_blobs(bgr, active_colors=['red'], min_area_px=1e9)
|
|
||||||
assert result == []
|
|
||||||
|
|
||||||
|
|
||||||
# ── find_color_blobs — detection ─────────────────────────────────────────────
|
|
||||||
|
|
||||||
class TestFindColorBlobsDetection:
|
|
||||||
|
|
||||||
def _large_rect(self, color_bgr, color_name) -> np.ndarray:
|
|
||||||
"""100×100 image with a 60×60 solid-color rectangle centred."""
|
|
||||||
img = _blank(h=100, w=100)
|
|
||||||
img[20:80, 20:80] = color_bgr
|
|
||||||
return img
|
|
||||||
|
|
||||||
def test_red_rect_detected(self):
|
|
||||||
blobs = find_color_blobs(self._large_rect(_RED_BGR, 'red'), active_colors=['red'])
|
|
||||||
assert len(blobs) >= 1
|
|
||||||
assert blobs[0].color_name == 'red'
|
|
||||||
|
|
||||||
def test_green_rect_detected(self):
|
|
||||||
blobs = find_color_blobs(self._large_rect(_GREEN_BGR, 'green'), active_colors=['green'])
|
|
||||||
assert len(blobs) >= 1
|
|
||||||
assert blobs[0].color_name == 'green'
|
|
||||||
|
|
||||||
def test_blue_rect_detected(self):
|
|
||||||
blobs = find_color_blobs(self._large_rect(_BLUE_BGR, 'blue'), active_colors=['blue'])
|
|
||||||
assert len(blobs) >= 1
|
|
||||||
assert blobs[0].color_name == 'blue'
|
|
||||||
|
|
||||||
def test_first_contour_id_is_zero(self):
|
|
||||||
img = _blank(h=100, w=100)
|
|
||||||
img[20:80, 20:80] = _RED_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red'])
|
|
||||||
assert blobs[0].contour_id == 0
|
|
||||||
|
|
||||||
def test_confidence_in_range(self):
|
|
||||||
img = _blank(h=100, w=100)
|
|
||||||
img[20:80, 20:80] = _GREEN_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['green'])
|
|
||||||
assert blobs
|
|
||||||
assert 0.0 <= blobs[0].confidence <= 1.0
|
|
||||||
|
|
||||||
def test_cx_within_image(self):
|
|
||||||
img = _blank(h=100, w=100)
|
|
||||||
img[20:80, 20:80] = _BLUE_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['blue'])
|
|
||||||
assert blobs
|
|
||||||
assert 0.0 <= blobs[0].cx <= 100.0
|
|
||||||
|
|
||||||
def test_cy_within_image(self):
|
|
||||||
img = _blank(h=100, w=100)
|
|
||||||
img[20:80, 20:80] = _BLUE_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['blue'])
|
|
||||||
assert blobs
|
|
||||||
assert 0.0 <= blobs[0].cy <= 100.0
|
|
||||||
|
|
||||||
def test_w_positive(self):
|
|
||||||
img = _blank(h=100, w=100)
|
|
||||||
img[20:80, 20:80] = _RED_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red'])
|
|
||||||
assert blobs[0].w > 0
|
|
||||||
|
|
||||||
def test_h_positive(self):
|
|
||||||
img = _blank(h=100, w=100)
|
|
||||||
img[20:80, 20:80] = _RED_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red'])
|
|
||||||
assert blobs[0].h > 0
|
|
||||||
|
|
||||||
def test_area_px_positive(self):
|
|
||||||
img = _blank(h=100, w=100)
|
|
||||||
img[20:80, 20:80] = _RED_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red'])
|
|
||||||
assert blobs[0].area_px > 0
|
|
||||||
|
|
||||||
def test_area_px_reasonable(self):
|
|
||||||
"""area_px should be roughly within the rectangle we drew."""
|
|
||||||
img = _blank(h=100, w=100)
|
|
||||||
img[20:80, 20:80] = _GREEN_BGR # 60×60 = 3600 px
|
|
||||||
blobs = find_color_blobs(img, active_colors=['green'], min_area_px=100.0)
|
|
||||||
assert blobs
|
|
||||||
assert 1000 <= blobs[0].area_px <= 4000
|
|
||||||
|
|
||||||
|
|
||||||
# ── find_color_blobs — filtering ─────────────────────────────────────────────
|
|
||||||
|
|
||||||
class TestFindColorBlobsFiltering:
|
|
||||||
|
|
||||||
def test_active_colors_none_detects_all(self):
|
|
||||||
"""Image with red+green patches → both found when active_colors=None."""
|
|
||||||
img = _blank(h=128, w=128)
|
|
||||||
img[10:50, 10:50] = _RED_BGR
|
|
||||||
img[10:50, 70:110] = _GREEN_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=None, min_area_px=100.0)
|
|
||||||
names = {b.color_name for b in blobs}
|
|
||||||
assert 'red' in names
|
|
||||||
assert 'green' in names
|
|
||||||
|
|
||||||
def test_restricted_active_colors(self):
|
|
||||||
"""Only red requested → no green blobs returned."""
|
|
||||||
img = _blank(h=128, w=128)
|
|
||||||
img[10:50, 10:50] = _RED_BGR
|
|
||||||
img[10:50, 70:110] = _GREEN_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red'], min_area_px=100.0)
|
|
||||||
assert all(b.color_name == 'red' for b in blobs)
|
|
||||||
|
|
||||||
def test_max_blobs_per_color_limits(self):
|
|
||||||
"""Four separate red rectangles but max_blobs=2 → at most 2 blobs."""
|
|
||||||
img = _blank(h=200, w=200)
|
|
||||||
img[10:40, 10:40] = _RED_BGR
|
|
||||||
img[10:40, 80:110] = _RED_BGR
|
|
||||||
img[100:130, 10:40] = _RED_BGR
|
|
||||||
img[100:130, 80:110] = _RED_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red'],
|
|
||||||
min_area_px=100.0, max_blobs_per_color=2)
|
|
||||||
red_blobs = [b for b in blobs if b.color_name == 'red']
|
|
||||||
assert len(red_blobs) <= 2
|
|
||||||
|
|
||||||
def test_two_blobs_detected_when_max_allows(self):
|
|
||||||
"""Two red rectangles detected when max_blobs_per_color >= 2."""
|
|
||||||
img = _blank(h=200, w=200)
|
|
||||||
img[10:60, 10:60] = _RED_BGR
|
|
||||||
img[10:60, 130:180] = _RED_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red'],
|
|
||||||
min_area_px=100.0, max_blobs_per_color=10)
|
|
||||||
red_blobs = [b for b in blobs if b.color_name == 'red']
|
|
||||||
assert len(red_blobs) >= 2
|
|
||||||
|
|
||||||
def test_small_blob_filtered_by_min_area(self):
|
|
||||||
"""Small 5×5 red patch filtered by min_area_px=500."""
|
|
||||||
img = _blank(h=64, w=64)
|
|
||||||
img[28:33, 28:33] = _RED_BGR # 5×5 = 25 px contour area
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red'], min_area_px=500.0)
|
|
||||||
assert blobs == []
|
|
||||||
|
|
||||||
|
|
||||||
# ── find_color_blobs — multi-color ───────────────────────────────────────────
|
|
||||||
|
|
||||||
class TestFindColorBlobsMultiColor:
|
|
||||||
|
|
||||||
def test_red_and_green_in_same_image(self):
|
|
||||||
img = _blank(h=128, w=128)
|
|
||||||
img[10:60, 10:60] = _RED_BGR
|
|
||||||
img[10:60, 68:118] = _GREEN_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red', 'green'], min_area_px=100.0)
|
|
||||||
names = {b.color_name for b in blobs}
|
|
||||||
assert 'red' in names, 'red blob should be detected'
|
|
||||||
assert 'green' in names, 'green blob should be detected'
|
|
||||||
|
|
||||||
def test_contour_ids_per_color_start_at_zero(self):
|
|
||||||
"""contour_id should be 0 for the first (largest) blob of each color."""
|
|
||||||
img = _blank(h=200, w=200)
|
|
||||||
img[10:80, 10:80] = _RED_BGR
|
|
||||||
img[10:80, 110:180] = _BLUE_BGR
|
|
||||||
blobs = find_color_blobs(img, active_colors=['red', 'blue'], min_area_px=100.0)
|
|
||||||
for color in ('red', 'blue'):
|
|
||||||
first = next((b for b in blobs if b.color_name == color), None)
|
|
||||||
assert first is not None, f'{color} blob not found'
|
|
||||||
assert first.contour_id == 0, f'{color} first blob contour_id != 0'
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
|
||||||
pytest.main([__file__, '-v'])
|
|
||||||
@ -16,9 +16,6 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
|||||||
# Issue #233 — QR code reader
|
# Issue #233 — QR code reader
|
||||||
"msg/QRDetection.msg"
|
"msg/QRDetection.msg"
|
||||||
"msg/QRDetectionArray.msg"
|
"msg/QRDetectionArray.msg"
|
||||||
# Issue #274 — HSV color segmentation
|
|
||||||
"msg/ColorDetection.msg"
|
|
||||||
"msg/ColorDetectionArray.msg"
|
|
||||||
DEPENDENCIES std_msgs geometry_msgs vision_msgs builtin_interfaces
|
DEPENDENCIES std_msgs geometry_msgs vision_msgs builtin_interfaces
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@ -1,14 +0,0 @@
|
|||||||
# ColorDetection.msg — single HSV color-segmented object detection (Issue #274)
|
|
||||||
#
|
|
||||||
# color_name : target color label ("red", "green", "blue", "yellow", "orange")
|
|
||||||
# confidence : mask fill ratio inside bbox (contour_area / bbox_area, 0–1)
|
|
||||||
# bbox : axis-aligned bounding box in image pixels (center + size)
|
|
||||||
# area_px : contour area in pixels² (use for size filtering downstream)
|
|
||||||
# contour_id : 0-based index of this detection within the current frame
|
|
||||||
#
|
|
||||||
std_msgs/Header header
|
|
||||||
string color_name
|
|
||||||
float32 confidence
|
|
||||||
vision_msgs/BoundingBox2D bbox
|
|
||||||
float32 area_px
|
|
||||||
uint32 contour_id
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
# ColorDetectionArray.msg — frame-level list of HSV color-segmented objects (Issue #274)
|
|
||||||
std_msgs/Header header
|
|
||||||
ColorDetection[] detections
|
|
||||||
Loading…
x
Reference in New Issue
Block a user