3. ビジョンによる色認識の実装

実験の目的

ROS2 で画像情報を取得し、トピックをパブリッシュできるようになる。2. 色認識の原理を理解する。3. 物体の色を認識できるようになる。

色認識の原理

RGB と HSV のカラーモデル： デジタル画像処理では一般に、RGB（赤・緑・青）と HSV（色相・彩度・明度）の 2 種類のカラーモデルが用いられます。RGB は表現が直感的ですが、R・G・B の数値と色の三属性との間には直接の関係がなく、モデルのチャンネルが物体の具体的な色情報をうまく反映できません。一方、HSV モデルは人が色を表現・解釈する方法により近く、HSV で色を記述するとより直感的になります。

RGB と HSV の違い：

(1) RGB モデル。3 次元座標：

RGB：光の三原色（Red, Green, Blue）

原点から白色の頂点までの中心軸はグレースケール線で、r、g、b の 3 成分が等しくなります。強度は 3 成分のベクトルで表すことができます。

RGB で色相・彩度・明暗の変化を理解すると次のようになります。

色相の変化：3 つの座標軸 RGB の最大成分の頂点と、黄・マゼンタ・シアン（YMC）の頂点を結ぶ線

彩度の変化：RGB の頂点および CMY の頂点から、原点と白色の頂点を結ぶ中心軸までの距離

明暗の変化：中心軸上の点の位置。原点に近いほど暗く、白色の頂点に近いほど明るくなります。

(2) HSV モデル。逆円錐モデル：

このモデルは、色相・彩度・明暗によって色を表現します。

H は色相です。

S は彩度です。S = 0 のときはグレースケールのみになります。

V は明度で、色の明るさの度合いを表します。ただし光の強度とは直接の関係はありません（多少の関係はあります）。

(3) RGB と HSV の関係

上記の直感的な理解から、RGB の 3 次元座標の中心軸を立てて平たくすると、HSV の円錐モデルになります。

ただし、V は RGB の最大成分のみを取り出したものなので、強度とは直接の関係がありません。一方、RGB は光の強度（またはグレースケール）の変化を反映できます。

v = max(r, g, b)

RGB から HSV への変換：

(4) HSV の色範囲

応用シーン

自律移動ロボットや産業用ロボットアームなどにリアルタイムの視覚入力を提供し、物体検出、映像監視、画像解析などのシーンに対応します。

医療業界では、色の異なる薬品の仕分けに利用できます。

機能要件

画像領域内の物体の色を認識し、認識結果を表示します。

ハードウェアの準備

No.	名称	数量	備考
1	RDK X5	1	組込みメインボード（32G メモリカード付き）。上位機システムを実行します。
2	カードリーダー	1	システムイメージの書き込みに使用します。
3	USB カメラ	1	画像を取得します。
4	電源ケーブル	1	給電用。
5	LAN ケーブル	1	ネットワーク接続用。

回路接続

ハードウェアモジュール

ヒント：ハードウェアモジュールの使用説明

1.1 ハードウェアインターフェース：RDK X5 のインターフェースをすばやく把握できます。

1.1 USB カメラの使い方《1.1 USB カメラの使い方》

実機の配線

ソフトウェア設計

設計プロセス

ビジョンによる色認識の設計

ソースコードのダウンロード

資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。

プログラムコード

コードの構成：

camera_node.py：リアルタイムの画像取得とパブリッシュを行い、カメラのトピックをパブリッシュします。

color_detection.launch.py：複数色の対象物の検出・認識、画像のサブスクライブ、結果のパブリッシュを行います。

具体的なコード：

color_detection.launch.py は色認識の処理プログラムです。プロジェクト全体のコードはソースコード（上記「ソースコードのダウンロード」参照）から入手してください。

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np

class ColorDetector(Node):
    def __init__(self):
        super().__init__('color_detector')

        # 声明 ROI 参数（可动态调整）
        self.declare_parameters(
            namespace='',
            parameters=[
                ('roi_x', 155),
                ('roi_y', 120),
                ('roi_width', 385),
                ('roi_height', 250),
                ('min_area', 500)
            ]
        )

        self.roi_x = self.get_parameter('roi_x').value
        self.roi_y = self.get_parameter('roi_y').value
        self.roi_w = self.get_parameter('roi_width').value
        self.roi_h = self.get_parameter('roi_height').value
        self.min_area = self.get_parameter('min_area').value

        self.get_logger().info(f"ROI 设置: x={self.roi_x}, y={self.roi_y}, w={self.roi_w}, h={self.roi_h}")

        # 订阅原始图像
        self.subscription = self.create_subscription(
            Image,
            '/p4/img_raw',
            self.image_callback,
            10
        )
        self.subscription

        # 发布检测结果图像
        self.image_pub = self.create_publisher(Image, '/p4/color_detection', 10)

        self.bridge = CvBridge()

        # HSV 颜色范围
        self.color_ranges = {
            'red': [
                (np.array([0, 43, 46]), np.array([10, 255, 255])),
                (np.array([156, 43, 46]), np.array([180, 255, 255]))
            ],
            'green': [
                (np.array([35, 100, 100]), np.array([85, 255, 255]))
            ],
            'blue': [
                (np.array([90, 100, 100]), np.array([130, 255, 255]))
            ]
        }

        self.draw_colors = {
            'red': (0, 0, 255),
            'green': (0, 255, 0),
            'blue': (255, 0, 0)
        }
        self.center_color = (0, 165, 255)  # Orange in BGR

        self.get_logger().info("颜色检测节点已启动，使用 ROI 检测红/绿/蓝，并发布结果到 /p4/color_detection")

    def image_callback(self, msg):
        try:
            # 转换为 OpenCV 图像
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            original_h, original_w = cv_image.shape[:2]

            # 校验 ROI 是否越界
            x1 = max(0, self.roi_x)
            y1 = max(0, self.roi_y)
            x2 = min(original_w, self.roi_x + self.roi_w)
            y2 = min(original_h, self.roi_y + self.roi_h)

            if x2 <= x1 or y2 <= y1:
                self.get_logger().warn("ROI 设置无效，跳过处理")
                return

            # 提取 ROI
            roi = cv_image[y1:y2, x1:x2]
            hsv_roi = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
            cv2.rectangle(cv_image, (x1, y1), (x2, y2), (0, 255, 255), 5)  # 黄色 ROI 框

            # 在原图副本上绘制（保持原始尺寸）
            output = cv_image.copy()

            # 可选：在 output 上画出 ROI 边框（调试用）
            # cv2.rectangle(output, (x1, y1), (x2, y2), (255, 255, 0), 2)

            for color_name in ['red', 'green', 'blue']:
                mask = None
                for lower, upper in self.color_ranges[color_name]:
                    temp_mask = cv2.inRange(hsv_roi, lower, upper)
                    if mask is None:
                        mask = temp_mask
                    else:
                        mask = cv2.bitwise_or(mask, temp_mask)

                if mask is None:
                    continue

                # 形态学去噪
                kernel = np.ones((5, 5), np.uint8)
                mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
                mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)

                # 查找轮廓
                contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

                for cnt in contours:
                    area = cv2.contourArea(cnt)
                    if area < self.min_area:
                        continue

                    # 轮廓坐标是相对于 ROI 的，需转换回原图坐标
                    cnt_global = cnt + np.array([x1, y1])  # 平移

                    # 绘制边界框（在原图坐标）
                    x, y, w, h = cv2.boundingRect(cnt_global)
                    cv2.rectangle(output, (x, y), (x + w, y + h), self.draw_colors[color_name], 2)

                    # 计算中心点
                    M = cv2.moments(cnt_global)
                    if M["m00"] != 0:
                        cx = int(M["m10"] / M["m00"])
                        cy = int(M["m01"] / M["m00"])
                        cv2.circle(output, (cx, cy), 7, self.center_color, -1)
                        cv2.putText(output, color_name, (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, self.draw_colors[color_name], 2)

            # # 显示结果（可选）
            # cv2.imshow('Color Detection', output)
            # cv2.waitKey(1)

            # 转换为 ROS Image 消息并发布
            img_msg = self.bridge.cv2_to_imgmsg(output, "bgr8")
            img_msg.header = msg.header  # 保留原始时间戳和 frame_id
            self.image_pub.publish(img_msg)

        except Exception as e:
            self.get_logger().error(f"处理图像时出错: {e}")

    def destroy_node(self):
        cv2.destroyAllWindows()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = ColorDetector()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"节点运行出错: {str(e)}")
    finally:
        try:
            rclpy.shutdown()
        except RuntimeError as e:
            error_msg = str(e)
            if "already called" in error_msg or "already shutdown" in error_msg:
                pass
            else:
                print(f"Unexpected error during shutdown: {e}", file=sys.stderr)

if __name__ == '__main__':
    main()

操作手順

192.168.8.111 にリモート接続します。次のコマンドを入力して Enter キーを押します。

ssh sunrise@192.168.8.111

パスワード sunrise を入力します。下図を参照してください：

カメラを起動し、カメラのトピックをパブリッシュします。次のコマンドを入力して Enter キーを押します。

ros2 run opencv_camera_pkg camera_node

下図を参照してください：

ローカルのターミナルを開き、色認識プログラムを起動します。次のコマンドを入力します。

ros2 launch color_detection color_detection.launch.py

下図を参照してください：

Enter キーを押すと RVIZ 画面が起動し、取得した画像と認識結果を確認できます。

実験結果

さまざまな色の物体をカメラの下に置くと、物体の色を認識できます。下図のように、blue、green、red の 3 色の物体を認識しています。

サポート窓口

資料のダウンロード・技術サポートについては、HumansX株式会社までお問い合わせください。