ArUco 마커 위치 인식하기

1. 강의 목표

이번 강의에서는 Ubuntu 22.04 Server가 설치된 TurtleBot3 Burger에서 ROS 2 Humble 환경을 사용하여 Pi Camera 2 영상을 받아오고, OpenCV의 ArUco 마커 인식 기능을 이용해 로봇 기준의 마커 위치를 추정하는 방법을 다룹니다.

2. 필요한 패키지 설치

TurtleBot3 Burger에서 다음 패키지를 설치합니다.

sudo apt update
sudo apt install -y \
  ros-humble-cv-bridge \
  ros-humble-image-transport \
  python3-opencv \
  python3-numpy

cv_bridge는 ROS 2의 sensor_msgs/msg/Image 메시지와 OpenCV 이미지 표현을 서로 변환하는 패키지입니다. 즉, ROS 2 카메라 토픽을 OpenCV에서 처리하려면 사실상 필수입니다.

image_transport는 ROS 이미지 토픽을 효율적으로 송수신하기 위한 계층이며, 압축 이미지 전송 같은 기능과 함께 자주 사용됩니다.

설치 후 OpenCV에서 ArUco 모듈이 사용 가능한지 확인합니다.

python3 - << 'EOF'
import cv2
print(cv2.__version__)
print(hasattr(cv2, "aruco"))
EOF

출력 결과에서 True가 나와야 합니다.

4.x.x
True

만약 False가 나온다면 다음 패키지가 필요할 수 있습니다.

sudo apt install -y python3-opencv

또는 pip 환경을 따로 쓰는 경우에는 다음이 필요할 수 있습니다.

pip3 install opencv-contrib-python

단, TurtleBot3 같은 SBC 환경에서는 pip로 OpenCV를 무리하게 설치하면 시스템 OpenCV와 충돌할 수 있습니다. 강의 환경에서는 가능하면 Ubuntu apt 패키지를 우선 사용하는 것이 안정적입니다.

7. ArUco 마커 준비

OpenCV에서 사용할 마커 딕셔너리를 먼저 정해야 합니다.

이번 강의에서는 다음 딕셔너리를 사용합니다.

DICT_4X4_50

의미는 다음과 같습니다.

1. 4X4마커 내부 코드가 4×4 비트 구조라는 뜻입니다.
2. 50사용할 수 있는 마커 ID가 0번부터 49번까지 있다는 뜻입니다.

초급 강의에서는 DICT_4X4_50이 적당합니다. 패턴이 단순해서 인식이 빠르고, 실습용 마커 개수도 충분합니다.

마커 이미지는 Python으로 직접 생성할 수 있습니다.

mkdir -p ~/aruco_markers
cd ~/aruco_markers

다음 파일을 생성합니다.

nano generate_aruco_marker.py

내용은 다음과 같습니다.

import cv2
import numpy as np

marker_id = 0
marker_size_px = 600
output_file = "aruco_4x4_id0.png"

aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)

marker_image = np.zeros((marker_size_px, marker_size_px), dtype=np.uint8)
marker_image = cv2.aruco.generateImageMarker(
    aruco_dict,
    marker_id,
    marker_size_px,
    marker_image,
    1
)

cv2.imwrite(output_file, marker_image)
print(f"Saved: {output_file}")

실행합니다.

python3 generate_aruco_marker.py

출력할 때 주의할 점은 다음과 같습니다.

1. 마커는 정사각형으로 출력합니다.
2. 종이가 휘지 않게 평평하게 붙입니다.
3. 검은색과 흰색 대비가 확실해야 합니다.
4. 주변에 흰색 여백이 어느 정도 있어야 합니다.
5. 실제 출력된 마커 한 변의 길이를 자로 측정합니다.

예를 들어 출력한 마커의 실제 크기가 10cm라면 나중에 코드에서 다음 값으로 사용합니다.

marker_size = 0.10

단위는 meter입니다.

8. ROS 2 패키지 생성

이제 ArUco 인식용 ROS 2 Python 패키지를 생성합니다.

cd ~/turtlebot3_ws/src
ros2 pkg create aruco_localization \
  --build-type ament_python \
  --dependencies rclpy sensor_msgs std_msgs cv_bridge

생성된 구조는 다음과 같습니다.

aruco_localization/
├── aruco_localization/
│   ├── __init__.py
├── package.xml
├── setup.py
├── setup.cfg
└── resource/
    └── aruco_localization

노드 파일을 생성합니다.

cd ~/turtlebot3_ws/src/aruco_localization/aruco_localization
touch aruco_detector_node.py

9. ArUco 인식 노드 전체 소스

다음 코드를 입력합니다.

import math
import json

import cv2
import numpy as np

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import Image, CompressedImage
from std_msgs.msg import String
from cv_bridge import CvBridge


class ArucoDetectorNode(Node):
    def __init__(self):
        super().__init__('aruco_detector_node')

        self.declare_parameter('image_topic', '/camera/image_raw/compressed')
        # self.declare_parameter('image_topic', '/camera/image_raw')
        self.declare_parameter('marker_size', 0.10)
        self.declare_parameter('dictionary', 'DICT_4X4_50')
        self.declare_parameter('publish_debug_image', True)

        self.image_topic = self.get_parameter('image_topic').value
        self.marker_size = float(self.get_parameter('marker_size').value)
        self.dictionary_name = self.get_parameter('dictionary').value
        self.publish_debug_image = bool(
            self.get_parameter('publish_debug_image').value
        )

        self.bridge = CvBridge()

        self.marker_pub = self.create_publisher(
            String,
            '/aruco/markers',
            10
        )

        self.debug_image_pub = self.create_publisher(
            Image,
            '/aruco/debug_image',
            10
        )

        self.image_sub = self.create_subscription(
            CompressedImage,
            # Image,
            self.image_topic,
            self.image_callback,
            10
        )

        self.aruco_dict = self.get_aruco_dictionary(self.dictionary_name)

        if hasattr(cv2.aruco, "DetectorParameters"):
            self.aruco_params = cv2.aruco.DetectorParameters()
        else:
            self.aruco_params = cv2.aruco.DetectorParameters_create()

        if hasattr(cv2.aruco, "ArucoDetector"):
            self.aruco_detector = cv2.aruco.ArucoDetector(
                self.aruco_dict,
                self.aruco_params
            )
        else:
            self.aruco_detector = None

        self.camera_matrix = None
        self.dist_coeffs = None

        self.get_logger().info('Aruco detector node started')
        self.get_logger().info(f'Subscribed image topic: {self.image_topic}')
        self.get_logger().info(f'Marker size: {self.marker_size} m')
        self.get_logger().info(f'Aruco dictionary: {self.dictionary_name}')

    def get_aruco_dictionary(self, dictionary_name):
        dictionary_map = {
            'DICT_4X4_50': cv2.aruco.DICT_4X4_50,
            'DICT_4X4_100': cv2.aruco.DICT_4X4_100,
            'DICT_5X5_50': cv2.aruco.DICT_5X5_50,
            'DICT_5X5_100': cv2.aruco.DICT_5X5_100,
            'DICT_6X6_50': cv2.aruco.DICT_6X6_50,
            'DICT_6X6_100': cv2.aruco.DICT_6X6_100,
        }

        if dictionary_name not in dictionary_map:
            self.get_logger().warn(
                f'Unknown dictionary {dictionary_name}, use DICT_4X4_50'
            )
            dictionary_name = 'DICT_4X4_50'

        return cv2.aruco.getPredefinedDictionary(dictionary_map[dictionary_name])

    def image_callback(self, msg):
        try:
            frame = self.bridge.compressed_imgmsg_to_cv2(
            # frame = self.bridge.imgmsg_to_cv2(
                msg,
                desired_encoding='bgr8'
            )
        except Exception as e:
            self.get_logger().error(f'cv_bridge error: {e}')
            return

        frame = cv2.flip(frame, -1)

        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

        if self.aruco_detector is not None:
            corners, ids, rejected = self.aruco_detector.detectMarkers(gray)
        else:
            corners, ids, rejected = cv2.aruco.detectMarkers(
                gray ,
                self.aruco_dict,
                parameters=self.aruco_params
            )

        result_list = []

        if ids is not None:
            cv2.aruco.drawDetectedMarkers(frame, corners, ids)

            for i, marker_id in enumerate(ids.flatten()):
                marker_corners = corners[i][0]

                center_x = float(np.mean(marker_corners[:, 0]))
                center_y = float(np.mean(marker_corners[:, 1]))

                pixel_width_1 = np.linalg.norm(
                    marker_corners[0] - marker_corners[1]
                )
                pixel_width_2 = np.linalg.norm(
                    marker_corners[2] - marker_corners[3]
                )
                pixel_width = float((pixel_width_1 + pixel_width_2) / 2.0)

                image_height, image_width = gray.shape

                normalized_x = (center_x - image_width / 2.0) / (image_width / 2.0)
                normalized_y = (center_y - image_height / 2.0) / (image_height / 2.0)

                marker_info = {
                    'id': int(marker_id),
                    'center_pixel': {
                        'x': round(center_x, 2),
                        'y': round(center_y, 2)
                    },
                    'normalized_position': {
                        'x': round(normalized_x, 4),
                        'y': round(normalized_y, 4)
                    },
                    'pixel_width': round(pixel_width, 2)
                }

                result_list.append(marker_info)

                cv2.circle(
                    frame,
                    (int(center_x), int(center_y)),
                    5,
                    (0, 0, 255),
                    -1
                )

                cv2.putText(
                    frame,
                    f'ID:{marker_id}',
                    (int(center_x) - 30, int(center_y) - 20),
                    cv2.FONT_HERSHEY_SIMPLEX,
                    0.6,
                    (0, 255, 0),
                    2
                )

        output_msg = String()
        output_msg.data = json.dumps(result_list)
        self.marker_pub.publish(output_msg)

        if self.publish_debug_image:
            try:
                debug_msg = self.bridge.cv2_to_imgmsg(
                    frame,
                    encoding='bgr8'
                )
                debug_msg.header = msg.header
                self.debug_image_pub.publish(debug_msg)
            except Exception as e:
                self.get_logger().error(f'Debug image publish error: {e}')


def main(args=None):
    rclpy.init(args=args)
    node = ArucoDetectorNode()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

10. 소스 코드 자세한 설명

1) 기본 import

import math
import json

import cv2
import numpy as np

cv2는 OpenCV입니다. ArUco 마커 검출, 영상 변환, 디버깅 화면 그리기에 사용합니다.

numpy는 마커 코너 좌표 계산에 사용합니다.

json은 인식 결과를 문자열로 발행하기 위해 사용합니다. 강의 초반에는 커스텀 메시지를 만들기보다 JSON 문자열을 쓰는 편이 이해하기 쉽습니다.

math는 현재 코드에서는 필수는 아닙니다. 이후 각도 계산이나 거리 계산을 추가할 때 사용할 수 있습니다.

2) ROS 2 관련 import

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import Image
from std_msgs.msg import String
from cv_bridge import CvBridge

rclpy는 ROS 2 Python 클라이언트 라이브러리입니다.

Node는 ROS 2 노드 클래스를 만들기 위한 기본 클래스입니다.

Image는 카메라 토픽 메시지 타입입니다.

String은 마커 인식 결과를 문자열로 발행하기 위해 사용합니다.

CvBridge는 ROS 2 이미지 메시지와 OpenCV 이미지를 변환합니다. ROS 2 영상 처리 노드에서는 핵심 역할을 합니다.

3) 노드 클래스 정의

class ArucoDetectorNode(Node):
    def __init__(self):
        super().__init__('aruco_detector_node')

ArucoDetectorNode는 직접 만든 ROS 2 노드 클래스입니다.

super().__init__('aruco_detector_node')는 노드 이름을 aruco_detector_node로 설정합니다.

실행 후 다음 명령으로 노드 이름을 확인할 수 있습니다.

ros2 node list

예상 출력은 다음과 같습니다.

/aruco_detector_node

4) 파라미터 선언

self.declare_parameter('image_topic', '/camera/image_raw')
self.declare_parameter('marker_size', 0.10)
self.declare_parameter('dictionary', 'DICT_4X4_50')
self.declare_parameter('publish_debug_image', True)

ROS 2 파라미터를 사용하면 코드를 수정하지 않고 실행 옵션만 바꿀 수 있습니다.

각 파라미터의 의미는 다음과 같습니다.

1. image_topic구독할 카메라 토픽 이름입니다. 기본값은 /camera/image_raw입니다.
2. marker_size실제 출력된 ArUco 마커 한 변의 길이입니다. 단위는 meter입니다. 기본값 0.10은 10cm를 의미합니다.
3. dictionary사용할 ArUco 딕셔너리입니다. 기본값은 DICT_4X4_50입니다.
4. publish_debug_image디버깅 영상을 발행할지 결정합니다. 기본값은 True입니다.

실행할 때 파라미터를 바꾸려면 다음처럼 입력합니다.

ros2 run aruco_localization aruco_detector_node \
  --ros-args \
  -p image_topic:=/camera/image_raw \
  -p marker_size:=0.10 \
  -p dictionary:=DICT_4X4_50

5) Publisher 생성

self.marker_pub = self.create_publisher(
    String,
    '/aruco/markers',
    10
)

이 Publisher는 인식된 마커 정보를 /aruco/markers 토픽으로 발행합니다.

메시지 타입은 std_msgs/msg/String입니다.

발행되는 데이터 예시는 다음과 같습니다.

[
  {
    "id": 0,
    "center_pixel": {
      "x": 312.5,
      "y": 241.3
    },
    "normalized_position": {
      "x": -0.0234,
      "y": 0.0054
    },
    "pixel_width": 128.7
  }
]

6) 디버깅 이미지 Publisher 생성

self.debug_image_pub = self.create_publisher(
    Image,
    '/aruco/debug_image',
    10
)

이 Publisher는 ArUco 마커 테두리와 ID가 그려진 영상을 발행합니다.

원격 PC에서 다음 명령으로 확인할 수 있습니다.

rqt_image_view

토픽은 다음을 선택합니다.

/aruco/debug_image

7) Image Subscriber 생성

self.image_sub = self.create_subscription(
    Image,
    self.image_topic,
    self.image_callback,
    10
)

이 Subscriber는 /camera/image_raw 토픽을 구독합니다.

새 이미지가 들어올 때마다 image_callback() 함수가 자동으로 호출됩니다.

여기서 중요한 구조는 다음과 같습니다.

카메라 이미지 수신
   ↓
image_callback() 실행
   ↓
OpenCV 이미지로 변환
   ↓
ArUco 검출
   ↓
결과 토픽 발행

8) ArUco 딕셔너리 설정

self.aruco_dict = self.get_aruco_dictionary(self.dictionary_name)

OpenCV는 여러 종류의 ArUco 딕셔너리를 제공합니다.

강의에서는 DICT_4X4_50을 사용하지만, 필요하면 DICT_5X5_100, DICT_6X6_100 등으로 바꿀 수 있습니다.

return cv2.aruco.getPredefinedDictionary(dictionary_map[dictionary_name])

이 코드는 문자열로 받은 딕셔너리 이름을 OpenCV 내부 딕셔너리 객체로 변환합니다.

9) ROS Image를 OpenCV Image로 변환

frame = self.bridge.imgmsg_to_cv2(
    msg,
    desired_encoding='bgr8'
)

ROS 2의 /camera/image_raw 메시지는 OpenCV가 바로 처리할 수 있는 형식이 아닙니다.

그래서 cv_bridge를 이용해 ROS 이미지 메시지를 OpenCV 이미지로 변환합니다.

bgr8은 OpenCV에서 일반적으로 사용하는 컬러 이미지 형식입니다.

10) 흑백 영상 변환

gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

ArUco 마커는 흑백 패턴이기 때문에 컬러 정보가 꼭 필요하지 않습니다.

흑백 영상으로 변환하면 계산량이 줄고 검출이 단순해집니다.

11) ArUco 마커 검출

corners, ids, rejected = cv2.aruco.detectMarkers(
    gray,
    self.aruco_dict,
    parameters=self.detector_params
)

이 부분이 핵심입니다.

반환값은 다음과 같습니다.

1. corners검출된 각 마커의 네 꼭짓점 좌표입니다.
2. ids검출된 마커 ID입니다.
3. rejected마커 후보로 보였지만 최종적으로 거절된 영역입니다.

마커가 검출되지 않으면 ids는 None입니다.

그래서 다음 조건문이 필요합니다.

if ids is not None:

12) 검출된 마커 표시

cv2.aruco.drawDetectedMarkers(frame, corners, ids)

이 코드는 영상 위에 검출된 마커의 테두리와 ID를 그려줍니다.

강의할 때 이 디버깅 영상이 매우 중요합니다. 수강생이 “마커를 인식하고 있다”는 것을 바로 눈으로 확인할 수 있기 때문입니다.

13) 마커 중심 좌표 계산

marker_corners = corners[i][0]

center_x = float(np.mean(marker_corners[:, 0]))
center_y = float(np.mean(marker_corners[:, 1]))

ArUco 마커는 네 개의 꼭짓점 좌표를 가집니다.

예를 들어 다음과 같은 구조입니다.

corner 0: left-top
corner 1: right-top
corner 2: right-bottom
corner 3: left-bottom

중심 좌표는 네 꼭짓점의 평균으로 구합니다.

center_x = 네 꼭짓점 x 좌표 평균
center_y = 네 꼭짓점 y 좌표 평균

이 값은 픽셀 좌표입니다.

예를 들어 영상 해상도가 640×480이면 중심 근처 좌표는 대략 다음과 같습니다.

x = 320
y = 240

14) 마커 픽셀 폭 계산

pixel_width_1 = np.linalg.norm(
    marker_corners[0] - marker_corners[1]
)
pixel_width_2 = np.linalg.norm(
    marker_corners[2] - marker_corners[3]
)
pixel_width = float((pixel_width_1 + pixel_width_2) / 2.0)

마커의 위쪽 변 길이와 아래쪽 변 길이를 픽셀 단위로 계산한 뒤 평균을 냅니다.

카메라에 가까우면 마커가 크게 보이므로 pixel_width가 커집니다.

카메라에서 멀어지면 마커가 작게 보이므로 pixel_width가 작아집니다.

즉, pixel_width는 거리 추정의 기초 데이터가 됩니다.

15) 정규화된 위치 계산

image_height, image_width = gray.shape

normalized_x = (center_x - image_width / 2.0) / (image_width / 2.0)
normalized_y = (center_y - image_height / 2.0) / (image_height / 2.0)

픽셀 좌표만 보면 해상도에 따라 값이 달라집니다.

그래서 중심 좌표를 -1.0부터 1.0 사이 값으로 정규화합니다.

예를 들어 640×480 영상에서 마커가 화면 중앙에 있으면 다음과 비슷합니다.

normalized_x = 0.0
normalized_y = 0.0

마커가 화면 왼쪽에 있으면 다음과 같습니다.

normalized_x < 0

마커가 화면 오른쪽에 있으면 다음과 같습니다.

normalized_x > 0

이 값은 로봇 제어에 바로 사용할 수 있습니다.

예를 들어 마커를 화면 중앙에 오도록 로봇을 회전시키는 제어는 다음처럼 생각할 수 있습니다.

normalized_x > 0  →  로봇을 오른쪽으로 회전
normalized_x < 0  →  로봇을 왼쪽으로 회전
normalized_x ≈ 0  →  정렬 완료

16) 결과를 JSON으로 구성

marker_info = {
    'id': int(marker_id),
    'center_pixel': {
        'x': round(center_x, 2),
        'y': round(center_y, 2)
    },
    'normalized_position': {
        'x': round(normalized_x, 4),
        'y': round(normalized_y, 4)
    },
    'pixel_width': round(pixel_width, 2)
}

각 마커에 대한 정보를 Python dictionary로 만듭니다.

이 구조는 수업에서 설명하기 쉽습니다.

id                  → 마커 번호
center_pixel         → 영상 안에서의 중심 픽셀 좌표
normalized_position  → 화면 중심 기준 상대 위치
pixel_width          → 영상에서 보이는 마커 크기

이후 다음 코드로 JSON 문자열로 변환합니다.

output_msg = String()
output_msg.data = json.dumps(result_list)
self.marker_pub.publish(output_msg)

결과는 /aruco/markers 토픽으로 발행됩니다.

확인은 다음 명령으로 합니다.

ros2 topic echo /aruco/markers

17) 디버깅 영상 발행

debug_msg = self.bridge.cv2_to_imgmsg(
    frame,
    encoding='bgr8'
)
debug_msg.header = msg.header
self.debug_image_pub.publish(debug_msg)

OpenCV에서 처리한 이미지를 다시 ROS 2 Image 메시지로 변환합니다.

그리고 /aruco/debug_image 토픽으로 발행합니다.

원격 PC에서 이 토픽을 보면 마커 인식 결과를 바로 확인할 수 있습니다.

11. setup.py 수정

노드를 ros2 run으로 실행하려면 setup.py에 entry point를 추가해야 합니다.

cd ~/turtlebot3_ws/src/aruco_localization
nano setup.py

내용을 다음처럼 수정합니다.

from setuptools import setup

package_name = 'aruco_localization'

setup(
    name=package_name,
    version='0.0.1',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='ubuntu',
    maintainer_email='ubuntu@example.com',
    description='ArUco marker localization package for TurtleBot3 Burger',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'aruco_detector_node = aruco_localization.aruco_detector_node:main',
        ],
    },
)

중요한 부분은 다음입니다.

entry_points={
    'console_scripts': [
        'aruco_detector_node = aruco_localization.aruco_detector_node:main',
    ],
},

12. package.xml 확인

package.xml에는 의존성이 들어 있어야 합니다.

nano package.xml

다음 항목이 있는지 확인합니다.

<?xml version="1.0"?>
<package format="3">
  <name>aruco_localization</name>
  <version>0.0.1</version>
  <description>ArUco marker localization package for TurtleBot3 Burger</description>
  <maintainer email="ubuntu@example.com">ubuntu</maintainer>
  <license>Apache-2.0</license>

  <depend>rclpy</depend>
  <depend>sensor_msgs</depend>
  <depend>std_msgs</depend>
  <depend>cv_bridge</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

여기서 중요한 부분은 다음입니다.

<depend>cv_bridge</depend>

13. 빌드

워크스페이스 루트로 이동합니다.

cd ~/turtlebot3_ws

패키지를 빌드합니다.

colcon build --packages-select aruco_localization

환경을 다시 설정합니다.

source install/setup.bash

빌드 후 노드가 인식되는지 확인합니다.

ros2 pkg executables aruco_localization

예상 출력은 다음과 같습니다.

aruco_localization aruco_detector_node

14. 실행 순서

실습에서는 터미널을 최소 3개 사용하면 좋습니다.

1) 터미널 1: 카메라 실행

TurtleBot3에 SSH 접속 후 실행합니다.

source /opt/ros/humble/setup.bash
source ~/turtlebot3_ws/install/setup.bash

ros2 launch turtlebot3_bringup camera.launch.py

2) 터미널 2: ArUco 인식 노드 실행

source /opt/ros/humble/setup.bash
source ~/turtlebot3_ws/install/setup.bash

ros2 run aruco_localization aruco_detector_node

파라미터를 명시해서 실행하려면 다음처럼 합니다.

ros2 run aruco_localization aruco_detector_node \
  --ros-args \
  -p image_topic:=/camera/image_raw \
  -p marker_size:=0.10 \
  -p dictionary:=DICT_4X4_50 \
  -p publish_debug_image:=true

실행 시 numpy 관련 에러가 발생할 경우 아래의 명령을 실행하여 낮은 버전의 numpy를 설치하시 바랍니다.

python3 -m pip install "numpy==1.26.4" --force-reinstall

그리고 [ERROR]: Debug image publish error: 16 에러가 발생할 경우 pip OpenCV가 ROS의 cv_bridge와 충돌 중일 가능성이 큽니다.

아래의 명령들을 실행하여 충돌 원인을 제거하시기 바랍니다.

ROS 2 Humble에서는 보통 이쪽이 더 안정적입니다.

python3 -m pip uninstall -y opencv-python opencv-contrib-python opencv-python-headless opencv-contrib-python-headless
sudo apt update
sudo apt install --reinstall python3-opencv ros-humble-cv-bridge

3) 터미널 3: 결과 확인

ros2 topic echo /aruco/markers

마커가 보이면 다음과 비슷한 출력이 나옵니다.

data: '[{"id": 0, "center_pixel": {"x": 315.2, "y": 238.7}, "normalized_position": {"x": -0.015, "y": -0.0054}, "pixel_width": 142.6}]'

4) 원격 PC: 디버깅 영상 확인

원격 PC에서 실행합니다.

rqt_image_view

토픽을 다음으로 선택합니다.

/aruco/debug_image

정상이라면 마커 외곽선과 ID가 표시됩니다.

15. 위치 인식의 기본 개념

현재 소스는 마커의 정확한 3D 위치까지는 계산하지 않고, 화면 기준 상대 위치를 계산합니다.

즉, 다음 정보를 얻습니다.

마커가 화면 중앙보다 왼쪽인지 오른쪽인지
마커가 화면 중앙보다 위쪽인지 아래쪽인지
마커가 화면에서 얼마나 크게 보이는지

이것만으로도 기초 로봇 제어가 가능합니다.

예를 들어 도킹 제어를 한다면 다음과 같이 구성할 수 있습니다.

1. 마커가 화면 오른쪽에 있다.로봇을 오른쪽으로 회전합니다.
2. 마커가 화면 왼쪽에 있다.로봇을 왼쪽으로 회전합니다.
3. 마커가 화면 중앙에 있다.로봇을 앞으로 이동합니다.
4. 마커가 충분히 크게 보인다.목표 지점에 가까워졌다고 판단하고 정지합니다.

이 방식은 정확한 카메라 캘리브레이션 없이도 설명할 수 있어서 강의 초반에 적합합니다.

16. 카메라 캘리브레이션을 이용한 3D 위치 추정

더 정확한 위치 인식을 하려면 카메라 내부 파라미터가 필요합니다.

필요한 값은 다음과 같습니다.

1. camera_matrix카메라 초점거리와 중심점 정보입니다.
2. dist_coeffs렌즈 왜곡 계수입니다.
3. marker_size실제 마커 한 변 길이입니다.

이 값이 있으면 OpenCV의 pose estimation 기능을 이용해 카메라 기준 마커의 3D 위치를 구할 수 있습니다.

개념적으로는 다음과 같습니다.

2D 영상 속 마커 코너 좌표
+ 실제 마커 크기
+ 카메라 내부 파라미터
= 카메라 기준 마커의 3D 위치와 자세

수업에서는 다음 단계로 확장할 수 있습니다.

rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
    corners,
    self.marker_size,
    self.camera_matrix,
    self.dist_coeffs
)

여기서 tvecs는 카메라 기준 마커의 위치입니다.

일반적으로 다음처럼 해석합니다.

tvec[0] = x 방향 위치
tvec[1] = y 방향 위치
tvec[2] = z 방향 위치

단위는 marker_size와 같은 단위입니다. marker_size를 meter로 넣으면 tvec도 meter 단위로 나옵니다.

주의할 점은 카메라 좌표계와 로봇 좌표계가 다르다는 것입니다.

일반적인 카메라 좌표계는 다음과 비슷하게 해석합니다.

x: 카메라 오른쪽
y: 카메라 아래쪽
z: 카메라 앞쪽

로봇의 일반적인 base_link 좌표계는 다음과 같습니다.

x: 로봇 전방
y: 로봇 왼쪽
z: 로봇 위쪽

그래서 실제 주행 제어에 사용하려면 카메라 좌표계를 로봇 좌표계로 변환해야 합니다. 이때 ROS 2의 TF 개념이 필요합니다.

17. 3D 위치 추정 버전 코드 예시

카메라 캘리브레이션 값을 알고 있다고 가정하면 다음처럼 코드를 확장할 수 있습니다.

아래 값은 예시입니다. 실제 카메라에서는 반드시 캘리브레이션으로 얻은 값을 사용해야 합니다.

self.camera_matrix = np.array([
    [508.65932,   0.     , 320.18936,
     0.     , 509.8392 , 238.81706,
     0.     ,   0.     ,   1.     ]
], dtype=np.float32)

self.dist_coeffs = np.array([
    [0.185728, -0.296759, -0.004200, 0.000739, 0.000000]
], dtype=np.float32)

마커 검출 후 다음 코드를 추가할 수 있습니다.

        self.camera_matrix = None
        self.dist_coeffs = None

        self.camera_matrix = np.array([
            [508.65932,   0.     , 320.18936,
            0.     , 509.8392 , 238.81706,
            0.     ,   0.     ,   1.     ]
        ], dtype=np.float32)

        self.dist_coeffs = np.array([
            [0.185728, -0.296759, -0.004200, 0.000739, 0.000000]
        ], dtype=np.float32)




                fixed_corners = []
                for c in corners:
                    c = np.asarray(c, dtype=np.float32)
                    c = c.reshape((1, 4, 2))
                    fixed_corners.append(c)

                camera_matrix = np.asarray(self.camera_matrix, dtype=np.float64)
                camera_matrix = camera_matrix.reshape((3, 3))

                dist_coeffs = np.asarray(self.dist_coeffs, dtype=np.float64)
                dist_coeffs = dist_coeffs.reshape((-1, 1))

                marker_size = float(self.marker_size)

                rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
                    corners,
                    marker_size,
                    camera_matrix,
                    dist_coeffs
                )

                cv2.aruco.drawDetectedMarkers(frame, fixed_corners, ids)

                for i in range(len(ids)):
                    rvec = np.asarray(rvecs[i], dtype=np.float64).reshape((3, 1))
                    tvec = np.asarray(tvecs[i], dtype=np.float64).reshape((3, 1))

                    cv2.drawFrameAxes(
                        frame,
                        camera_matrix,
                        dist_coeffs,
                        rvec,
                        tvec,
                        marker_size * 0.5
                    )

이렇게 하면 디버깅 영상에 마커 좌표축이 표시됩니다.

distance는 카메라와 마커 사이의 3차원 거리입니다.

distance = math.sqrt(x * x + y * y + z * z)

하지만 실제 로봇 제어에서는 단순 거리보다 z 값을 더 많이 씁니다. 카메라 전방 방향 거리를 의미하기 때문입니다.

18. launch 파일 만들기

매번 긴 명령을 입력하지 않도록 launch 파일을 만들 수 있습니다.

cd ~/turtlebot3_ws/src/aruco_localization
mkdir -p launch
touch launch/aruco_detector.launch.py

내용은 다음과 같습니다.

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    aruco_node = Node(
        package='aruco_localization',
        executable='aruco_detector_node',
        name='aruco_detector_node',
        output='screen',
        parameters=[
            {
                'image_topic': '/camera/image_raw',
                'marker_size': 0.10,
                'dictionary': 'DICT_4X4_50',
                'publish_debug_image': True,
            }
        ]
    )

    return LaunchDescription([
        aruco_node
    ])

setup.py에 launch 파일 설치 설정을 추가합니다.

from setuptools import setup
import os
from glob import glob

package_name = 'aruco_localization'

setup(
    name=package_name,
    version='0.0.1',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='ubuntu',
    maintainer_email='ubuntu@example.com',
    description='ArUco marker localization package for TurtleBot3 Burger',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'aruco_detector_node = aruco_localization.aruco_detector_node:main',
        ],
    },
)

다시 빌드합니다.

cd ~/turtlebot3_ws
colcon build --packages-select aruco_localization
source install/setup.bash

이제 다음 명령으로 실행할 수 있습니다.

ros2 launch aruco_localization aruco_detector.launch.py