260612

好吧，在回家之前，在学校这边事可能会一直都比较多。

我虽然喜欢在学校里待着，但是仅限教学楼，宿舍里还是太差了。

嘛，尽力学吧，反正其实也不差这两天。

我们继续后续的步骤。

接下来我们要接入前向相机和图像桥接。

首先接入前向相机，在 src/kibot_one_sim/models/kibot_one_base/model.sdf 找到 lidar_joint 后，在它后面加上一个独立的 camera_link：

    <link name="camera_link">
      <pose>0.29 0 0.24 0 0 0</pose>
      <sensor name="front_camera" type="camera">
        <topic>/camera/image_raw</topic>
        <update_rate>15</update_rate>
        <always_on>true</always_on>
        <visualize>true</visualize>
        <gz_frame_id>camera_link</gz_frame_id>
        <camera>
          <horizontal_fov>1.047</horizontal_fov>
          <image>
            <width>320</width>
            <height>240</height>
            <format>R8G8B8</format>
          </image>
          <clip>
            <near>0.05</near>
            <far>8.0</far>
          </clip>
        </camera>
      </sensor>
    </link>

其他的不用多做解释，我们这里的相机设置了水平视场角为 1.047。

这个水平视场角的作用是，这个值越大，能看到的就越宽，但是远处的东西看起来就越小。

具体就是像这样的：

然后把它固定到我们的 base_link 上：

    <joint name="camera_joint" type="fixed">
      <parent>base_link</parent>
      <child>camera_link</child>
    </joint>

接着把我们的图像在 Gazebo 和 ROS 中进行桥接起来，在 src/kibot_one_sim/config/ros_gz_bridge.yaml 中补下面的内容：

- ros_topic_name: "/camera/image_raw"
  gz_topic_name: "/camera/image_raw"
  ros_type_name: "sensor_msgs/msg/Image"
  gz_type_name: "gz.msgs.Image"
  direction: "GZ_TO_ROS"

桥接和相机、传感器的部分完成了，我们接下来实现红色旗帜检测的核心代码。

我们先创建src/kibot_one_control/kibot_one_control/flag_detection_core.py，放入所需的结果类型：

from dataclasses import dataclass

@dataclass(frozen=True)
class ColorDetection:
    detected: bool
    center_x: float
    center_y: float
    pixel_count: int
    confidence: float

接下来我们接入对图像进行编码的函数：

def _channels_for_encoding(encoding: str) -> int:
    normalized = encoding.lower()
    if normalized in {"rgb8", "bgr8"}:
        return 3
    if normalized in {"rgba8", "bgra8"}:
        return 4
    return 0


def _rgb_components(pixel: bytes, encoding: str) -> Tuple[int, int, int]:
    normalized = encoding.lower()
    if normalized in {"rgb8", "rgba8"}:
        return pixel[0], pixel[1], pixel[2]
    return pixel[2], pixel[1], pixel[0]


def _empty_detection() -> ColorDetection:
    return ColorDetection(False, 0.0, 0.0, 0, 0.0)

然后实现我们的检测函数：

def detect_red_flag_pixels(
    image_data: bytes,
    width: int,
    height: int,
    encoding: str,
    min_red: int = 120,
    red_margin: int = 45,
    min_pixel_count: int = 80,
) -> ColorDetection:
    channels = _channels_for_encoding(encoding=encoding)
    if width <= 0 or height <= 0 or channels == 0:
        return _empty_detection()
  
    expected_size = width * height * channels
    if len(image_data) < expected_size:
        return _empty_detection()
  
    red_count = 0
    x_sum = 0
    y_sum = 0
    for index in range(0, expected_size, channels):
        pixel = image_data[index:index + channels]
        red, greeen, blue = _rgb_components(pixel=pixel, encoding=encoding)
        if red >= min_red and red >= greeen + red_margin and red >= blue + red_margin:
            pixel_index = index // channels
            x_sum += pixel_index % width
            y_sum += pixel_index // width
            red_count += 1

    if red_count < min_pixel_count:
        return ColorDetection(False, 0.0, 0.0, red_count, red_count / float(width * height))
  
    return ColorDetection(
        True,
        center_x=x_sum / red_count,
        center_y=y_sum / red_count,
        pixel_count=red_count,
        confidence=min(1.0, red_count / float(width * height))
    )

我来解释一下这个函数。

我们先计算这个图片的通道数，如果编码不受支持或者图片字节长度小于预期的字节长度，都认为是非法的图片而直接返回检测失败。

然后以通道数为步长遍历字节流，每次取出一个像素的若干字节，我们就可以得到每个像素每个通道的值。

然后计算每个像素的 RGB 值，如果红色通道值明显大于蓝色和绿色通道、且大于预期的红色最小值，则偏红色像素 +1，且累计偏红色像素的 x, y 坐标。

最后通过计算偏红色像素的总量，如果小于最小的预期像素数量，则也认为是噪声而直接返回检测失败，否则返回检测成功，并返回检测到的红色像素质心、红色像素的总量以及置信度。

然后我们来增加一个单元测试吧 src/kibot_one_control/test/test_flag_detection_core.py：

from kibot_one_control.flag_detection_core import detect_red_flag_pixels


def _rgb_image(width: int, height: int, fill: tuple[int, int, int]) -> bytearray:
    image = bytearray()
    for _ in range(width * height):
        image.extend(fill)
    return image

def test_detect_red_flag_pixels_reports_center_and_count() -> None:
    width = 10
    height = 8
    image = _rgb_image(width, height, (20, 20, 20))
    for y in range(2, 6):
        for x in range(3, 7):
            offset = (y * width + x) * 3
            image[offset:offset + 3] = bytes((220, 20, 20))

    detection = detect_red_flag_pixels(bytes(image), width, height, "rgb8", min_pixel_count=4)

    assert detection.detected is True
    assert detection.pixel_count == 16
    assert detection.center_x == 4.5
    assert detection.center_y == 3.5

def test_detect_red_flag_pixels_rejects_small_red_noise() -> None:
    image = _rgb_image(8, 8, (10, 10, 10))
    image[0:3] = bytes((230, 10, 10))

    detection = detect_red_flag_pixels(bytes(image), 8, 8, "rgb8", min_pixel_count=4)

    assert detection.detected is False
    assert detection.pixel_count == 1

def test_detect_red_flag_pixels_supports_bgr8() -> None:
    image = bytearray()
    for _ in range(12):
        image.extend((10, 20, 210))

    detection = detect_red_flag_pixels(bytes(image), 4, 3, "bgr8", min_pixel_count=3)

    assert detection.detected is True
    assert detection.center_x == 1.5
    assert detection.center_y == 1.0

(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotTwo$ python -m pytest -q src/kibot_one_control/test/test_flag_detection_core.py
...                                                                                                                                                                                   [100%]
3 passed in 0.01s

全部通过，符合预期。