Initial commit

dev_ws/src/signal_board_detector/config/signal_board_detector.yaml

@@ -0,0 +1,7 @@
+signal_board_detector:
+  ros__parameters:
+    image_topic: "/image_raw"
+    busy_yellow_ratio: 0.45
+    idle_yellow_ratio_min: 0.03
+    idle_yellow_ratio_max: 0.28
+    min_contour_area: 1200.0

dev_ws/src/signal_board_detector/launch/signal_board_detector.launch.py

@@ -0,0 +1,20 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory('signal_board_detector'),
+        'config', 'signal_board_detector.yaml'
+    )
+    return LaunchDescription([
+        Node(
+            package='signal_board_detector',
+            executable='signal_board_detector',
+            name='signal_board_detector',
+            output='screen',
+            parameters=[config],
+        )
+    ])

dev_ws/src/signal_board_detector/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>signal_board_detector</name>
+  <version>0.1.0</version>
+  <description>USB camera based signal board detector.</description>
+  <maintainer email="openai@example.com">OpenAI</maintainer>
+  <license>Apache-2.0</license>
+
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>cv_bridge</depend>
+
+  <exec_depend>python3-opencv</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

dev_ws/src/signal_board_detector/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/signal_board_detector
+[install]
+install_scripts=$base/lib/signal_board_detector

dev_ws/src/signal_board_detector/setup.py

@@ -0,0 +1,26 @@
+from setuptools import setup
+
+package_name = 'signal_board_detector'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name + '/launch', ['launch/signal_board_detector.launch.py']),
+        ('share/' + package_name + '/config', ['config/signal_board_detector.yaml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='OpenAI',
+    maintainer_email='openai@example.com',
+    description='USB camera based signal board detector',
+    license='Apache-2.0',
+    entry_points={
+        'console_scripts': [
+            'signal_board_detector = signal_board_detector.signal_board_detector_node:main',
+        ],
+    },
+)

dev_ws/src/signal_board_detector/signal_board_detector/signal_board_detector_node.py

@@ -0,0 +1,95 @@
+import cv2
+import numpy as np
+import rclpy
+from cv_bridge import CvBridge
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+
+
+class SignalBoardDetector(Node):
+    def __init__(self) -> None:
+        super().__init__('signal_board_detector')
+        self.declare_parameter('image_topic', '/image_raw')
+        self.declare_parameter('busy_yellow_ratio', 0.45)
+        self.declare_parameter('idle_yellow_ratio_min', 0.03)
+        self.declare_parameter('idle_yellow_ratio_max', 0.28)
+        self.declare_parameter('min_contour_area', 1200.0)
+
+        image_topic = self.get_parameter('image_topic').value
+        self.busy_yellow_ratio = float(self.get_parameter('busy_yellow_ratio').value)
+        self.idle_yellow_ratio_min = float(self.get_parameter('idle_yellow_ratio_min').value)
+        self.idle_yellow_ratio_max = float(self.get_parameter('idle_yellow_ratio_max').value)
+        self.min_contour_area = float(self.get_parameter('min_contour_area').value)
+
+        self.bridge = CvBridge()
+        self.result_pub = self.create_publisher(String, '/signal_board/result', 10)
+        self.debug_pub = self.create_publisher(Image, '/signal_board/debug_image', 10)
+        self.create_subscription(Image, image_topic, self.on_image, 10)
+
+    def publish_result(self, result: str) -> None:
+        msg = String()
+        msg.data = result
+        self.result_pub.publish(msg)
+
+    def detect_shape(self, contour) -> str:
+        peri = cv2.arcLength(contour, True)
+        approx = cv2.approxPolyDP(contour, 0.04 * peri, True)
+        vertices = len(approx)
+        if vertices == 3:
+            return 'triangle'
+        if vertices == 4:
+            return 'square'
+        return 'star'
+
+    def on_image(self, msg: Image) -> None:
+        frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
+
+        yellow_mask = cv2.inRange(hsv, (15, 80, 80), (40, 255, 255))
+        gray_mask = cv2.inRange(hsv, (0, 0, 40), (180, 50, 220))
+
+        kernel = np.ones((5, 5), np.uint8)
+        yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_OPEN, kernel)
+        yellow_mask = cv2.morphologyEx(yellow_mask, cv2.MORPH_CLOSE, kernel)
+        gray_mask = cv2.morphologyEx(gray_mask, cv2.MORPH_OPEN, kernel)
+
+        total_pixels = frame.shape[0] * frame.shape[1]
+        yellow_ratio = float(np.count_nonzero(yellow_mask)) / float(max(total_pixels, 1))
+        gray_ratio = float(np.count_nonzero(gray_mask)) / float(max(total_pixels, 1))
+
+        result = 'unknown'
+        contours, _ = cv2.findContours(yellow_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        contours = [c for c in contours if cv2.contourArea(c) >= self.min_contour_area]
+
+        if yellow_ratio >= self.busy_yellow_ratio:
+            result = 'busy'
+        elif self.idle_yellow_ratio_min <= yellow_ratio <= self.idle_yellow_ratio_max and gray_ratio > 0.15 and contours:
+            contour = max(contours, key=cv2.contourArea)
+            shape = self.detect_shape(contour)
+            result = f'idle_{shape}'
+            x, y, w, h = cv2.boundingRect(contour)
+            cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
+            cv2.putText(frame, result, (x, max(30, y - 10)), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
+        else:
+            cv2.putText(frame, result, (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 2)
+
+        cv2.putText(frame, f'yellow={yellow_ratio:.3f}', (20, 65), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 0, 0), 2)
+        self.publish_result(result)
+        debug_msg = self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
+        debug_msg.header = msg.header
+        self.debug_pub.publish(debug_msg)
+
+
+def main() -> None:
+    rclpy.init()
+    node = SignalBoardDetector()
+    try:
+        rclpy.spin(node)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()