ROS 2 Humble TurtleBot3 Burger에서 실제 충전스테이션 파킹 알고리즘 방식의 ArUco 도킹 구현 #1

by

1. 실습 목표

이번 실습에서는 Ubuntu 22.04 Server가 설치된 TurtleBot3 Burger에서 ROS 2 Humble, Pi Camera 2, OpenCV ArUco를 사용하여 충전스테이션 파킹 알고리즘과 유사한 자동 도킹 노드를 구현합니다.

실제 충전스테이션 도킹은 보통 다음 순서로 동작합니다.

  1. 충전스테이션 마커 또는 신호를 탐색합니다.
  2. 바로 충전 단자로 돌진하지 않고, 먼저 진입 기준 위치로 이동합니다.
  3. 충전스테이션의 정면 축과 로봇의 방향을 맞춥니다.
  4. 정렬이 완료된 뒤 매우 낮은 속도로 최종 진입합니다.
  5. 접촉 구간에서는 천천히 밀어 넣습니다.
  6. 충전 전류 또는 충전 상태를 확인합니다.
  7. 충전이 확인되면 도킹 완료로 판단합니다.
  8. 실패하면 후진 후 재탐색합니다.

이번 글에서는 이 흐름을 ROS 2 노드로 구현합니다.

핵심은 단순한 “마커 추종”이 아니라 “상태머신 기반 충전스테이션 파킹 알고리즘”입니다.

2. 전체 동작 구조

이번 실습의 전체 구조는 다음과 같습니다.

Pi Camera 2
    ↓
/camera/image_raw
/camera/camera_info
    ↓
ArUco Marker Detection
    ↓
충전스테이션 상대 위치 계산
    ↓
상태머신 기반 도킹 제어
    ↓
/cmd_vel
    ↓
TurtleBot3 Burger 이동
    ↓
접촉 또는 충전 상태 확인
    ↓
도킹 완료

노드에서 사용하는 주요 토픽은 다음과 같습니다.

입력 이미지 토픽:
  /camera/image_raw

카메라 정보 토픽:
  /camera/camera_info

속도 명령 토픽:
  /cmd_vel

배터리 상태 토픽:
  /battery_state

디버그 이미지 토픽:
  /charging_dock/debug_image

도킹 상태 토픽:
  /charging_dock/state

도킹 완료 토픽:
  /charging_dock/docked

/battery_state는 실제 충전 여부를 확인하기 위해 사용합니다.
로봇 환경에 따라 이 토픽이 없을 수도 있습니다.

이 경우 실습에서는 거리 기반으로 도킹 완료를 확인할 수 있지만, 실제 충전스테이션에서는 반드시 충전 전류, 충전 전압, 접촉 스위치, 리미트 스위치, IR 센서, ToF 센서 중 하나로 최종 접촉 확인을 해야 합니다.

3. 실제 충전스테이션 파킹 알고리즘 개념

이번에 구현할 알고리즘은 다음 상태를 가집니다.

SEARCH_MARKER
    ↓
APPROACH_PRE_DOCK
    ↓
ALIGN_DOCK_AXIS
    ↓
FINAL_APPROACH
    ↓
CONTACT_PUSH
    ↓
CHARGE_VERIFY
    ↓
DOCKED

실패하면 다음 상태로 이동합니다.

RECOVERY_BACKUP
    ↓
SEARCH_MARKER

최대 재시도 횟수를 넘으면 다음 상태가 됩니다.

FAILED

각 상태의 의미는 다음과 같습니다.

1) SEARCH_MARKER

충전스테이션의 ArUco 마커를 찾는 상태입니다.

마커가 보이지 않으면 로봇은 제자리에서 천천히 회전합니다.
마커가 보이면 다음 상태로 넘어갑니다.

2) APPROACH_PRE_DOCK

충전스테이션 바로 앞까지 가는 것이 아니라, 먼저 안전한 진입 기준 위치까지 접근합니다.

이 위치를 pre_dock_distance_m으로 설정합니다.

예를 들어 다음과 같이 설정할 수 있습니다.

pre_dock_distance_m = 0.70

이 말은 마커 기준 약 70cm 앞까지 먼저 접근한다는 의미입니다.

실제 충전스테이션 도킹에서 중요한 것은 바로 최종 접촉 위치로 들어가지 않는 것입니다.
먼저 스테이션 정면에서 정렬할 수 있는 위치를 잡아야 합니다.

3) ALIGN_DOCK_AXIS

진입 기준 위치에 도착하면 로봇은 충전스테이션의 중심선과 자신의 방향을 맞춥니다.

이때 사용하는 값은 두 가지입니다.

lateral error:
  마커 중심이 카메라 기준 좌우로 얼마나 벗어났는가

bearing error:
  마커 중심이 카메라 정면에서 어느 각도에 있는가

로봇이 정면축에 맞지 않으면 최종 진입 때 충전 단자가 비껴갈 수 있습니다.
그래서 이 단계에서 좌우 오차와 방향 오차를 줄입니다.

4) FINAL_APPROACH

정렬이 완료되면 매우 낮은 속도로 충전스테이션으로 접근합니다.

이 단계에서는 속도를 크게 낮춥니다.

예를 들어 다음과 같이 설정합니다.

final_approach_speed_mps = 0.025

TurtleBot3 Burger 기준으로 0.02~0.04m/s 정도가 적당합니다.
빠르게 접근하면 충전 단자나 스테이션 구조물에 충격을 줄 수 있습니다.

5) CONTACT_PUSH

최종 거리까지 접근하면 로봇은 아주 짧은 시간 동안 더 천천히 밀어 넣습니다.

이 동작은 실제 충전스테이션에서 매우 중요합니다.

이유는 다음과 같습니다.

  1. 충전 단자에 약간의 기구적 탄성이 있습니다.
  2. 바퀴 오차 때문에 마지막 접촉이 완전히 되지 않을 수 있습니다.
  3. 살짝 밀어 넣어야 접점이 안정적으로 붙을 수 있습니다.
  4. 단순히 거리만 맞춘다고 충전이 시작되는 것은 아닙니다.

그래서 최종 진입 후 다음과 같은 저속 접촉 밀어넣기 동작을 수행합니다.

contact_push_speed_mps = 0.010
contact_push_time_sec = 1.2

6) CHARGE_VERIFY

이 단계에서는 충전이 실제로 시작되었는지 확인합니다.

확인 방법은 로봇 구성에 따라 다릅니다.

  1. /battery_state의 power_supply_status가 CHARGING인지 확인
  2. 충전 전류가 일정 값 이상인지 확인
  3. 충전 접점 GPIO 입력 확인
  4. 리미트 스위치 확인
  5. 도킹 완료 센서 확인

이번 예제에서는 /battery_state를 사용할 수 있도록 구현합니다.

단, TurtleBot3 환경에 따라 /battery_state에 충전 상태가 정확히 들어오지 않을 수 있습니다.
실제 충전스테이션으로 만들려면 이 부분을 반드시 자신의 하드웨어에 맞게 연결해야 합니다.

7) DOCKED

충전이 확인되면 도킹 완료 상태입니다.

이 상태에서는 /cmd_vel을 계속 0으로 발행합니다.

8) RECOVERY_BACKUP

도킹 중 마커를 잃거나 충전 확인에 실패하면 로봇은 잠깐 후진합니다.

후진 후 다시 마커를 탐색합니다.

이것이 실제 파킹 알고리즘과 단순 추종 알고리즘의 큰 차이입니다.
실패했을 때 멈추기만 하는 것이 아니라, 안전하게 빠져나온 뒤 다시 시도합니다.

4. 패키지 생성

TurtleBot3 워크스페이스로 이동합니다.

cd ~/turtlebot3_ws/src

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

ros2 pkg create tb3_charging_dock \
  --build-type ament_python \
  --dependencies rclpy sensor_msgs geometry_msgs std_msgs cv_bridge

패키지 폴더로 이동합니다.

cd ~/turtlebot3_ws/src/tb3_charging_dock

launch 폴더와 노드 파일을 생성합니다.

mkdir -p launch
touch launch/charging_dock.launch.py
touch tb3_charging_dock/charging_dock_node.py

최종 구조는 다음과 같습니다.

tb3_charging_dock/
├── package.xml
├── setup.py
├── setup.cfg
├── resource/
│   └── tb3_charging_dock
├── launch/
│   └── charging_dock.launch.py
└── tb3_charging_dock/
    ├── __init__.py
    └── charging_dock_node.py

5. 의존 패키지 설치

이번 실습에서는 다음 패키지가 필요합니다.

sudo apt update
sudo apt install -y \
  ros-humble-cv-bridge \
  ros-humble-vision-opencv \
  python3-opencv \
  python3-numpy \
  python3-colcon-common-extensions

이미 설치되어 있다면 이 과정은 생략해도 됩니다.

설치 여부는 다음 명령으로 확인합니다.

dpkg -l | grep ros-humble-cv-bridge
dpkg -l | grep python3-opencv

OpenCV에서 ArUco 모듈을 사용할 수 있는지도 확인합니다.

python3 - <<'PY'
import cv2
print('OpenCV version:', cv2.__version__)
print('has aruco:', hasattr(cv2, 'aruco'))
PY

다음처럼 출력되어야 합니다.

has aruco: True

False가 나오면 현재 OpenCV에 ArUco 모듈이 포함되어 있지 않은 것입니다.

6. 카메라 토픽 확인

카메라 토픽을 확인합니다.

ros2 topic list | grep camera

예상 토픽은 다음과 같습니다.

/camera/image_raw
/camera/camera_info

Pi Camera 2 노드 설정에 따라 다음처럼 나올 수도 있습니다.

/picam2/image_raw
/picam2/camera_info

이미지 주기를 확인합니다.

ros2 topic hz /camera/image_raw

카메라 내부 파라미터도 확인합니다.

ros2 topic echo /camera/camera_info --once

camera_info가 있어야 ArUco 마커의 거리 추정이 가능합니다.

7. 충전스테이션용 ArUco 마커 배치

이번 알고리즘은 충전스테이션 정면에 ArUco 마커가 붙어 있다고 가정합니다.

권장 조건은 다음과 같습니다.

  1. 마커는 충전스테이션 정면 중앙에 부착합니다.
  2. 마커 평면은 로봇이 접근하는 방향과 수직이 되도록 합니다.
  3. 마커 중심과 카메라 높이는 최대한 비슷하게 맞춥니다.
  4. 마커 크기는 10cm 이상을 권장합니다.
  5. 마커 주변에는 흰색 여백을 충분히 둡니다.
  6. 조명 반사가 적은 무광 종이를 사용합니다.

예를 들어 한 변이 10cm인 ArUco 마커를 사용한다면 실행 시 다음 값을 사용합니다.

marker_size_m:=0.10

실제 출력 크기와 marker_size_m 값이 다르면 거리 추정값이 틀어집니다.

8. 노드 소스 작성

파일을 엽니다.

touch ~/turtlebot3_ws/src/tb3_charging_dock/tb3_charging_dock/charging_dock_node.py

아래 소스를 입력합니다.

#!/usr/bin/env python3

import math
from dataclasses import dataclass
from enum import Enum
from typing import Optional, Union

import cv2
import numpy as np

import rclpy
from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.qos import (
    QoSProfile,
    ReliabilityPolicy,
    HistoryPolicy,
    DurabilityPolicy,
)

from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist
from sensor_msgs.msg import BatteryState, CameraInfo, Image, CompressedImage
from std_msgs.msg import Bool, String


class DockState(Enum):
    SEARCH_MARKER = 'SEARCH_MARKER'
    APPROACH_PRE_DOCK = 'APPROACH_PRE_DOCK'
    ALIGN_DOCK_AXIS = 'ALIGN_DOCK_AXIS'
    FINAL_APPROACH = 'FINAL_APPROACH'
    CONTACT_PUSH = 'CONTACT_PUSH'
    CHARGE_VERIFY = 'CHARGE_VERIFY'
    DOCKED = 'DOCKED'
    RECOVERY_BACKUP = 'RECOVERY_BACKUP'
    FAILED = 'FAILED'


@dataclass
class ArucoObservation:
    marker_id: int
    x_m: float
    y_m: float
    z_m: float
    bearing_rad: float
    marker_normal_yaw_rad: float
    center_x: float
    center_y: float
    image_width: int
    image_height: int
    area: float

class ChargingDockNode(Node):
    def __init__(self) -> None:
        super().__init__('charging_dock_node')

        self.declare_parameter('image_topic', '/camera/image_raw/compressed')
        self.declare_parameter('camera_info_topic', '/camera/camera_info')
        self.declare_parameter('cmd_vel_topic', '/cmd_vel')
        self.declare_parameter('battery_topic', '/battery_state')
        self.declare_parameter('debug_image_topic', '/charging_dock/debug_image/compressed')
        self.declare_parameter('state_topic', '/charging_dock/state')
        self.declare_parameter('docked_topic', '/charging_dock/docked')

        self.declare_parameter('aruco_dictionary', 'DICT_4X4_50')
        self.declare_parameter('target_marker_id', 0)
        self.declare_parameter('marker_size_m', 0.10)

        self.declare_parameter('pre_dock_distance_m', 0.70)
        self.declare_parameter('pre_dock_tolerance_m', 0.06)
        self.declare_parameter('final_dock_distance_m', 0.18)

        self.declare_parameter('lateral_tolerance_m', 0.035)
        self.declare_parameter('bearing_tolerance_rad', 0.060)
        self.declare_parameter('final_lateral_limit_m', 0.060)
        self.declare_parameter('final_bearing_limit_rad', 0.100)

        self.declare_parameter('control_rate_hz', 10.0)
        self.declare_parameter('max_approach_speed_mps', 0.070)
        self.declare_parameter('min_approach_speed_mps', 0.018)
        self.declare_parameter('final_approach_speed_mps', 0.025)
        self.declare_parameter('contact_push_speed_mps', 0.010)
        self.declare_parameter('max_angular_speed_rps', 0.80)
        self.declare_parameter('max_reverse_speed_mps', 0.040)

        self.declare_parameter('k_z', 0.45)
        self.declare_parameter('k_bearing', 1.80)
        self.declare_parameter('k_lateral', 1.40)
        self.declare_parameter('k_final_bearing', 1.20)
        self.declare_parameter('k_final_lateral', 0.90)
        self.declare_parameter('k_normal_yaw', 0.30)

        self.declare_parameter('search_angular_speed_rps', 0.28)
        self.declare_parameter('marker_lost_timeout_sec', 0.70)
        self.declare_parameter('stale_stop_timeout_sec', 0.20)
        self.declare_parameter('contact_push_time_sec', 1.20)
        self.declare_parameter('charge_verify_timeout_sec', 6.0)
        self.declare_parameter('recovery_backup_time_sec', 1.20)
        self.declare_parameter('max_retry_count', 3)
        self.declare_parameter('max_docking_time_sec', 90.0)

        self.declare_parameter('use_battery_verify', False)
        self.declare_parameter('charge_current_threshold_a', 0.05)
        self.declare_parameter('battery_timeout_sec', 2.0)

        self.declare_parameter('enable_debug_image', True)
        self.declare_parameter('log_throttle_sec', 1.0)

        self.image_topic = self.get_parameter('image_topic').value
        self.camera_info_topic = self.get_parameter('camera_info_topic').value
        self.cmd_vel_topic = self.get_parameter('cmd_vel_topic').value
        self.battery_topic = self.get_parameter('battery_topic').value
        self.debug_image_topic = self.get_parameter('debug_image_topic').value
        self.state_topic = self.get_parameter('state_topic').value
        self.docked_topic = self.get_parameter('docked_topic').value

        self.aruco_dictionary_name = self.get_parameter('aruco_dictionary').value
        self.target_marker_id = int(self.get_parameter('target_marker_id').value)
        self.marker_size_m = float(self.get_parameter('marker_size_m').value)
        self.pre_dock_distance_m = float(self.get_parameter('pre_dock_distance_m').value)
        self.pre_dock_tolerance_m = float(self.get_parameter('pre_dock_tolerance_m').value)
        self.final_dock_distance_m = float(self.get_parameter('final_dock_distance_m').value)
        self.lateral_tolerance_m = float(self.get_parameter('lateral_tolerance_m').value)
        self.bearing_tolerance_rad = float(self.get_parameter('bearing_tolerance_rad').value)
        self.final_lateral_limit_m = float(self.get_parameter('final_lateral_limit_m').value)
        self.final_bearing_limit_rad = float(self.get_parameter('final_bearing_limit_rad').value)
        self.control_rate_hz = float(self.get_parameter('control_rate_hz').value)
        self.max_approach_speed_mps = float(self.get_parameter('max_approach_speed_mps').value)
        self.min_approach_speed_mps = float(self.get_parameter('min_approach_speed_mps').value)
        self.final_approach_speed_mps = float(self.get_parameter('final_approach_speed_mps').value)
        self.contact_push_speed_mps = float(self.get_parameter('contact_push_speed_mps').value)
        self.max_angular_speed_rps = float(self.get_parameter('max_angular_speed_rps').value)
        self.max_reverse_speed_mps = float(self.get_parameter('max_reverse_speed_mps').value)

        self.k_z = float(self.get_parameter('k_z').value)
        self.k_bearing = float(self.get_parameter('k_bearing').value)
        self.k_lateral = float(self.get_parameter('k_lateral').value)
        self.k_final_bearing = float(self.get_parameter('k_final_bearing').value)
        self.k_final_lateral = float(self.get_parameter('k_final_lateral').value)
        self.k_normal_yaw  = float(self.get_parameter('k_normal_yaw').value)

        self.search_angular_speed_rps = float(self.get_parameter('search_angular_speed_rps').value)
        self.marker_lost_timeout_sec = float(self.get_parameter('marker_lost_timeout_sec').value)
        self.stale_stop_timeout_sec = float(self.get_parameter('stale_stop_timeout_sec').value)
        self.contact_push_time_sec = float(self.get_parameter('contact_push_time_sec').value)
        self.charge_verify_timeout_sec = float(self.get_parameter('charge_verify_timeout_sec').value)
        self.recovery_backup_time_sec = float(self.get_parameter('recovery_backup_time_sec').value)
        self.max_retry_count = int(self.get_parameter('max_retry_count').value)
        self.max_docking_time_sec = float(self.get_parameter('max_docking_time_sec').value)
        self.use_battery_verify = self._get_bool_parameter('use_battery_verify')
        self.charge_current_threshold_a = float(self.get_parameter('charge_current_threshold_a').value)
        self.battery_timeout_sec = float(self.get_parameter('battery_timeout_sec').value)
        self.enable_debug_image = self._get_bool_parameter('enable_debug_image')
        self.log_throttle_sec = float(self.get_parameter('log_throttle_sec').value)

        self.bridge = CvBridge()

        self.camera_matrix: Optional[np.ndarray] = None
        self.dist_coeffs: Optional[np.ndarray] = None

        self.latest_observation: Optional[ArucoObservation] = None
        self.last_marker_time = self.get_clock().now() - Duration(seconds=999.0)
        self.latest_battery: Optional[BatteryState] = None
        self.last_battery_time = self.get_clock().now() - Duration(seconds=999.0)

        self.state = DockState.SEARCH_MARKER
        self.state_enter_time = self.get_clock().now()
        self.docking_start_time = self.get_clock().now()
        self.last_log_time = self.get_clock().now() - Duration(seconds=999.0)
        self.retry_count = 0

        self.last_tracking_angular_z = 0.0

        self.aruco_dict, self.aruco_params, self.aruco_detector = self._create_aruco_detector(
            self.aruco_dictionary_name
        )

        sensor_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )

        self.image_sub = self.create_subscription(
            CompressedImage,
            self.image_topic,
            self.image_callback,
            sensor_qos,
        )

        self.camera_info_sub = self.create_subscription(
            CameraInfo,
            self.camera_info_topic,
            self.camera_info_callback,
            sensor_qos,
        )

        self.battery_sub = self.create_subscription(
            BatteryState,
            self.battery_topic,
            self.battery_callback,
            10,
        )

        self.cmd_pub = self.create_publisher(Twist, self.cmd_vel_topic, 10)
        self.debug_pub = self.create_publisher(CompressedImage, self.debug_image_topic, sensor_qos)
        self.state_pub = self.create_publisher(String, self.state_topic, 10)
        self.docked_pub = self.create_publisher(Bool, self.docked_topic, 10)

        timer_period = 1.0 / max(self.control_rate_hz, 0.5)
        self.control_timer = self.create_timer(timer_period, self.control_loop)

        self.get_logger().info(
            f'Charging dock node started. image={self.image_topic}, '
            f'camera_info={self.camera_info_topic}, cmd_vel={self.cmd_vel_topic}, '
            f'battery={self.battery_topic}, marker_id={self.target_marker_id}'
        )

    def get_tracking_observation(
        self,
        lost_reason: str,
    ) -> Optional[ArucoObservation]:
        if self.latest_observation is None:
            self.publish_stop()
            self.start_recovery(lost_reason)
            return None

        observation_age = self._elapsed(self.last_marker_time)

        if observation_age > self.marker_lost_timeout_sec:
            self.publish_stop()
            self.start_recovery(lost_reason)
            return None

        if observation_age > self.stale_stop_timeout_sec:
            self.publish_stop()
            self._throttled_info(
                f'waiting for marker reacquisition, '
                f'age={observation_age:.2f}s'
            )
            return None

        return self.latest_observation

    def _get_bool_parameter(self, name: str) -> bool:
        value = self.get_parameter(name).value

        if isinstance(value, bool):
            return value

        if isinstance(value, str):
            return value.lower() in ['true', '1', 'yes', 'on']

        return bool(value)

    def _create_aruco_detector(self, dictionary_name: str):
        if not hasattr(cv2, 'aruco'):
            raise RuntimeError('cv2.aruco 모듈이 없습니다. OpenCV 설치 상태를 확인하세요.')

        if not hasattr(cv2.aruco, dictionary_name):
            valid_names = [name for name in dir(cv2.aruco) if name.startswith('DICT_')]
            raise RuntimeError(
                f'지원하지 않는 ArUco dictionary: {dictionary_name}. '
                f'사용 가능 예: {valid_names[:10]}'
            )

        aruco_dict = cv2.aruco.getPredefinedDictionary(
            getattr(cv2.aruco, dictionary_name)
        )

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

        aruco_detector = None

        if hasattr(cv2.aruco, 'ArucoDetector'):
            aruco_detector = cv2.aruco.ArucoDetector(aruco_dict, aruco_params)

        return aruco_dict, aruco_params, aruco_detector

    def camera_info_callback(self, msg: CameraInfo) -> None:
        if self.camera_matrix is None:
            self.camera_matrix = np.array(msg.k, dtype=np.float64).reshape(3, 3)
            self.dist_coeffs = np.array(msg.d, dtype=np.float64)

            self.get_logger().info(
                'CameraInfo received. ArUco pose estimation is enabled.'
            )

    def battery_callback(self, msg: BatteryState) -> None:
        self.latest_battery = msg
        self.last_battery_time = self.get_clock().now()

    def image_callback(self, msg: CompressedImage) -> None:
        try:
            frame = self.bridge.compressed_imgmsg_to_cv2(msg, desired_encoding='bgr8')
        except CvBridgeError as exc:
            self.get_logger().warn(f'cv_bridge conversion failed: {exc}')
            return

        frame = cv2.flip(frame, -1)

        image_height, image_width = frame.shape[:2]
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

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

        selected_index = None
        observation = None

        if ids is not None and len(ids) > 0:
            ids_flat = ids.flatten().astype(int)
            selected_index = self._select_marker_index(ids_flat, corners)

            if selected_index is not None:
                observation = self._make_observation(
                    ids_flat,
                    corners,
                    selected_index,
                    image_width,
                    image_height,
                )

                if observation is not None:
                    self.latest_observation = observation
                    self.last_marker_time = self.get_clock().now()

        if self.enable_debug_image:
            debug_frame = self._draw_debug_image(
                frame,
                corners,
                ids,
                observation,
                selected_index,
            )

            try:
                debugout_msg = self.bridge.cv2_to_compressed_imgmsg(
                    debug_frame,
                    dst_format='jpg'
                )
                debugout_msg.header = msg.header

                self.debug_pub.publish(debugout_msg)
                
            except CvBridgeError as exc:
                self.get_logger().warn(f'debug image publish failed: {exc}')

    def _select_marker_index(self, ids_flat: np.ndarray, corners) -> Optional[int]:
        target_indices = np.where(ids_flat == self.target_marker_id)[0]

        if len(target_indices) > 0:
            return int(target_indices[0])

        if self.target_marker_id < 0:
            areas = [
                abs(cv2.contourArea(corner.reshape(4, 2).astype(np.float32)))
                for corner in corners
            ]
            return int(np.argmax(areas))

        return None

    def _make_observation(
        self,
        ids_flat: np.ndarray,
        corners,
        selected_index: int,
        image_width: int,
        image_height: int,
    ) -> Optional[ArucoObservation]:
        if self.camera_matrix is None or self.dist_coeffs is None:
            self._throttled_warn('CameraInfo is not available. Docking control is waiting.')
            return None

        try:
            rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
                [corners[selected_index]],
                self.marker_size_m,
                self.camera_matrix,
                self.dist_coeffs,
            )
        except Exception as exc:
            self._throttled_warn(f'ArUco pose estimation failed: {exc}')
            return None

        corner = corners[selected_index].reshape(4, 2)
        center = corner.mean(axis=0)
        area = abs(cv2.contourArea(corner.astype(np.float32)))

        tvec = tvecs[0][0]
        rvec = rvecs[0][0]

        x_m = float(tvec[0])
        y_m = float(tvec[1])
        z_m = float(tvec[2])

        bearing_rad = math.atan2(x_m, max(z_m, 1e-6))

        rotation_matrix, _ = cv2.Rodrigues(rvec)
        marker_normal = rotation_matrix[:, 2].astype(np.float64)

        if marker_normal[2] > 0.0:
            marker_normal = -marker_normal

        marker_normal_yaw_rad = math.atan2(
            float(marker_normal[0]),
            float(-marker_normal[2]),
        )

        marker_normal_yaw_rad = self._normalize_angle(marker_normal_yaw_rad)

        return ArucoObservation(
            marker_id=int(ids_flat[selected_index]),
            x_m=x_m,
            y_m=y_m,
            z_m=z_m,
            bearing_rad=bearing_rad,
            marker_normal_yaw_rad=marker_normal_yaw_rad,
            center_x=float(center[0]),
            center_y=float(center[1]),
            image_width=image_width,
            image_height=image_height,
            area=float(area),
        )

    def control_loop(self) -> None:
        self.publish_state()

        if self._elapsed(self.docking_start_time) > self.max_docking_time_sec:
            if self.state not in [DockState.DOCKED, DockState.FAILED]:
                self.transition_to(DockState.FAILED, 'max docking time exceeded')

        if self.state == DockState.DOCKED:
            self.publish_stop()
            self.publish_docked(True)
            return

        if self.state == DockState.FAILED:
            self.publish_stop()
            self.publish_docked(False)
            return

        if self.is_charging():
            self.transition_to(DockState.DOCKED, 'charging detected')
            self.publish_stop()
            self.publish_docked(True)
            return

        if self.state == DockState.SEARCH_MARKER:
            self.handle_search_marker()
        elif self.state == DockState.APPROACH_PRE_DOCK:
            self.handle_approach_pre_dock()
        elif self.state == DockState.ALIGN_DOCK_AXIS:
            self.handle_align_dock_axis()
        elif self.state == DockState.FINAL_APPROACH:
            self.handle_final_approach()
        elif self.state == DockState.CONTACT_PUSH:
            self.handle_contact_push()
        elif self.state == DockState.CHARGE_VERIFY:
            self.handle_charge_verify()
        elif self.state == DockState.RECOVERY_BACKUP:
            self.handle_recovery_backup()

    def handle_search_marker(self) -> None:
        obs = self.get_valid_observation()

        if obs is not None:
            self.transition_to(DockState.APPROACH_PRE_DOCK, 'marker acquired')
            return


        elapsed = self._elapsed(self.state_enter_time)

        if self.last_tracking_angular_z > 0.0:
            initial_direction = -1.0
        elif self.last_tracking_angular_z < 0.0:
            initial_direction = 1.0
        else:
            initial_direction = 1.0

        if elapsed < 2.0:
            direction = initial_direction
        else:
            search_phase = int((elapsed - 2.0) / 3.0)
            direction = (
                -initial_direction
                if search_phase % 2 == 0
                else initial_direction
            )

        angular_z = (
            direction * min(self.search_angular_speed_rps, 0.12)
        )

        self.publish_cmd(0.0, angular_z)

        self._throttled_info(
            f'SEARCH_MARKER: direction={direction:+.0f}, '
            f'angular_z={angular_z:+.3f}'
        )

    def handle_approach_pre_dock(self) -> None:
        obs = self.get_tracking_observation(
            'marker lost during pre-dock approach'
        )

        if obs is None:
            return

        distance_error = obs.z_m - self.pre_dock_distance_m

        if abs(distance_error) <= self.pre_dock_tolerance_m:
            self.transition_to(DockState.ALIGN_DOCK_AXIS, 'pre-dock distance reached')
            self.publish_stop()
            return

        linear_x = self.k_z * distance_error

        if linear_x > 0.0:
            linear_x = max(self.min_approach_speed_mps, linear_x)

        linear_x = self._clamp(
            linear_x,
            -self.max_reverse_speed_mps,
            self.max_approach_speed_mps,
        )

        angular_z = self.compute_approach_angular(obs)

        self.publish_cmd(linear_x, angular_z)

        self._throttled_info(
            f'APPROACH_PRE_DOCK: z={obs.z_m:.3f}, x={obs.x_m:.3f}, '
            f'bearing={obs.bearing_rad:.3f}, cmd=({linear_x:.3f}, {angular_z:.3f})'
        )

    def handle_align_dock_axis(self) -> None:

        obs = self.get_tracking_observation(
            'marker lost during dock-axis alignment'
        )

        if obs is None:
            return

        aligned = (
            abs(obs.x_m) <= self.final_lateral_limit_m
            and abs(obs.bearing_rad) <= self.final_bearing_limit_rad
        )

        if aligned:
            self.publish_stop()
            self.transition_to(
                DockState.FINAL_APPROACH,
                'dock axis aligned'
            )
            return

        angular_z = self.compute_approach_angular(obs)

        angular_z = self._clamp(
            angular_z,
            -0.08,
            0.08,
        )

        if abs(obs.bearing_rad) > 0.12:
            linear_x = 0.0
        else:
            linear_x = 0.012

        self.publish_cmd(linear_x, angular_z)

    def handle_final_approach(self) -> None:
        obs = self.get_tracking_observation(
            'marker lost during final approach'
        )

        if obs is None:
            return

        obs = self.get_valid_observation()

        if obs is None:
            self.start_recovery('marker lost during final approach')
            return

        if abs(obs.x_m) > self.final_lateral_limit_m:
            self.transition_to(DockState.ALIGN_DOCK_AXIS, 'final lateral error too large')
            self.publish_stop()
            return

        if abs(obs.bearing_rad) > self.final_bearing_limit_rad:
            self.transition_to(DockState.ALIGN_DOCK_AXIS, 'final bearing error too large')
            self.publish_stop()
            return

        if obs.z_m <= self.final_dock_distance_m:
            self.transition_to(DockState.CONTACT_PUSH, 'final dock distance reached')
            self.publish_stop()
            return

        angular_z = (
            -self.k_final_bearing * obs.bearing_rad
            -self.k_final_lateral * obs.x_m
        )

        angular_z = self._clamp(
            angular_z,
            -self.max_angular_speed_rps * 0.5,
            self.max_angular_speed_rps * 0.5,
        )

        self.publish_cmd(self.final_approach_speed_mps, angular_z)

        self._throttled_info(
            f'FINAL_APPROACH: z={obs.z_m:.3f}, x={obs.x_m:.3f}, '
            f'bearing={obs.bearing_rad:.3f}, cmd=({self.final_approach_speed_mps:.3f}, {angular_z:.3f})'
        )

    def handle_contact_push(self) -> None:
        if self.is_charging():
            self.transition_to(DockState.DOCKED, 'charging detected during contact push')
            self.publish_stop()
            self.publish_docked(True)
            return

        elapsed = self._elapsed(self.state_enter_time)

        if elapsed < self.contact_push_time_sec:
            self.publish_cmd(self.contact_push_speed_mps, 0.0)
            self._throttled_info(
                f'CONTACT_PUSH: gently pushing contacts, elapsed={elapsed:.2f}s'
            )
            return

        self.publish_stop()
        self.transition_to(DockState.CHARGE_VERIFY, 'contact push finished')

    def handle_charge_verify(self) -> None:
        self.publish_stop()

        if not self.use_battery_verify:
            self.transition_to(DockState.DOCKED, 'battery verification disabled')
            self.publish_docked(True)
            return

        if True:
        # if self.is_charging():
            self.transition_to(DockState.DOCKED, 'charging verified')
            self.publish_docked(True)
            return

        elapsed = self._elapsed(self.state_enter_time)

        if elapsed > self.charge_verify_timeout_sec:
            self.start_recovery('charging verification failed')
            return

        self._throttled_info(
            f'CHARGE_VERIFY: waiting for charging signal, elapsed={elapsed:.2f}s'
        )

    def handle_recovery_backup(self) -> None:
        elapsed = self._elapsed(self.state_enter_time)

        if elapsed < self.recovery_backup_time_sec:
            self.publish_cmd(-self.max_reverse_speed_mps, 0.0)
            self._throttled_info(
                f'RECOVERY_BACKUP: backing up, retry={self.retry_count}/{self.max_retry_count}'
            )
            return

        if self.retry_count >= self.max_retry_count:
            self.transition_to(DockState.FAILED, 'retry count exceeded')
            self.publish_stop()
            return

        self.transition_to(DockState.SEARCH_MARKER, 'recovery finished')

    def compute_approach_angular(self, obs: ArucoObservation) -> float:
        angular_z = -self.k_bearing * obs.bearing_rad
                    
        self.get_logger().info(
            f'x={obs.x_m:+.3f}, '
            f'z={obs.z_m:+.3f}, '
            f'bearing={obs.bearing_rad:+.3f}, '
            f'angular_z={angular_z:+.3f}'
        )

        return self._clamp(
            angular_z,
            -self.max_angular_speed_rps,
            self.max_angular_speed_rps,
        )

    def start_recovery(self, reason: str) -> None:
        self.retry_count += 1
        self.transition_to(DockState.RECOVERY_BACKUP, reason)

    def transition_to(self, new_state: DockState, reason: str = '') -> None:
        if self.state == new_state:
            return

        old_state = self.state
        self.state = new_state
        self.state_enter_time = self.get_clock().now()

        self.get_logger().info(
            f'STATE: {old_state.value} -> {new_state.value}. reason={reason}'
        )

    def get_valid_observation(self) -> Optional[ArucoObservation]:
        if self.latest_observation is None:
            return None

        if self._elapsed(self.last_marker_time) > self.marker_lost_timeout_sec:
            return None

        return self.latest_observation

    def is_charging(self) -> bool:
        if not self.use_battery_verify:
            return False

        if self.latest_battery is None:
            return False

        if self._elapsed(self.last_battery_time) > self.battery_timeout_sec:
            return False

        if self.latest_battery.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_CHARGING:
            return True

        current = self.latest_battery.current

        if not math.isnan(current) and current > self.charge_current_threshold_a:
            return True

        return False

    def publish_cmd(self, linear_x: float, angular_z: float) -> None:
        cmd = Twist()

        cmd.linear.x = self._clamp(
            linear_x,
            -self.max_reverse_speed_mps,
            self.max_approach_speed_mps,
        )

        cmd.angular.z = self._clamp(
            angular_z,
            -self.max_angular_speed_rps,
            self.max_angular_speed_rps,
        )

        if (
            self.state in [
                DockState.APPROACH_PRE_DOCK,
                DockState.ALIGN_DOCK_AXIS,
                DockState.FINAL_APPROACH,
            ]
            and abs(cmd.angular.z) > 0.01
        ):
            self.last_tracking_angular_z = cmd.angular.z

        self.cmd_pub.publish(cmd)

    def publish_stop(self) -> None:
        self.cmd_pub.publish(Twist())

    def publish_state(self) -> None:
        msg = String()
        msg.data = self.state.value
        self.state_pub.publish(msg)

    def publish_docked(self, docked: bool) -> None:
        msg = Bool()
        msg.data = docked
        self.docked_pub.publish(msg)

    def _draw_debug_image(
        self,
        frame,
        corners,
        ids,
        observation: Optional[ArucoObservation],
        selected_index: Optional[int],
    ):
        debug = frame.copy()

        if ids is not None and len(ids) > 0:
            cv2.aruco.drawDetectedMarkers(debug, corners, ids)

        h, w = debug.shape[:2]

        cv2.line(debug, (w // 2, 0), (w // 2, h), (255, 255, 255), 1)

        state_text = f'state={self.state.value}'
        cv2.putText(
            debug,
            state_text,
            (10, 25),
            cv2.FONT_HERSHEY_SIMPLEX,
            0.60,
            (255, 255, 0),
            2,
            cv2.LINE_AA,
        )

        if observation is not None:
            cx = int(observation.center_x)
            cy = int(observation.center_y)

            cv2.circle(debug, (cx, cy), 5, (0, 0, 255), -1)

            info_1 = (
                f'id={observation.marker_id} '
                f'z={observation.z_m:.2f}m '
                f'x={observation.x_m:.2f}m'
            )

            info_2 = (
                f'bearing={observation.bearing_rad:.2f} '
                f'retry={self.retry_count}/{self.max_retry_count}'
            )

            cv2.putText(
                debug,
                info_1,
                (10, 55),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.60,
                (0, 255, 0),
                2,
                cv2.LINE_AA,
            )

            cv2.putText(
                debug,
                info_2,
                (10, 85),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.60,
                (0, 255, 0),
                2,
                cv2.LINE_AA,
            )

            if (
                selected_index is not None
                and self.camera_matrix is not None
                and self.dist_coeffs is not None
            ):
                try:
                    rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
                        [corners[selected_index]],
                        self.marker_size_m,
                        self.camera_matrix,
                        self.dist_coeffs,
                    )

                    cv2.drawFrameAxes(
                        debug,
                        self.camera_matrix,
                        self.dist_coeffs,
                        rvecs[0],
                        tvecs[0],
                        self.marker_size_m * 0.5,
                    )
                except Exception:
                    pass
        else:
            cv2.putText(
                debug,
                'target marker not found',
                (10, 55),
                cv2.FONT_HERSHEY_SIMPLEX,
                0.60,
                (0, 0, 255),
                2,
                cv2.LINE_AA,
            )

        return debug

    def _elapsed(self, start_time) -> float:
        return (self.get_clock().now() - start_time).nanoseconds * 1e-9

    def _throttled_info(self, msg: str) -> None:
        if self._elapsed(self.last_log_time) >= self.log_throttle_sec:
            self.get_logger().info(msg)
            self.last_log_time = self.get_clock().now()

    def _throttled_warn(self, msg: str) -> None:
        if self._elapsed(self.last_log_time) >= self.log_throttle_sec:
            self.get_logger().warn(msg)
            self.last_log_time = self.get_clock().now()
    @staticmethod
    def _normalize_angle(angle: float) -> float:
        return math.atan2(math.sin(angle), math.cos(angle))
    
    @staticmethod
    def _clamp(value: float, low: float, high: float) -> float:
        return max(low, min(high, value))

    def destroy_node(self):
        for _ in range(5):
            self.publish_stop()

        return super().destroy_node()


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

    node = None

    try:
        node = ChargingDockNode()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        if node is not None:
            node.destroy_node()

        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()

실행 권한을 추가합니다.

chmod +x ~/turtlebot3_ws/src/tb3_charging_dock/tb3_charging_dock/charging_dock_node.py

9. 노드 소스 설명

1) import 영역 설명

먼저 기본 Python 모듈을 사용한다.

import math
from dataclasses import dataclass
from enum import Enum
from typing import Optional

math는 각도 계산에 사용한다.
dataclass는 ArUco 관측 정보를 구조체처럼 저장하기 위해 사용한다.
Enum은 도킹 상태를 명확하게 정의하기 위해 사용한다.
Optional은 값이 있을 수도 있고 없을 수도 있는 변수를 표현할 때 사용한다.

OpenCV와 NumPy도 사용한다.

import cv2
import numpy as np

cv2는 ArUco 검출, 영상 변환, 디버그 이미지 표시 등에 사용된다.
numpy는 카메라 행렬과 왜곡 계수, 좌표 계산에 사용된다.

ROS 2 관련 모듈은 다음과 같다.

import rclpy
from rclpy.duration import Duration
from rclpy.node import Node

rclpy는 ROS 2 Python 노드를 만들기 위한 라이브러리다.
Node는 ROS 2 노드 클래스를 만들 때 상속받는다.
Duration은 상태 경과 시간, 마커 분실 시간, 배터리 메시지 timeout 등을 계산할 때 사용한다.

메시지 타입은 다음을 사용한다.

from geometry_msgs.msg import Twist
from sensor_msgs.msg import BatteryState, CameraInfo, Image, CompressedImage
from std_msgs.msg import Bool, String

각 메시지의 역할은 다음과 같다.

Image
  카메라 영상 수신

CompressedImage
JPEG, PNG로 압축된  카메라 영상 수신

CameraInfo
  카메라 내부 파라미터 수신

Twist
  /cmd_vel 속도 명령 발행

BatteryState
  충전 상태 확인

String
  현재 도킹 상태 발행

Bool
  도킹 완료 여부 발행

2) DockState 상태머신

소스에서는 도킹 상태를 다음과 같이 정의한다.

class DockState(Enum):
    SEARCH_MARKER = 'SEARCH_MARKER'
    APPROACH_PRE_DOCK = 'APPROACH_PRE_DOCK'
    ALIGN_DOCK_AXIS = 'ALIGN_DOCK_AXIS'
    FINAL_APPROACH = 'FINAL_APPROACH'
    CONTACT_PUSH = 'CONTACT_PUSH'
    CHARGE_VERIFY = 'CHARGE_VERIFY'
    DOCKED = 'DOCKED'
    RECOVERY_BACKUP = 'RECOVERY_BACKUP'
    FAILED = 'FAILED'

각 상태의 의미는 다음과 같다.

SEARCH_MARKER는 충전스테이션의 ArUco 마커를 찾는 상태다.
마커가 보이지 않으면 로봇이 제자리에서 천천히 회전한다.

APPROACH_PRE_DOCK은 충전스테이션 바로 앞까지 가지 않고, 먼저 진입 기준 위치까지 접근하는 상태다.

ALIGN_DOCK_AXIS는 로봇이 충전스테이션의 정면축과 맞도록 정렬하는 상태다.

FINAL_APPROACH는 정렬이 끝난 뒤 매우 낮은 속도로 충전스테이션에 접근하는 상태다.

CONTACT_PUSH는 최종 거리까지 접근한 뒤 충전 접점이 안정적으로 닿도록 아주 낮은 속도로 잠깐 밀어 넣는 상태다.

CHARGE_VERIFY는 실제 충전이 시작되었는지 확인하는 상태다.

DOCKED는 도킹이 완료된 상태다.

RECOVERY_BACKUP은 도킹 중 문제가 발생했을 때 잠시 후진하는 복구 상태다.

FAILED는 최대 재시도 횟수 초과 또는 제한 시간 초과로 도킹에 실패한 상태다.

이처럼 상태를 나누면 각 단계에서 해야 할 동작이 명확해진다.
실제 로봇 도킹에서는 이런 상태머신 구조가 단순 추종 방식보다 훨씬 안정적이다.

정상적인 상태 전환은 다음과 같습니다.

SEARCH_MARKER
    ↓
APPROACH_PRE_DOCK
    ↓
ALIGN_DOCK_AXIS
    ↓
FINAL_APPROACH
    ↓
CONTACT_PUSH
    ↓
CHARGE_VERIFY
    ↓
DOCKED

문제가 발생하면 다음 복구 흐름을 수행합니다.

마커 손실 또는 충전 실패
    ↓
RECOVERY_BACKUP
    ↓
SEARCH_MARKER

재시도 횟수를 초과하면 FAILED 상태로 전환합니다.

3) ArucoObservation 데이터 클래스

ArUco 마커의 관측 정보는 ArucoObservation에 저장한다.

@dataclass
class ArucoObservation:
    marker_id: int
    x_m: float
    y_m: float
    z_m: float
    bearing_rad: float
    marker_normal_yaw_rad: float
    center_x: float
    center_y: float
    image_width: int
    image_height: int
    area: float
marker_id

검출한 ArUco 마커 ID입니다.

x_m

카메라 좌표계에서 마커의 좌우 위치입니다. 일반적인 OpenCV 카메라 좌표계에서는 오른쪽 방향이 양수입니다.

y_m

카메라 좌표계에서 마커의 수직 위치입니다.

z_m

카메라에서 마커까지의 전방 거리입니다. 자동 도킹에서 가장 중요한 거리 값입니다.

bearing_rad

카메라 정면과 마커 사이의 방위각입니다.

marker_normal_yaw_rad

마커 평면의 법선 방향으로부터 계산한 마커의 회전 각도입니다.

center_x, center_y

영상에서 마커 중심의 픽셀 좌표입니다.

image_width, image_height

카메라 영상의 크기입니다.

area

영상에서 마커가 차지하는 픽셀 면적입니다.

현재 제어에는 직접 사용되지 않지만 마커 신뢰도 평가나 여러 마커 중 가장 가까운 마커를 선택할 때 활용할 수 있습니다.

4) ChargingDockNode 클래스 초기화

노드 클래스는 다음과 같이 시작한다.

class ChargingDockNode(Node):
    def __init__(self) -> None:
        super().__init__('charging_dock_node')

노드 이름은 charging_dock_node이다.

ROS 2에서 실행 중인 노드는 다음 명령으로 확인할 수 있다.

ros2 node list

정상 실행되면 다음과 비슷하게 표시된다.

/charging_dock_node

5) 주요 파라미터 설명

이 노드는 많은 파라미터를 사용한다.
실제 로봇에서는 카메라 위치, 마커 크기, 충전스테이션 구조, 바닥 상태에 따라 튜닝이 필요하기 때문이다.

토픽 관련 파라미터
self.declare_parameter('image_topic', '/camera/image_raw/compressed')
self.declare_parameter('camera_info_topic', '/camera/camera_info')
self.declare_parameter('cmd_vel_topic', '/cmd_vel')
self.declare_parameter('battery_topic', '/battery_state')
self.declare_parameter(
    'debug_image_topic',
    '/charging_dock/debug_image/compressed'
)
self.declare_parameter('state_topic', '/charging_dock/state')
self.declare_parameter('docked_topic', '/charging_dock/docked')
image_topic

압축 카메라 영상 토픽입니다.

camera_info_topic

카메라 내부 파라미터 토픽입니다.

cmd_vel_topic

이동로봇 속도 명령 토픽입니다.

battery_topic

배터리 상태 토픽입니다.

debug_image_topic

ArUco 검출 결과를 표시한 디버그 영상 토픽입니다.

state_topic

현재 도킹 상태를 발행하는 토픽입니다.

docked_topic

도킹 성공 여부를 발행하는 토픽입니다.

ArUco 관련 파라미터

self.declare_parameter('aruco_dictionary', 'DICT_4X4_50')
self.declare_parameter('target_marker_id', 0)
self.declare_parameter('marker_size_m', 0.10)
aruco_dictionary

사용할 ArUco Dictionary입니다.

기본값은 DICT_4X4_50입니다.

target_marker_id

충전 스테이션에 설치된 목표 마커 ID입니다.

기본값은 0입니다.

marker_size_m

실제 ArUco 마커 한 변의 길이입니다.

기본값 0.10은 10cm를 의미합니다.

이 값이 실제 마커 크기와 다르면 계산되는 거리도 틀어집니다.

예를 들어 실제 마커가 15cm인데 코드에는 10cm로 설정되어 있으면 계산된 거리가 실제보다 작게 나올 수 있습니다.

도킹 거리 파라미터

self.declare_parameter('pre_dock_distance_m', 0.70)
self.declare_parameter('pre_dock_tolerance_m', 0.06)
self.declare_parameter('final_dock_distance_m', 0.18)
pre_dock_distance_m

1차 접근 목표 거리입니다.

기본값은 0.7m입니다.

pre_dock_tolerance_m

1차 접근 거리의 허용 오차입니다.

목표 거리가 0.7m이고 허용 오차가 0.06m이면 약 0.64m에서 0.76m 사이에서 접근 완료로 판단합니다.

final_dock_distance_m

최종 접근 종료 거리입니다.

마커까지의 거리가 0.18m 이하가 되면 CONTACT_PUSH 상태로 전환합니다.

카메라 위치와 충전 단자 위치가 다르므로 실제 로봇 구조에 맞게 조정해야 합니다.

동작 흐름은 다음과 같다.

마커 탐색
    ↓
마커 기준 약 0.70m 위치까지 접근
    ↓
도킹축 정렬
    ↓
마커 기준 약 0.18m 위치까지 저속 접근
    ↓
접촉 밀어넣기

정렬 오차 파라미터

self.declare_parameter('lateral_tolerance_m', 0.035)
self.declare_parameter('bearing_tolerance_rad', 0.060)
self.declare_parameter('final_lateral_limit_m', 0.060)
self.declare_parameter('final_bearing_limit_rad', 0.100)
final_lateral_limit_m

최종 접근을 허용하는 좌우 오차 한계입니다.

기본값 0.06m는 6cm입니다.

final_bearing_limit_rad

최종 접근을 허용하는 방향 오차 한계입니다.

기본값 0.1rad는 약 5.73도입니다.

lateral_tolerance_m

현재 코드에서는 선언과 로딩만 되어 있고 실제 상태 전환에는 사용되지 않습니다.

bearing_tolerance_rad

이 파라미터도 현재 제어 로직에서는 사용되지 않습니다.

속도 파라미터

self.declare_parameter('control_rate_hz', 10.0)
self.declare_parameter('max_approach_speed_mps', 0.070)
self.declare_parameter('min_approach_speed_mps', 0.018)
self.declare_parameter('final_approach_speed_mps', 0.025)
self.declare_parameter('contact_push_speed_mps', 0.010)
self.declare_parameter('max_angular_speed_rps', 0.80)
self.declare_parameter('max_reverse_speed_mps', 0.040)
control_rate_hz

제어 루프 실행 주기입니다.

기본값은 10Hz입니다.

max_approach_speed_mps

1차 접근 최대 전진 속도입니다.

min_approach_speed_mps

모터의 정지 마찰을 극복하기 위한 최소 전진 속도입니다.

final_approach_speed_mps

최종 접근 단계에서 사용하는 고정 전진 속도입니다.

contact_push_speed_mps

충전 접점을 밀착시킬 때 사용하는 저속 전진 속도입니다.

max_angular_speed_rps

최대 회전 속도입니다.

max_reverse_speed_mps

복구 동작에서 사용하는 최대 후진 속도입니다.

제어 게인

self.declare_parameter('k_z', 0.45)
self.declare_parameter('k_bearing', 1.80)
self.declare_parameter('k_lateral', 1.40)
self.declare_parameter('k_final_bearing', 1.20)
self.declare_parameter('k_final_lateral', 0.90)
self.declare_parameter('k_normal_yaw', 0.30)
k_z

거리 오차를 선속도로 변환하는 비례 게인입니다.

k_bearing

방위각 오차를 각속도로 변환하는 비례 게인입니다.

k_lateral

좌우 위치 오차 제어용 게인이지만 현재 접근 제어에서는 사용되지 않습니다.

k_final_bearing

최종 접근 단계에서 방위각 오차를 보정합니다.

k_final_lateral

최종 접근 단계에서 좌우 위치 오차를 보정합니다.

k_normal_yaw

마커 법선 방향 보정용 게인이지만 현재 제어식에는 적용되지 않습니다.

시간과 재시도 파라미터

self.declare_parameter('search_angular_speed_rps', 0.28)
self.declare_parameter('marker_lost_timeout_sec', 0.70)
self.declare_parameter('stale_stop_timeout_sec', 0.20)
self.declare_parameter('contact_push_time_sec', 1.20)
self.declare_parameter('charge_verify_timeout_sec', 6.0)
self.declare_parameter('recovery_backup_time_sec', 1.20)
self.declare_parameter('max_retry_count', 3)
self.declare_parameter('max_docking_time_sec', 90.0)
search_angular_speed_rps

마커 탐색 회전 속도입니다.

marker_lost_timeout_sec

마커를 완전히 잃었다고 판단하는 시간입니다.

stale_stop_timeout_sec

마커 데이터가 오래됐다고 판단해 일시 정지하는 시간입니다.

contact_push_time_sec

충전 접점을 밀어주는 시간입니다.

charge_verify_timeout_sec

충전 시작 신호를 기다리는 최대 시간입니다.

recovery_backup_time_sec

복구 과정에서 후진하는 시간입니다.

max_retry_count

최대 도킹 재시도 횟수입니다.

max_docking_time_sec

전체 도킹 제한 시간입니다.

배터리 확인 파라미

self.declare_parameter('use_battery_verify', False)
self.declare_parameter('charge_current_threshold_a', 0.05)
self.declare_parameter('battery_timeout_sec', 2.0)
use_battery_verify

실제 배터리 충전 여부를 확인할지 결정합니다.

기본값은 False입니다.

charge_current_threshold_a

충전 중이라고 판단할 전류 임계값입니다.

battery_timeout_sec

배터리 메시지의 최대 유효 시간입니다.

6) Boolean 파라미터 처리

def _get_bool_parameter(self, name: str) -> bool:
    value = self.get_parameter(name).value

    if isinstance(value, bool):
        return value

    if isinstance(value, str):
        return value.lower() in ['true', '1', 'yes', 'on']

    return bool(value)

이 함수는 파라미터가 Boolean 형식이나 문자열 형식으로 전달되는 경우를 모두 처리합니다.

다음 문자열은 모두 참으로 처리됩니다.

true
1
yes
on

YAML 파일에서는 가능하면 다음처럼 실제 Boolean 값을 사용하는 것이 좋습니다.

use_battery_verify: true
enable_debug_image: true

7) 노드 내부 변수 초기화

self.bridge = CvBridge()

CvBridge는 ROS 이미지 메시지와 OpenCV 영상 사이를 변환합니다.

카메라 보정 정보는 처음에 None으로 설정됩니다.

self.camera_matrix = None
self.dist_coeffs = None

마커 관측값과 배터리 정보도 초기에는 존재하지 않습니다.

self.latest_observation = None
self.latest_battery = None

마지막 수신 시간은 현재 시간보다 999초 전으로 초기화합니다.

self.last_marker_time = (
    self.get_clock().now() - Duration(seconds=999.0)
)

이를 통해 노드 시작 직후 오래된 데이터가 유효한 데이터로 처리되는 것을 방지합니다.

초기 상태는 마커 탐색입니다.

self.state = DockState.SEARCH_MARKER

8) QoS 설정

카메라와 CameraInfo 구독에는 센서 데이터용 QoS가 적용됩니다.

sensor_qos = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    durability=DurabilityPolicy.VOLATILE,
    history=HistoryPolicy.KEEP_LAST,
    depth=1,
)
BEST_EFFORT

일부 프레임이 유실되더라도 최신 데이터를 빠르게 전달하는 것을 우선합니다.

VOLATILE

구독자가 연결되기 전의 메시지는 저장하지 않습니다.

KEEP_LAST

최근 메시지만 유지합니다.

depth=1

가장 최신 메시지 한 개만 보관합니다.

영상 처리에서는 오래된 프레임을 순서대로 처리하는 것보다 최신 프레임을 즉시 처리하는 것이 중요하므로 적절한 설정입니다.

9) Subscriber 구성

압축 카메라 영상
self.image_sub = self.create_subscription(
    CompressedImage,
    self.image_topic,
    self.image_callback,
    sensor_qos,
)
카메라 정보
self.camera_info_sub = self.create_subscription(
    CameraInfo,
    self.camera_info_topic,
    self.camera_info_callback,
    sensor_qos,
)
배터리 상태
self.battery_sub = self.create_subscription(
    BatteryState,
    self.battery_topic,
    self.battery_callback,
    10,
)

10) Publisher 구성

로봇 속도 명령
self.cmd_pub = self.create_publisher(
    Twist,
    self.cmd_vel_topic,
    10
)
디버그 영상
self.debug_pub = self.create_publisher(
    CompressedImage,
    self.debug_image_topic,
    sensor_qos
)
현재 도킹 상태
self.state_pub = self.create_publisher(
    String,
    self.state_topic,
    10
)
도킹 성공 여부
self.docked_pub = self.create_publisher(
    Bool,
    self.docked_topic,
    10
)

11) 제어 타이머

timer_period = 1.0 / max(self.control_rate_hz, 0.5)
self.control_timer = self.create_timer(
    timer_period,
    self.control_loop
)

기본 제어 주파수가 10Hz이므로 제어 루프는 0.1초마다 실행됩니다.

max()를 사용한 이유는 control_rate_hz가 0 또는 너무 작은 값으로 설정됐을 때 0으로 나누는 오류를 방지하기 위해서입니다.

영상 콜백은 마커 관측값을 갱신하고, 실제 속도 제어는 타이머 콜백에서 수행합니다.

이처럼 영상 처리와 제어 처리를 분리한 구조는 유지보수 측면에서 유리합니다.

12) ArUco 검출기 생성

def _create_aruco_detector(self, dictionary_name: str):

먼저 OpenCV에 ArUco 모듈이 존재하는지 확인합니다.

if not hasattr(cv2, 'aruco'):
    raise RuntimeError(
        'cv2.aruco 모듈이 없습니다. OpenCV 설치 상태를 확인하세요.'
    )

cv2.aruco가 없다면 일반 OpenCV가 아니라 contrib 모듈이 포함된 OpenCV 설치가 필요할 수 있습니다.

Dictionary 이름도 검사합니다.

if not hasattr(cv2.aruco, dictionary_name):

정상적인 Dictionary라면 다음 코드로 생성합니다.

aruco_dict = cv2.aruco.getPredefinedDictionary(
    getattr(cv2.aruco, dictionary_name)
)

OpenCV 버전에 따라 검출 파라미터 생성 방식이 다르므로 두 방식을 모두 지원합니다.

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

최신 OpenCV에서는 ArucoDetector 객체를 사용합니다.

if hasattr(cv2.aruco, 'ArucoDetector'):
    aruco_detector = cv2.aruco.ArucoDetector(
        aruco_dict,
        aruco_params
    )

구버전에서는 기존 cv2.aruco.detectMarkers()를 사용합니다.

13) CameraInfo 처리

def camera_info_callback(self, msg: CameraInfo) -> None:

카메라 내부 행렬은 다음 코드로 저장됩니다.

self.camera_matrix = np.array(
    msg.k,
    dtype=np.float64
).reshape(3, 3)

일반적인 카메라 내부 행렬은 다음 형태입니다.

fx   0  cx
 0  fy  cy
 0   0   1

fx, fy는 초점 거리이고 cx, cy는 영상 중심점입니다.

렌즈 왜곡 계수는 다음 코드로 저장합니다.

self.dist_coeffs = np.array(
    msg.d,
    dtype=np.float64
)

현재 코드는 첫 번째 CameraInfo 메시지만 저장합니다.

if self.camera_matrix is None:

카메라 해상도나 캘리브레이션 값이 실행 중 변경되지 않는 환경에서는 문제가 없습니다. 하지만 카메라 설정이 동적으로 변경되는 시스템에서는 매번 갱신하도록 수정해야 합니다.

14) 배터리 콜백

def battery_callback(self, msg: BatteryState) -> None:
    self.latest_battery = msg
    self.last_battery_time = self.get_clock().now()

최신 배터리 메시지와 수신 시간을 저장합니다.

수신 시간을 함께 저장하는 이유는 오래된 배터리 상태를 현재 상태로 잘못 판단하지 않기 위해서입니다.

15) 카메라 영상 변환

압축 영상은 다음 코드로 OpenCV 영상으로 변환합니다.

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

변환에 실패하면 경고 로그를 출력하고 해당 프레임 처리를 종료합니다.

except CvBridgeError as exc:
    self.get_logger().warn(
        f'cv_bridge conversion failed: {exc}'
    )
    return

Leave a Comment