ROS 2 Humble + TurtleBot3 Burger + Pi Camera 2 색상 물체 추종 로봇 만들기

1. 강의 목표

이번 강의의 목표는 다음과 같습니다.

1. Pi Camera 2에서 ROS 2 이미지 토픽을 수신합니다.
2. OpenCV를 사용하여 특정 색상 영역을 검출합니다.
3. 검출된 물체의 중심 좌표를 계산합니다.
5. 물체가 화면 중앙에 오도록 TurtleBot3를 회전시킵니다.
6. 물체 크기에 따라 전진 또는 정지하도록 제어합니다.
7. ROS 2 패키지 구조, 노드 소스, 빌드 방법, 실행 방법을 이해합니다.

2. 전체 시스템 구성

이번 실습 환경은 다음과 같습니다.

1. 로봇 플랫폼: TurtleBot3 Burger
2. SBC: Raspberry Pi 계열 보드
3. OS: Ubuntu 22.04 Server
4. ROS 2: Humble Hawksbill
5. 카메라: Raspberry Pi Camera Module 2
6. 원격 제어: Remote PC에서 SSH 접속
7. 영상 확인: Remote PC의 rqt_image_view 또는 rqt
8. 주요 토픽
   1. /camera/image_raw
   2. /cmd_vel
   3. /color_follower/debug_image
   4. /color_follower/mask

ROS 2에서 카메라 영상은 일반적으로 sensor_msgs/msg/Image 타입으로 발행됩니다. 이 메시지를 OpenCV 이미지로 변환하기 위해 cv_bridge를 사용합니다.

cv_bridge는 ROS 이미지 메시지와 OpenCV 이미지 사이의 변환을 담당하는 패키지입니다.

3. 동작 원리

색상 물체 추종 로봇의 핵심 원리는 비교적 단순합니다.

1. 카메라 영상을 구독합니다.
2. BGR 이미지를 HSV 색공간으로 변환합니다.
3. 추종하고 싶은 색상 범위만 마스크로 추출합니다.
4. 노이즈를 제거합니다.
5. 가장 큰 색상 영역을 찾습니다.
6. 해당 영역의 중심점을 계산합니다.
7. 중심점이 화면 왼쪽에 있으면 로봇을 왼쪽으로 회전시킵니다.
8. 중심점이 화면 오른쪽에 있으면 로봇을 오른쪽으로 회전시킵니다.
9. 물체가 너무 멀면 전진합니다.
10. 물체가 충분히 가까우면 정지합니다.

여기서 HSV 색공간을 사용하는 이유는 RGB 또는 BGR보다 색상 분리가 쉽기 때문입니다.

BGR에서는 밝기 변화에 따라 B, G, R 값이 동시에 변합니다. 반면 HSV는 색상 Hue, 채도 Saturation, 명도 Value로 나누어 표현하므로 “빨간색”, “파란색”, “초록색”과 같은 색상을 검출할 때 더 유리합니다.

4. 진행 전 확인 사항

먼저 TurtleBot3 기본 bringup이 정상적으로 동작하는지 확인하겠습니다.

로봇 SBC에 SSH로 접속합니다.

ssh sjyong@192.168.200.28

TurtleBot3 모델을 설정합니다.

export TURTLEBOT3_MODEL=burger"

TurtleBot3 bringup을 실행합니다.

ros2 launch turtlebot3_bringup robot.launch.py

다른 터미널에서 카메라 launch를 실행합니다.

ros2 launch turtlebot3_bringup camera.launch.py  format:=BGR888

카메라 토픽이 정상적으로 나오는지 확인합니다.

ros2 topic list

다음 토픽이 보이면 카메라 토픽이 정상적으로 발행되고 있는 것입니다.

/camera/image_raw

토픽 타입도 확인합니다.

ros2 topic info /camera/image_raw

예상 결과는 다음과 비슷합니다.

Type: sensor_msgs/msg/Image
Publisher count: 1
Subscription count: 0

원격 PC에서 영상을 확인하려면 다음 명령을 실행합니다.

rqt_image_view

또는 다음 명령으로 실행하셔도 됩니다.

ros2 run rqt_image_view rqt_image_view

rqt_image_view에서 /camera/image_raw를 선택했을 때 영상이 보이면 준비가 완료된 것입니다.

5. ROS 2 네트워크 설정 확인

로봇과 원격 PC가 서로 ROS 2 토픽을 주고받으려면 다음 조건이 맞아야 합니다.

1. 같은 네트워크에 연결되어 있어야 합니다.
2. 같은 ROS_DOMAIN_ID를 사용해야 합니다.
3. 방화벽이 DDS 통신을 막고 있지 않아야 합니다.
4. 로봇과 원격 PC 모두 ROS 2 환경 설정이 되어 있어야 합니다.

로봇과 원격 PC 양쪽에 다음 설정을 적용합니다.

export ROS_DOMAIN_ID=200
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp

ROS_DOMAIN_ID 값은 반드시 30일 필요는 없습니다. 다만 로봇과 원격 PC가 같은 값을 사용해야 합니다.

환경 설정을 확인합니다.

echo $ROS_DOMAIN_ID
echo $RMW_IMPLEMENTATION

6. 필요한 패키지 설치

로봇 SBC에서 다음 패키지를 설치합니다.

sudo apt update
sudo apt install -y \
  python3-opencv \
  ros-humble-cv-bridge \
  ros-humble-vision-opencv \
  ros-humble-image-transport \
  ros-humble-rqt-image-view

7. 패키지 생성

ROS 2 Python 패키지를 생성합니다.

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

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

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

여기에 색상 추종 노드를 추가합니다.

cd ~/turtlebot3_ws/src/color_follower/color_follower
touch color_follower_node.py

8. 전체 소스 코드

다음 코드를 color_follower/color_follower/color_follower_node.py에 작성합니다.

import cv2
import numpy as np

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

from cv_bridge import CvBridge


class ColorFollowerNode(Node):
    def __init__(self):
        super().__init__('color_follower_node')

        self.declare_parameter('image_topic', '/camera/image_raw')
        self.declare_parameter('cmd_vel_topic', '/cmd_vel')

        self.declare_parameter('target_color', 'red')

        self.declare_parameter('linear_speed', 0.08)
        self.declare_parameter('angular_gain', 0.004)

        self.declare_parameter('min_area', 800)
        self.declare_parameter('stop_area', 18000)

        self.declare_parameter('max_angular_speed', 0.8)
        self.declare_parameter('image_center_tolerance', 40)

        image_topic = self.get_parameter('image_topic').value
        cmd_vel_topic = self.get_parameter('cmd_vel_topic').value

        self.linear_speed = float(self.get_parameter('linear_speed').value)
        self.angular_gain = float(self.get_parameter('angular_gain').value)
        self.min_area = int(self.get_parameter('min_area').value)
        self.stop_area = int(self.get_parameter('stop_area').value)
        self.max_angular_speed = float(self.get_parameter('max_angular_speed').value)
        self.image_center_tolerance = int(self.get_parameter('image_center_tolerance').value)

        self.bridge = CvBridge()

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

        self.cmd_pub = self.create_publisher(
            Twist,
            cmd_vel_topic,
            10
        )

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

        self.mask_pub = self.create_publisher(
            Image,
            '/color_follower/mask',
            10
        )

        self.get_logger().info('Color follower node started')
        self.get_logger().info(f'Subscribed image topic: {image_topic}')
        self.get_logger().info(f'Published cmd_vel topic: {cmd_vel_topic}')

    def get_hsv_mask(self, hsv_image):
        target_color = self.get_parameter('target_color').value

        if target_color == 'red':
            lower_red_1 = np.array([0, 100, 80])
            upper_red_1 = np.array([10, 255, 255])

            lower_red_2 = np.array([170, 100, 80])
            upper_red_2 = np.array([180, 255, 255])

            mask_1 = cv2.inRange(hsv_image, lower_red_1, upper_red_1)
            mask_2 = cv2.inRange(hsv_image, lower_red_2, upper_red_2)

            mask = cv2.bitwise_or(mask_1, mask_2)

        elif target_color == 'green':
            lower_green = np.array([40, 80, 80])
            upper_green = np.array([85, 255, 255])

            mask = cv2.inRange(hsv_image, lower_green, upper_green)

        elif target_color == 'blue':
            lower_blue = np.array([95, 80, 80])
            upper_blue = np.array([130, 255, 255])

            mask = cv2.inRange(hsv_image, lower_blue, upper_blue)

        elif target_color == 'yellow':
            lower_yellow = np.array([20, 100, 100])
            upper_yellow = np.array([35, 255, 255])

            mask = cv2.inRange(hsv_image, lower_yellow, upper_yellow)

        else:
            self.get_logger().warn(f'Unknown target_color: {target_color}, use red')
            lower_red_1 = np.array([0, 100, 80])
            upper_red_1 = np.array([10, 255, 255])
            lower_red_2 = np.array([170, 100, 80])
            upper_red_2 = np.array([180, 255, 255])
            mask_1 = cv2.inRange(hsv_image, lower_red_1, upper_red_1)
            mask_2 = cv2.inRange(hsv_image, lower_red_2, upper_red_2)
            mask = cv2.bitwise_or(mask_1, mask_2)

        return mask

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

        height, width, _ = frame.shape
        image_center_x = width // 2

        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        mask = self.get_hsv_mask(hsv)

        kernel = np.ones((5, 5), np.uint8)

        mask = cv2.erode(mask, kernel, iterations=1)
        mask = cv2.dilate(mask, kernel, iterations=2)

        contours, _ = cv2.findContours(
            mask,
            cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE
        )

        debug_frame = frame.copy()

        if len(contours) == 0:
            self.stop_robot()
            self.publish_debug_images(debug_frame, mask)
            return

        largest_contour = max(contours, key=cv2.contourArea)
        area = cv2.contourArea(largest_contour)

        if area < self.min_area:
            self.stop_robot()
            self.publish_debug_images(debug_frame, mask)
            return

        moments = cv2.moments(largest_contour)

        if moments['m00'] == 0:
            self.stop_robot()
            self.publish_debug_images(debug_frame, mask)
            return

        object_center_x = int(moments['m10'] / moments['m00'])
        object_center_y = int(moments['m01'] / moments['m00'])

        error_x = object_center_x - image_center_x

        x, y, w, h = cv2.boundingRect(largest_contour)

        cv2.rectangle(
            debug_frame,
            (x, y),
            (x + w, y + h),
            (0, 255, 0),
            2
        )

        cv2.circle(
            debug_frame,
            (object_center_x, object_center_y),
            8,
            (0, 0, 255),
            -1
        )

        cv2.line(
            debug_frame,
            (image_center_x, 0),
            (image_center_x, height),
            (255, 0, 0),
            2
        )

        cv2.putText(
            debug_frame,
            f'area: {int(area)} error_x: {error_x}',
            (20, 40),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.7,
            (0, 255, 255),
            2
        )

        twist = Twist()

        if abs(error_x) > self.image_center_tolerance:
            angular_z = -self.angular_gain * error_x
            angular_z = max(
                min(angular_z, self.max_angular_speed),
                -self.max_angular_speed
            )
            twist.angular.z = angular_z
        else:
            twist.angular.z = 0.0

        if area < self.stop_area:
            twist.linear.x = self.linear_speed
        else:
            twist.linear.x = 0.0

        self.cmd_pub.publish(twist)
        self.publish_debug_images(debug_frame, mask)

    def publish_debug_images(self, debug_frame, mask):
        try:
            debug_msg = self.bridge.cv2_to_imgmsg(debug_frame, encoding='bgr8')
            mask_msg = self.bridge.cv2_to_imgmsg(mask, encoding='mono8')

            self.debug_image_pub.publish(debug_msg)
            self.mask_pub.publish(mask_msg)

        except Exception as e:
            self.get_logger().error(f'Failed to publish debug image: {e}')

    def stop_robot(self):
        twist = Twist()
        twist.linear.x = 0.0
        twist.angular.z = 0.0
        self.cmd_pub.publish(twist)


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

    node = ColorFollowerNode()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.stop_robot()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

9. 소스 코드 상세 설명

1) 라이브러리 import

import cv2
import numpy as np

cv2는 OpenCV 라이브러리입니다. 카메라 영상 처리, 색상 변환, 마스크 생성, 윤곽선 검출에 사용합니다.

numpy는 이미지 데이터를 배열로 처리하기 위해 사용합니다. OpenCV 이미지는 내부적으로 NumPy 배열 형태입니다.

import rclpy
from rclpy.node import Node

rclpy는 ROS 2 Python 클라이언트 라이브러리입니다. Python으로 노드를 만들고, 토픽을 구독하고, 메시지를 발행할 때 사용합니다.

from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist

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

Twist는 로봇의 속도 명령 메시지입니다. TurtleBot3는 /cmd_vel 토픽으로 선속도와 각속도를 받아 움직입니다.

from cv_bridge import CvBridge

CvBridge는 ROS 2의 Image 메시지를 OpenCV 이미지로 변환하거나, OpenCV 이미지를 다시 ROS 2 Image 메시지로 변환할 때 사용합니다.

2) 노드 클래스 선언

class ColorFollowerNode(Node):

ROS 2 노드는 보통 Node 클래스를 상속해서 만듭니다.

이 클래스는 색상 추종 기능 전체를 담당합니다.

역할은 다음과 같습니다.

1. 카메라 이미지 구독
2. 색상 검출
3. 중심점 계산
4. 속도 명령 생성
5. 디버그 영상 발행

3) 파라미터 선언

self.declare_parameter('image_topic', '/camera/image_raw')
self.declare_parameter('cmd_vel_topic', '/cmd_vel')
self.declare_parameter('target_color', 'red')

파라미터를 사용하면 소스 코드를 직접 수정하지 않고 실행 명령에서 설정을 바꿀 수 있습니다.

예를 들어 기본값은 빨간색 추종이지만, 실행할 때 다음처럼 바꿀 수 있습니다.

ros2 run color_follower color_follower_node --ros-args -p target_color:=blue

이 방식은 강의에서 매우 중요합니다. 학생분들이 코드를 매번 수정하지 않고 실험값을 바꿀 수 있기 때문입니다.

self.declare_parameter('linear_speed', 0.08)
self.declare_parameter('angular_gain', 0.004)

linear_speed는 로봇의 전진 속도입니다.

TurtleBot3 Burger는 작은 로봇이므로 처음부터 빠른 속도를 주면 위험할 수 있습니다. 실습에서는 0.05에서 0.10 사이로 시작하시는 것이 좋습니다.

angular_gain은 화면 중심에서 벗어난 정도를 회전 속도로 변환하는 비례 제어 계수입니다.

값이 너무 작으면 로봇이 느리게 반응합니다.

값이 너무 크면 로봇이 좌우로 흔들릴 수 있습니다.

self.declare_parameter('min_area', 800)
self.declare_parameter('stop_area', 18000)

min_area는 물체로 인정할 최소 면적입니다.

작은 노이즈나 빛 반사를 물체로 오인하지 않기 위해 사용합니다.

stop_area는 로봇이 정지할 기준 면적입니다.

카메라에서 물체가 가까워질수록 영상에서 차지하는 면적이 커집니다. 따라서 면적이 일정 값 이상이면 충분히 가까워졌다고 판단하고 정지합니다.

self.declare_parameter('max_angular_speed', 0.8)
self.declare_parameter('image_center_tolerance', 40)

max_angular_speed는 최대 회전 속도 제한입니다.

제어식 결과가 너무 크게 나와도 이 값 이상으로 회전하지 않도록 제한합니다.

image_center_tolerance는 중심 오차 허용 범위입니다.

물체 중심이 화면 중심에서 약간 벗어났다고 계속 회전하면 로봇이 떨릴 수 있습니다. 그래서 일정 범위 안에서는 회전하지 않도록 합니다.

4) Subscriber 생성

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

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

카메라에서 이미지가 들어올 때마다 self.image_callback() 함수가 자동으로 실행됩니다.

마지막의 10은 QoS queue depth입니다. 쉽게 말해 메시지 대기열 크기입니다.

영상 처리가 느리면 오래된 이미지가 쌓일 수 있습니다. 실시간 로봇 제어에서는 오래된 이미지보다 최신 이미지가 중요합니다. 따라서 실제 운영에서는 QoS 설정을 더 세밀하게 조정할 수도 있습니다.

5) Publisher 생성

self.cmd_pub = self.create_publisher(
    Twist,
    cmd_vel_topic,
    10
)

이 Publisher는 /cmd_vel 토픽으로 로봇 속도 명령을 발행합니다.

TurtleBot3의 기본 이동 명령은 다음 구조를 사용합니다.

twist.linear.x
twist.angular.z

twist.linear.x는 전진 또는 후진 속도입니다.

twist.angular.z는 좌회전 또는 우회전 속도입니다.

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

self.mask_pub = self.create_publisher(
    Image,
    '/color_follower/mask',
    10
)

이 두 Publisher는 디버그 영상을 내보냅니다.

실제 로봇 동작에는 필수는 아니지만, 참고로 이 토픽을 추가하면 “왜 로봇이 움직이는지”를 눈으로 확인할 수 있기 때문입니다.

6) HSV 마스크 생성 함수

def get_hsv_mask(self, hsv_image):

이 함수는 HSV 이미지에서 특정 색상만 흰색으로 남기는 마스크를 만듭니다.

예를 들어 빨간색 검출 부분은 다음과 같습니다.

lower_red_1 = np.array([0, 100, 80])
upper_red_1 = np.array([10, 255, 255])

lower_red_2 = np.array([170, 100, 80])
upper_red_2 = np.array([180, 255, 255])

빨간색은 HSV 색상환에서 0도 근처와 180도 근처에 걸쳐 있습니다.

그래서 빨간색은 한 범위로만 잡으면 일부 빨간색을 놓칠 수 있습니다.

따라서 다음 두 범위를 합칩니다.

1. Hue 0~10
2. Hue 170~180

mask_1 = cv2.inRange(hsv_image, lower_red_1, upper_red_1)
mask_2 = cv2.inRange(hsv_image, lower_red_2, upper_red_2)

mask = cv2.bitwise_or(mask_1, mask_2)

cv2.inRange()는 지정한 범위 안에 들어오는 픽셀을 흰색으로 만들고, 범위 밖 픽셀은 검은색으로 만듭니다.

cv2.bitwise_or()는 두 마스크를 합칩니다.

초록색, 파란색, 노란색은 다음처럼 하나의 범위로 처리합니다.

lower_green = np.array([40, 80, 80])
upper_green = np.array([85, 255, 255])

이 값들은 절대적인 정답이 아닙니다. 조명, 카메라 노출, 물체 재질에 따라 조정해야 합니다.

강의에서는 학생분들이 이 값을 직접 바꿔보도록 진행하시면 좋습니다.

7) 이미지 콜백 함수

def image_callback(self, msg):

이 함수는 카메라 이미지가 들어올 때마다 실행됩니다.

먼저 ROS 이미지 메시지를 OpenCV 이미지로 변환합니다.

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

desired_encoding='bgr8'은 OpenCV에서 일반적으로 사용하는 BGR 8비트 컬러 이미지로 변환하겠다는 의미입니다.

변환에 실패하면 로봇을 정지시킵니다.

except Exception as e:
    self.get_logger().error(f'cv_bridge error: {e}')
    self.stop_robot()
    return

로봇 제어에서는 에러가 발생했을 때 계속 움직이는 것이 가장 위험합니다.

따라서 문제가 생기면 정지하는 방향으로 코드를 작성하는 것이 좋습니다.

이미지 상/하, 좌/우 반전을 수행합니다.

frame = cv2.flip(frame, -1)

8) 이미지 중심 계산

height, width, _ = frame.shape
image_center_x = width // 2

카메라 영상의 가로 크기를 구하고, 화면 중심 x좌표를 계산합니다.

예를 들어 영상 크기가 640×480이면 중심 x좌표는 320입니다.

로봇은 물체 중심이 이 값에 가까워지도록 회전합니다.

9) BGR에서 HSV로 변환

hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

카메라 원본 이미지는 BGR 형식입니다.

색상 검출을 쉽게 하기 위해 HSV 형식으로 변환합니다.

이후 get_hsv_mask() 함수를 호출합니다.

mask = self.get_hsv_mask(hsv)

10) 노이즈 제거

kernel = np.ones((5, 5), np.uint8)

mask = cv2.erode(mask, kernel, iterations=1)
mask = cv2.dilate(mask, kernel, iterations=2)

np.ones((5, 5), np.uint8)

결과는 이런 5×5 배열입니다.

1 1 1 1 1
1 1 1 1 1
1 1 1 1 1
1 1 1 1 1
1 1 1 1 1

np.uint8은 OpenCV에서 자주 쓰는 8비트 정수 타입입니다.

카메라 영상에는 빛 반사, 그림자, 작은 잡음이 포함될 수 있습니다.

erode는 흰색 영역을 깎아서 작은 노이즈를 제거합니다. 즉, 마스크에서 흰색 영역을 줄입니다.

dilate는 흰색 영역을 다시 키워서 실제 물체 영역을 복원합니다.

이 과정을 통해 마스크가 더 안정적으로 만들어집니다.

11) 윤곽선 검출

contours, _ = cv2.findContours(
    mask,
    cv2.RETR_EXTERNAL,
    cv2.CHAIN_APPROX_SIMPLE
)

cv2.findContours()는 마스크에서 흰색 영역의 외곽선을 찾습니다.

cv2.RETR_EXTERNAL은 가장 바깥쪽 윤곽선만 찾겠다는 의미입니다.

cv2.CHAIN_APPROX_SIMPLE은 윤곽선 정보를 압축해서 저장하겠다는 의미입니다.

검출된 윤곽선이 없으면 로봇을 정지합니다.

if len(contours) == 0:
    self.stop_robot()
    self.publish_debug_images(debug_frame, mask)
    return

물체가 보이지 않는데 로봇이 계속 움직이면 위험합니다.

그래서 물체를 잃어버리면 정지하도록 구성합니다.

12) 가장 큰 물체 선택

largest_contour = max(contours, key=cv2.contourArea)
area = cv2.contourArea(largest_contour)

같은 색상의 작은 잡음이 여러 개 검출될 수 있습니다.

각 contour의 면적을 계산해서 그중 면적이 가장 큰 contour를 선택합니다.

그중 가장 큰 영역을 실제 추종 대상이라고 판단합니다. area는 픽셀 단위 면적입니다.

if area < self.min_area:
    self.stop_robot()
    self.publish_debug_images(debug_frame, mask)
    return

self.min_area는 사용자가 정한 최소 유효 면적 기준값입니다.

검출 면적이 너무 작으면 노이즈로 판단하고 무시합니다.

13) 물체 중심 계산

moments = cv2.moments(largest_contour)

모멘트는 영상 영역의 중심, 면적 등을 계산할 때 사용하는 값입니다. 이 값 중에서 가장 중요한 값은 3개입니다.

m00 → 객체의 면적(객체가 차지하는 전체 픽셀 수)
m10 → x 방향 1차 모멘트(x좌표 방향의 누적값 -> x좌표를 모두 더한 값)
m01 → y 방향 1차 모멘트(y좌표 방향의 누적값-> y좌표를 모두 더한 값)

중심 좌표는 다음 식으로 계산합니다.

object_center_x = int(moments['m10'] / moments['m00'])
object_center_y = int(moments['m01'] / moments['m00'])

m00은 면적에 해당합니다.

객체 전체의 x좌표 합 / 객체 면적 = 객체의 평균 x좌표 이므로 객체를 이루는 픽셀들의 평균 x 위치를 구하는 것입니다.

m10 / m00은 x 중심입니다.

m01 / m00은 y 중심입니다.

14) 중심 오차 계산

error_x = object_center_x - image_center_x

이 값이 제어의 핵심입니다.

예를 들어 화면 중심이 320이고 물체 중심이 420이면 다음과 같습니다.

error_x = 420 - 320 = 100

물체가 화면 오른쪽에 있다는 뜻입니다.

반대로 물체 중심이 220이면 다음과 같습니다.

error_x = 220 - 320 = -100

물체가 화면 왼쪽에 있다는 뜻입니다.

이 오차를 이용해 로봇의 회전 방향을 결정합니다.

1)5 디버그 영상 표시

cv2.rectangle(
    debug_frame,
    (x, y),
    (x + w, y + h),
    (0, 255, 0),
    2
)

검출된 물체 주변에 사각형을 그립니다.

cv2.circle(
    debug_frame,
    (object_center_x, object_center_y),
    8,
    (0, 0, 255),
    -1
)

물체 중심점에 원을 그립니다.

cv2.line(
    debug_frame,
    (image_center_x, 0),
    (image_center_x, height),
    (255, 0, 0),
    2
)

화면 중앙에 세로선을 그립니다.

디버그 영상을 보면 다음 관계를 쉽게 이해할 수 있습니다.

1. 빨간 점이 물체 중심입니다.
2. 파란 선이 화면 중심입니다.
3. 빨간 점이 파란 선보다 오른쪽이면 로봇이 오른쪽으로 방향을 맞춥니다.
4. 빨간 점이 파란 선보다 왼쪽이면 로봇이 왼쪽으로 방향을 맞춥니다.

16) 회전 제어

if abs(error_x) > self.image_center_tolerance:
    angular_z = -self.angular_gain * error_x
    angular_z = max(
        min(angular_z, self.max_angular_speed),
        -self.max_angular_speed
    )
    twist.angular.z = angular_z
else:
    twist.angular.z = 0.0

여기서는 단순한 P 제어를 사용합니다.

P 제어는 오차에 비례해서 제어 출력을 만드는 방식입니다.

angular_z = -self.angular_gain * error_x

error_x가 크면 회전 속도도 커집니다.

error_x가 작으면 회전 속도도 작아집니다.

앞에 -가 붙은 이유는 카메라 좌표계와 로봇 회전 방향을 맞추기 위해서입니다.

OpenCV 이미지 좌표계에서는 x가 오른쪽으로 갈수록 커집니다.

반면 ROS에서 angular.z는 양수일 때 반시계 방향 회전입니다.

실제 로봇에서 방향이 반대로 움직이면 이 부호를 바꾸면 됩니다.

angular_z = self.angular_gain * error_x

이 부호를 수정하면 로봇은 반대로 회전합니다.

17) 전진/정지 제어

if area < self.stop_area:
    twist.linear.x = self.linear_speed
else:
    twist.linear.x = 0.0

물체의 면적이 작다는 것은 물체가 멀리 있다는 의미입니다.

그래서 로봇이 전진합니다.

반대로 물체의 면적이 크다는 것은 물체가 가까이 있다는 의미입니다.

그래서 로봇이 정지합니다.

이 방식은 거리 센서를 사용하지 않는 간단한 거리 추정 방식입니다.

정확한 거리를 알 수는 없지만, 색상 물체 추종 실습에는 충분히 사용할 수 있습니다.

18) 속도 명령 발행

self.cmd_pub.publish(twist)

계산된 속도 명령을 /cmd_vel로 발행합니다.

TurtleBot3는 이 명령을 받아 실제 바퀴를 움직입니다.

19) 디버그 이미지 발행

debug_msg = self.bridge.cv2_to_imgmsg(debug_frame, encoding='bgr8')
mask_msg = self.bridge.cv2_to_imgmsg(mask, encoding='mono8')

OpenCV 이미지를 다시 ROS 2 이미지 메시지로 변환합니다.

self.debug_image_pub.publish(debug_msg)
self.mask_pub.publish(mask_msg)

변환된 이미지를 ROS 2 토픽으로 발행합니다.

이제 원격 PC에서 rqt_image_view로 디버그 영상을 확인할 수 있습니다.

20) 정지 함수

def stop_robot(self):
    twist = Twist()
    twist.linear.x = 0.0
    twist.angular.z = 0.0
    self.cmd_pub.publish(twist)

이 함수는 로봇을 정지시키는 역할을 합니다.

다음 상황에서 호출됩니다.

1. 카메라 이미지 변환 실패
2. 물체 미검출
3. 검출 면적이 너무 작음
4. 모멘트 계산 실패
5. 노드 종료

로봇 제어 코드에서는 정지 함수를 명확히 만들어 두는 것이 좋습니다.

10. setup.py 수정

setup.py 파일을 다음과 같이 수정합니다.

from setuptools import find_packages, setup

package_name = 'color_follower'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    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='Color object follower for TurtleBot3 Burger using ROS 2 Humble and OpenCV',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'color_follower_node = color_follower.color_follower_node:main',
        ],
    },
)

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

entry_points={
    'console_scripts': [
        'color_follower_node = color_follower.color_follower_node:main',
    ],
},

이 설정을 추가하면 다음 명령으로 노드를 실행할 수 있습니다.

ros2 run color_follower color_follower_node

즉, color_follower_node.py 파일 안의 main() 함수가 ROS 2 실행 파일처럼 등록됩니다.

11. package.xml 확인

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

<?xml version="1.0"?>
<package format="3">
  <name>color_follower</name>
  <version>0.0.0</version>
  <description>Color object follower for TurtleBot3 Burger using ROS 2 Humble and OpenCV</description>
  <maintainer email="ubuntu@example.com">ubuntu</maintainer>
  <license>Apache-2.0</license>

  <depend>rclpy</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_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>

주요 의존성의 의미는 다음과 같습니다.

1. rclpy: Python으로 ROS 2 노드를 만들기 위한 기본 라이브러리입니다.
2. sensor_msgs: 카메라 이미지 메시지를 사용하기 위해 필요합니다.
3. geometry_msgs: /cmd_vel에 사용할 Twist 메시지를 사용하기 위해 필요합니다.
4. cv_bridge: ROS 이미지와 OpenCV 이미지를 서로 변환하기 위해 필요합니다.

12. 빌드

작업 공간 루트로 이동합니다.

cd ~/turtlebot3_ws

빌드합니다.

colcon build --packages-select color_follower

환경을 적용합니다.

source install/setup.bash

패키지가 정상적으로 인식되는지 확인합니다.

ros2 pkg list | grep color_follower

13. 실행 순서

강의에서는 터미널을 3개 이상 사용하는 것이 좋습니다.

첫 번째 터미널에서 TurtleBot3 기본 bringup을 실행합니다.

ssh ubuntu@ROBOT_IP
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_bringup robot.launch.py

두 번째 터미널에서 카메라를 실행합니다.

ssh ubuntu@ROBOT_IP
ros2 launch turtlebot3_bringup camera.launch.py format:=BGR888

세 번째 터미널에서 색상 추종 노드를 실행합니다.

source ~/turtlebot3_ws/install/setup.bash
ros2 run color_follower color_follower_node

기본 설정은 빨간색 물체 추종입니다.

파란색 물체를 추종하려면 다음과 같이 실행합니다.

ros2 run color_follower color_follower_node --ros-args -p target_color:=blue

초록색 물체를 추종하려면 다음과 같이 실행합니다.

ros2 run color_follower color_follower_node --ros-args -p target_color:=green

노란색 물체를 추종하려면 다음과 같이 실행합니다.

ros2 run color_follower color_follower_node --ros-args -p target_color:=yellow

14. 디버그 영상 확인

원격 PC에서 다음 명령을 실행합니다.

rqt_image_view

확인할 수 있는 토픽은 다음과 같습니다.

/camera/image_raw
/color_follower/debug_image
/color_follower/mask

각 토픽의 의미는 다음과 같습니다.

1. /camera/image_raw
   1. 카메라 원본 영상입니다.
2. /color_follower/debug_image
   1. 검출된 물체의 사각형, 중심점, 화면 중심선, 면적 정보가 표시된 영상입니다.

1. /color_follower/mask
   1. 색상 검출 결과를 흑백으로 보여주는 영상입니다.
   2. 흰색 영역은 검출된 색상입니다.
   3. 검은색 영역은 제외된 영역입니다.

강의에서는 /color_follower/debug_image와 /color_follower/mask를 함께 보여주시는 것이 좋습니다. 학생분들이 색상 검출이 실제로 어떻게 이루어지는지 바로 이해할 수 있습니다.

15. 파라미터 튜닝 방법

실행할 때 파라미터를 바꿀 수 있습니다.

예를 들어 전진 속도를 낮추고 싶다면 다음처럼 실행합니다.

ros2 run color_follower color_follower_node --ros-args \
  -p linear_speed:=0.05

회전 반응을 빠르게 하고 싶다면 다음처럼 실행합니다.

ros2 run color_follower color_follower_node --ros-args \
  -p angular_gain:=0.006

너무 작은 물체가 검출되지 않도록 하고 싶다면 다음처럼 실행합니다.

ros2 run color_follower color_follower_node --ros-args \
  -p min_area:=1500

물체와 더 멀리 떨어진 상태에서 정지하게 하려면 stop_area를 작게 설정합니다.

ros2 run color_follower color_follower_node --ros-args \
  -p stop_area:=12000

물체에 더 가까이 다가가게 하려면 stop_area를 크게 설정합니다.

ros2 run color_follower color_follower_node --ros-args \
  -p stop_area:=25000

여러 파라미터를 동시에 줄 수도 있습니다.

ros2 run color_follower color_follower_node --ros-args \
  -p target_color:=blue \
  -p linear_speed:=0.06 \
  -p angular_gain:=0.005 \
  -p min_area:=1000 \
  -p stop_area:=20000

16. launch 파일 만들기

매번 긴 명령을 입력하기 번거롭기 때문에 launch 파일을 만들어 두는 것이 좋습니다.

먼저 launch 폴더를 만듭니다.

cd ~/turtlebot3_ws/src/color_follower
mkdir launch
touch launch/color_follower.launch.py

launch/color_follower.launch.py에 다음 코드를 작성합니다.

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    color_follower_node = Node(
        package='color_follower',
        executable='color_follower_node',
        name='color_follower_node',
        output='screen',
        parameters=[
            {
                'image_topic': '/camera/image_raw',
                'cmd_vel_topic': '/cmd_vel',
                'target_color': 'red',
                'linear_speed': 0.08,
                'angular_gain': 0.004,
                'min_area': 800,
                'stop_area': 18000,
                'max_angular_speed': 0.8,
                'image_center_tolerance': 40,
            }
        ]
    )

    return LaunchDescription([
        color_follower_node
    ])

setup.py에서 launch 파일이 설치되도록 수정합니다.

import os
from glob import glob
from setuptools import find_packages, setup

package_name = 'color_follower'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    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='Color object follower for TurtleBot3 Burger using ROS 2 Humble and OpenCV',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'color_follower_node = color_follower.color_follower_node:main',
        ],
    },
)

다시 빌드합니다.

cd ~/turtlebot3_ws
colcon build --symlink-install
source install/setup.bash

launch 파일로 실행합니다.

ros2 launch color_follower color_follower.launch.py

17. 실습 과제

1) 과제 1: 색상 변경

기본 빨간색 추종 코드를 파란색 추종으로 바꿔봅니다.

ros2 run color_follower color_follower_node --ros-args -p target_color:=blue

확인할 내용은 다음과 같습니다.

1. /color_follower/mask에서 파란색 물체만 흰색으로 보이는지 확인합니다.
2. 로봇이 파란색 물체를 향해 회전하는지 확인합니다.
3. 물체가 가까워지면 정지하는지 확인합니다.

2) 과제 2: 속도 조정

전진 속도를 0.03, 0.06, 0.10으로 바꿔보며 로봇 움직임을 비교합니다.

ros2 run color_follower color_follower_node --ros-args -p linear_speed:=0.03

확인할 내용은 다음과 같습니다.

1. 속도가 너무 낮으면 추종이 답답한지 확인합니다.
2. 속도가 너무 높으면 위험하거나 흔들리는지 확인합니다.
3. TurtleBot3 Burger에 적절한 속도가 어느 정도인지 확인합니다.

3) 과제 3: 회전 게인 조정

angular_gain 값을 바꿔봅니다.

ros2 run color_follower color_follower_node --ros-args -p angular_gain:=0.002

ros2 run color_follower color_follower_node --ros-args -p angular_gain:=0.008

확인할 내용은 다음과 같습니다.

1. 게인이 작으면 반응이 느린지 확인합니다.
2. 게인이 크면 좌우로 흔들리는지 확인합니다.
3. 가장 안정적인 값이 얼마인지 확인합니다.

4) 과제 4: 정지 거리 조정

stop_area를 바꿔 정지 거리를 조절합니다.

ros2 run color_follower color_follower_node --ros-args -p stop_area:=10000

ros2 run color_follower color_follower_node --ros-args -p stop_area:=30000

확인할 내용은 다음과 같습니다.

1. stop_area가 작으면 더 멀리서 멈추는지 확인합니다.
2. stop_area가 크면 더 가까이 접근하는지 확인합니다.
3. 실제 안전 거리와 영상 면적 사이의 관계를 확인합니다.