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

by

16) 영상 반전 처리

frame = cv2.flip(frame, -1)

flipCode=-1은 영상을 상하좌우 모두 반전합니다.

즉, 영상을 180도 회전한 것과 같습니다.

이 코드는 카메라가 로봇에 거꾸로 장착된 환경을 전제로 합니다.

카메라 영상이 정상 방향이라면 이 코드를 제거해야 합니다.

실제 시스템에서는 다음과 같이 파라미터로 만드는 것이 좋습니다.

self.declare_parameter('flip_image', True)
if self.flip_image:
    frame = cv2.flip(frame, -1)

17) ArUco 마커 검출

먼저 컬러 영상을 흑백 영상으로 변환합니다.

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

최신 OpenCV에서는 다음 코드가 실행됩니다.

corners, ids, _ = (
    self.aruco_detector.detectMarkers(gray)
)

구버전에서는 다음 코드가 실행됩니다.

corners, ids, _ = cv2.aruco.detectMarkers(
    gray,
    self.aruco_dict,
    parameters=self.aruco_params,
)

corners에는 각 마커의 네 꼭짓점 좌표가 들어갑니다.

ids에는 검출된 마커 ID가 들어갑니다.

세 번째 반환값은 마커로 최종 판정되지 않은 후보 영역입니다.

18) 목표 마커 선택

def _select_marker_index(
    self,
    ids_flat: np.ndarray,
    corners
) -> Optional[int]:

설정된 목표 ID와 일치하는 마커를 찾습니다.

target_indices = np.where(
    ids_flat == self.target_marker_id
)[0]

일치하는 마커가 있으면 첫 번째 마커를 선택합니다.

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

target_marker_id를 음수로 설정하면 특정 ID를 사용하지 않고 가장 큰 마커를 선택합니다.

if self.target_marker_id < 0:

마커 면적은 다음 코드로 계산합니다.

areas = [
    abs(
        cv2.contourArea(
            corner.reshape(4, 2)
            .astype(np.float32)
        )
    )
    for corner in corners
]

가장 큰 마커는 일반적으로 카메라에 가장 가까운 마커일 가능성이 높습니다.

하지만 실사용 환경에서는 충전 스테이션 전용 ID를 지정하는 것이 안전합니다.

19) ArUco 마커 자세 추정

카메라 내부 파라미터가 없으면 자세를 계산할 수 없습니다.

if self.camera_matrix is None or self.dist_coeffs is None:
    return None

자세 추정은 다음 코드로 수행합니다.

rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
    [corners[selected_index]],
    self.marker_size_m,
    self.camera_matrix,
    self.dist_coeffs,
)

rvecs는 회전 벡터입니다.

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

marker_size_m를 미터 단위로 입력하므로 tvecs도 미터 단위로 계산됩니다.

20) 마커 중심과 면적 계산

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

마커 중심은 네 꼭짓점 픽셀 좌표의 평균입니다.

마커 면적은 OpenCV의 contourArea()로 계산합니다.

21) 카메라 좌표계에서의 마커 위치

tvec = tvecs[0][0]

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

일반적인 OpenCV 카메라 좌표계는 다음과 같습니다.

X축: 영상 오른쪽
Y축: 영상 아래쪽
Z축: 카메라 전방

따라서 x_m이 양수이면 마커가 카메라 오른쪽에 있고, 음수이면 왼쪽에 있습니다.

z_m은 카메라와 마커 사이의 전방 거리입니다.

22) 방위각 계산

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

방위각은 카메라 정면을 기준으로 마커가 좌우로 얼마나 벗어났는지 나타냅니다.

예를 들어 다음과 같은 위치라고 가정합니다.

x = 0.10m
z = 1.00m

방위각은 다음과 같습니다.

atan2(0.10, 1.00)
≈ 0.0997rad
≈ 5.71도

마커가 오른쪽에 있으므로 양수 값이 계산됩니다.

max(z_m, 1e-6)는 z_m이 0에 가까울 때 계산이 불안정해지는 것을 방지합니다.

23) 마커 법선 방향 계산

회전 벡터를 회전 행렬로 변환합니다.

rotation_matrix, _ = cv2.Rodrigues(rvec)

회전 행렬의 세 번째 열을 마커 평면의 법선 벡터로 사용합니다.

marker_normal = rotation_matrix[:, 2].astype(
    np.float64
)

법선 방향이 반대로 계산된 경우 방향을 뒤집습니다.

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

마커 법선의 Yaw 각도는 다음과 같이 계산합니다.

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
)

현재 코드에서는 이 값을 계산하고 저장하지만 실제 로봇 제어에는 사용하지 않습니다.

24) 마커 관측값 갱신

정상적인 관측값이 생성되면 최신 마커 정보와 시간을 저장합니다.

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

현재 프레임에서 마커를 놓쳤다고 해서 기존 관측값을 즉시 삭제하지는 않습니다.

대신 마지막 검출 시간과 현재 시간의 차이를 계산해 데이터가 유효한지 판단합니다.

이 방식은 한두 프레임 정도 마커 검출이 끊겨도 즉시 복구 동작으로 들어가지 않게 해줍니다.

25) 마커 데이터 유효성 검사

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
)

관측값이 marker_lost_timeout_sec보다 오래됐으면 마커를 완전히 잃었다고 판단합니다.

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

관측값이 stale_stop_timeout_sec보다 오래됐지만 완전 손실 시간에는 도달하지 않았다면 일단 정지합니다.

if observation_age > self.stale_stop_timeout_sec:
    self.publish_stop()
    return None

기본값을 적용하면 다음과 같이 동작합니다.

  1. 0초에서 0.2초

최신 마커 데이터를 사용합니다.

  1. 0.2초에서 0.7초

로봇을 정지하고 마커 재검출을 기다립니다.

  1. 0.7초 이상

마커 손실로 판단하고 후진 복구를 시작합니다.

자동 도킹에서는 과거 마커 정보를 이용해 계속 전진하지 않도록 하는 것이 중요합니다.

26) 메인 제어 루프

def control_loop(self) -> None:

제어 루프가 실행되면 먼저 현재 상태를 발행합니다.

self.publish_state()

전체 도킹 시간이 제한 시간을 초과하면 실패 상태로 전환합니다.

if (
    self._elapsed(self.docking_start_time)
    > self.max_docking_time_sec
):
    self.transition_to(
        DockState.FAILED,
        'max docking time exceeded'
    )

DOCKED 상태에서는 정지 명령과 도킹 성공 메시지를 계속 발행합니다.

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

FAILED 상태에서도 로봇이 움직이지 않도록 정지 명령을 계속 발행합니다.

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

도킹 중 실제 충전이 감지되면 즉시 성공 상태로 전환합니다.

if self.is_charging():
    self.transition_to(
        DockState.DOCKED,
        'charging detected'
    )

27) 마커 탐색 상태

def handle_search_marker(self) -> None:

유효한 마커가 이미 존재하면 1차 접근 상태로 전환합니다.

obs = self.get_valid_observation()

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

마커가 없으면 로봇을 회전시킵니다.

마지막 추적 회전 방향의 반대 방향을 초기 탐색 방향으로 선택합니다.

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

처음 2초 동안은 같은 방향으로 회전합니다.

이후에는 3초 간격으로 회전 방향을 바꿉니다.

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

탐색 각속도는 다음과 같이 계산됩니다.

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

여기서 중요한 점은 search_angular_speed_rps 기본값이 0.28rad/s이지만 코드가 실제 속도를 최대 0.12rad/s로 제한한다는 것입니다.

따라서 파라미터를 0.28로 설정해도 실제 탐색 속도는 0.12rad/s입니다.

28) 1차 접근 상태

def handle_approach_pre_dock(self) -> None:

먼저 유효한 마커 관측값을 가져옵니다.

obs = self.get_tracking_observation(
    'marker lost during pre-dock approach'
)

거리 오차는 다음과 같이 계산합니다.

distance_error = (
    obs.z_m
    - self.pre_dock_distance_m
)

현재 거리가 1.2m이고 목표 거리가 0.7m라면 거리 오차는 0.5m입니다.

거리 오차가 허용 범위 안에 들어오면 정렬 상태로 전환합니다.

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

선속도는 비례 제어로 계산합니다.

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,
)

29) 접근 회전 제어

def compute_approach_angular(
    self,
    obs: ArucoObservation
) -> float:

회전 속도는 다음 비례 제어식으로 계산합니다.

angular_z = (
    -self.k_bearing
    * obs.bearing_rad
)

마커가 오른쪽에 있으면 bearing_rad는 양수입니다.

각속도는 음수가 되므로 ROS 2 일반 좌표계 기준으로 로봇이 시계 방향, 즉 오른쪽으로 회전합니다.

계산된 각속도는 최대 회전 속도로 제한합니다.

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

현재 접근 회전 제어에서는 bearing_rad만 사용합니다.

x_m, marker_normal_yaw_rad, k_lateral, k_normal_yaw는 사용되지 않습니다.

30) 도킹 축 정렬 상태

def handle_align_dock_axis(self) -> None:

정렬 완료 조건은 다음과 같습니다.

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'
    )

정렬 상태에서는 각속도를 매우 낮게 제한합니다.

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

방위각 오차가 크면 제자리 회전만 수행합니다.

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

방위각 오차가 작아지면 0.012m/s로 천천히 전진하면서 정렬합니다.

else:
    linear_x = 0.012

31) 최종 접근 상태

def handle_final_approach(self) -> None:

먼저 마커 관측값을 검사합니다.

obs = self.get_tracking_observation(
    'marker lost during final approach'
)

그 직후 get_valid_observation()을 다시 호출합니다.

obs = self.get_valid_observation()

첫 번째 함수가 정상 관측값을 반환했다면 두 번째 호출은 사실상 중복입니다.

최종 접근 중 좌우 오차가 커지면 다시 정렬 상태로 돌아갑니다.

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

방위각 오차가 커져도 정렬 상태로 돌아갑니다.

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

최종 거리에 도달하면 접점 밀착 상태로 전환합니다.

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

32) 최종 접근 회전 제어

최종 접근에서는 방위각 오차와 좌우 위치 오차를 동시에 사용합니다.

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

1차 접근보다 더 정밀한 제어 방식입니다.

각속도는 최대 각속도의 절반으로 제한됩니다.

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
)

33) 접점 밀착 상태

def handle_contact_push(self) -> None:

접점 밀착 중 충전이 감지되면 즉시 정지하고 도킹 성공 상태로 전환합니다.

if self.is_charging():
    self.transition_to(
        DockState.DOCKED,
        'charging detected during contact push'
    )

충전이 아직 감지되지 않으면 매우 낮은 속도로 일정 시간 전진합니다.

if elapsed < self.contact_push_time_sec:
    self.publish_cmd(
        self.contact_push_speed_mps,
        0.0
    )

기본 설정에서는 다음과 같이 동작합니다.

접점 밀착 속도: 0.01m/s
접점 밀착 시간: 1.2초

이론적인 추가 이동 거리는 다음과 같습니다.

0.01m/s × 1.2초 = 0.012m

약 1.2cm를 추가로 전진합니다.

밀착 시간이 끝나면 충전 확인 상태로 전환합니다.

34) 충전 확인 상태의 문제점

def handle_charge_verify(self) -> None:

배터리 확인 기능이 비활성화되어 있으면 바로 도킹 성공으로 처리합니다.

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

하지만 배터리 확인 기능이 활성화된 경우에도 현재 소스는 실제 충전 여부를 검사하지 않습니다.

문제의 코드는 다음과 같습니다.

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

if True는 항상 참이므로 실제 충전이 시작되지 않았어도 무조건 도킹 성공으로 처리됩니다.

실사용 전 반드시 다음과 같이 수정해야 합니다.

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

이 부분은 현재 소스에서 가장 중요한 수정 사항입니다.

35) 충전 상태 판단

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

여기서 주의할 점은 배터리 드라이버마다 충전 전류의 부호 규칙이 다를 수 있다는 것입니다.

어떤 드라이버는 충전 전류를 양수로 표현하고, 어떤 드라이버는 음수로 표현합니다.

현재 코드는 양수 충전 전류만 인식합니다.

실제 사용 중인 배터리 드라이버의 전류 부호를 반드시 확인해야 합니다.

36) 복구 후진 상태

def handle_recovery_backup(self) -> None:

복구 상태에서는 일정 시간 동안 최대 후진 속도로 이동합니다.

if elapsed < self.recovery_backup_time_sec:
    self.publish_cmd(
        -self.max_reverse_speed_mps,
        0.0
    )

기본 설정에서는 다음과 같습니다.

후진 속도: 0.04m/s
후진 시간: 1.2초

이론적인 후진 거리는 다음과 같습니다.

0.04m/s × 1.2초 = 0.048m

약 4.8cm 후진합니다.

후진이 끝난 후 재시도 횟수를 확인합니다.

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

재시도 횟수가 남아 있으면 다시 마커 탐색 상태로 전환합니다.

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

37) 상태 전환 함수

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} '
    f'-> {new_state.value}. '
    f'reason={reason}'
)

상태 전환 이유를 로그로 남기는 것은 실제 도킹 실패 원인을 분석할 때 유용합니다.

38) 속도 명령 발행

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

선속도는 허용 범위로 제한합니다.

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

이 값은 마커를 잃었을 때 초기 탐색 방향을 정하는 데 사용됩니다.

39) 정지 명령

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

기본값으로 생성된 Twist 메시지는 모든 속도 값이 0입니다.

따라서 다음과 같은 정지 명령이 발행됩니다.

linear.x = 0.0
angular.z = 0.0

40) 상태와 도킹 결과 발행

현재 상태는 문자열로 발행합니다.

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

도킹 결과는 Boolean 값으로 발행합니다.

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

터미널에서는 다음 명령으로 확인할 수 있습니다.

ros2 topic echo /charging_dock/state
ros2 topic echo /charging_dock/docked

41) 디버그 영상 생성

디버그 영상에는 검출된 마커 외곽선이 표시됩니다.

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

영상 중앙에는 세로 기준선을 표시합니다.

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

현재 상태도 영상에 표시합니다.

state_text = f'state={self.state.value}'

마커가 검출되면 다음 정보가 표시됩니다.

  1. 마커 ID
  2. 전방 거리 z
  3. 좌우 위치 x
  4. 방위각
  5. 현재 재시도 횟수

마커 중심에는 빨간색 점을 표시합니다.

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

카메라 보정 정보가 있으면 마커 좌표축도 표시합니다.

cv2.drawFrameAxes(
    debug,
    self.camera_matrix,
    self.dist_coeffs,
    rvecs[0],
    tvecs[0],
    self.marker_size_m * 0.5,
)

42) 디버그 영상 발행

OpenCV 영상은 다시 압축 ROS 메시지로 변환합니다.

debugout_msg = (
    self.bridge.cv2_to_compressed_imgmsg(
        debug_frame,
        dst_format='jpg'
    )
)

원본 메시지의 헤더를 그대로 복사합니다.

debugout_msg.header = msg.header

디버그 영상은 다음 토픽으로 발행됩니다.

/charging_dock/debug_image/compressed

확인할 때는 rqt_image_view를 사용할 수 있습니다.

ros2 run rqt_image_view rqt_image_view

43) 로그 출력 제한

반복되는 로그가 너무 많이 출력되지 않도록 로그 간격을 제한합니다.

def _throttled_info(self, msg: str) -> None:
def _throttled_warn(self, msg: str) -> None:

기본적으로 1초마다 한 번만 로그가 출력됩니다.

if (
    self._elapsed(self.last_log_time)
    >= self.log_throttle_sec
):

현재 정보 로그와 경고 로그가 하나의 last_log_time을 공유합니다.

정보 로그가 출력된 직후 중요한 경고가 발생하면 경고가 바로 출력되지 않을 수 있습니다.

다음처럼 로그 시간을 분리하는 것이 좋습니다.

self.last_info_log_time
self.last_warn_log_time

44) 보조 함수

경과 시간 계산
def _elapsed(self, start_time) -> float:
    return (
        self.get_clock().now() - start_time
    ).nanoseconds * 1e-9

ROS 2 시간 차이를 초 단위 실수로 변환합니다.

각도 정규화
@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)
    )

선속도와 각속도가 허용 범위를 넘지 않도록 제한합니다.

45) 노드 종료 처리

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

    return super().destroy_node()

노드를 종료할 때 정지 명령을 다섯 번 발행합니다.

통신 지연이나 일부 메시지 손실이 발생하더라도 이동로봇에 정지 명령이 전달될 가능성을 높이기 위한 처리입니다.

46) main 함수

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

ROS 2 통신을 초기화한 뒤 노드를 생성하고 실행합니다.

node = ChargingDockNode()
rclpy.spin(node)

Ctrl+C가 입력되면 실행을 종료합니다.

except KeyboardInterrupt:
    pass

마지막으로 노드를 제거하고 ROS 2를 종료합니다.

finally:
    if node is not None:
        node.destroy_node()

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

10. launch 파일 작성

파일을 엽니다.

touch ~/turtlebot3_ws/src/tb3_charging_dock/launch/charging_dock.launch.py

아래 내용을 입력합니다.

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    image_topic = LaunchConfiguration('image_topic')
    camera_info_topic = LaunchConfiguration('camera_info_topic')
    cmd_vel_topic = LaunchConfiguration('cmd_vel_topic')
    battery_topic = LaunchConfiguration('battery_topic')

    target_marker_id = LaunchConfiguration('target_marker_id')
    marker_size_m = LaunchConfiguration('marker_size_m')

    pre_dock_distance_m = LaunchConfiguration('pre_dock_distance_m')
    final_dock_distance_m = LaunchConfiguration('final_dock_distance_m')
    lateral_tolerance_m = LaunchConfiguration('lateral_tolerance_m')
    bearing_tolerance_rad = LaunchConfiguration('bearing_tolerance_rad')
    k_z = LaunchConfiguration('k_z')
    k_lateral = LaunchConfiguration('k_lateral')
    k_bearing = LaunchConfiguration('k_bearing')
    search_angular_speed_rps = LaunchConfiguration('search_angular_speed_rps')
    marker_lost_timeout_sec = LaunchConfiguration('marker_lost_timeout_sec')
    stale_stop_timeout_sec = LaunchConfiguration('stale_stop_timeout_sec')
        
    use_battery_verify = LaunchConfiguration('use_battery_verify')

    return LaunchDescription([
        DeclareLaunchArgument('image_topic', default_value='/camera/image_raw/compressed'),
        DeclareLaunchArgument('camera_info_topic', default_value='/camera/camera_info'),
        DeclareLaunchArgument('cmd_vel_topic', default_value='/cmd_vel'),
        DeclareLaunchArgument('battery_topic', default_value='/battery_state'),

        DeclareLaunchArgument('target_marker_id', default_value='0'),
        DeclareLaunchArgument('marker_size_m', default_value='0.10'),

        DeclareLaunchArgument('pre_dock_distance_m', default_value='0.50'),
        DeclareLaunchArgument('final_dock_distance_m', default_value='0.15'),
        DeclareLaunchArgument('lateral_tolerance_m', default_value='0.060'),
        DeclareLaunchArgument('bearing_tolerance_rad', default_value='0.100'),
        DeclareLaunchArgument('k_z', default_value='0.20'),
        DeclareLaunchArgument('k_lateral', default_value='0.30'),
        DeclareLaunchArgument('k_bearing', default_value='0.80'),
        DeclareLaunchArgument('search_angular_speed_rps', default_value='0.10'),
        DeclareLaunchArgument('marker_lost_timeout_sec', default_value='0.80'),
        DeclareLaunchArgument('stale_stop_timeout_sec', default_value='0.15'),
        DeclareLaunchArgument('use_battery_verify', default_value='false'),

        Node(
            package='tb3_charging_dock',
            executable='charging_dock_node',
            name='charging_dock_node',
            output='screen',
            parameters=[{
                'image_topic': image_topic,
                'camera_info_topic': camera_info_topic,
                'cmd_vel_topic': cmd_vel_topic,
                'battery_topic': battery_topic,

                'target_marker_id': ParameterValue(target_marker_id, value_type=int),
                'marker_size_m': ParameterValue(marker_size_m, value_type=float),

                'pre_dock_distance_m': ParameterValue(pre_dock_distance_m, value_type=float),
                'final_dock_distance_m': ParameterValue(final_dock_distance_m, value_type=float),
                'lateral_tolerance_m': ParameterValue(lateral_tolerance_m, value_type=float),
                'bearing_tolerance_rad': ParameterValue(bearing_tolerance_rad, value_type=float),
                'k_z': ParameterValue(k_z, value_type=float),
                'k_lateral': ParameterValue(k_lateral, value_type=float),
                'k_bearing': ParameterValue(k_bearing, value_type=float),
                'search_angular_speed_rps': ParameterValue(search_angular_speed_rps, value_type=float),
                'marker_lost_timeout_sec': ParameterValue(marker_lost_timeout_sec, value_type=float),
                'stale_stop_timeout_sec': ParameterValue(stale_stop_timeout_sec, value_type=float),
                'use_battery_verify': ParameterValue(use_battery_verify, value_type=bool),
            }],
        ),
    ])

실제 충전 검증이 가능한 로봇이라면 실행 시 다음처럼 설정합니다.

use_battery_verify:=true

충전 상태 토픽이 없거나 아직 하드웨어가 준비되지 않았다면 실습에서는 다음 기본값을 사용합니다.

use_battery_verify:=false

단, 실제 충전스테이션 구현에서는 false로 두면 안 됩니다.
실제 시스템에서는 충전 여부를 반드시 확인해야 합니다.

12. package.xml 작성

파일을 엽니다.

아래 내용으로 수정합니다.

<?xml version="1.0"?>
<package format="3">
  <name>tb3_charging_dock</name>
  <version>0.1.0</version>
  <description>TurtleBot3 charging station parking and docking node using ArUco marker.</description>
  <maintainer email="user@example.com">dragon</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_python</buildtool_depend>

  <exec_depend>rclpy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>launch</exec_depend>
  <exec_depend>launch_ros</exec_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>

중요한 의존성은 다음과 같습니다.

<exec_depend>sensor_msgs</exec_depend>

Image, CameraInfo, BatteryState 메시지를 사용하기 위해 필요합니다.

<exec_depend>geometry_msgs</exec_depend>

Twist 메시지를 사용하기 위해 필요합니다.

<exec_depend>std_msgs</exec_depend>

도킹 상태와 도킹 완료 여부를 발행하기 위해 필요합니다.

<exec_depend>cv_bridge</exec_depend>

ROS 이미지와 OpenCV 이미지를 변환하기 위해 필요합니다.

13. 빌드

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

cd ~/turtlebot3_ws

빌드합니다.

colcon build --packages-select tb3_charging_dock

환경 설정을 적용합니다.

source install/setup.bash

14. 실행 전 확인

TurtleBot3 bringup을 먼저 실행합니다.

export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_bringup robot.launch.py

카메라 노드도 실행되어 있어야 합니다.
이미 rqt에서 Pi Camera 2 영상이 보인다면 카메라 노드는 정상입니다.

필수 토픽을 확인합니다.

ros2 topic list

다음 토픽들이 있어야 합니다.

/cmd_vel
/camera/image_raw
/camera/camera_info

배터리 상태 토픽이 있다면 다음도 확인합니다.

ros2 topic echo /battery_state --once

충전 상태가 실제로 들어오는 시스템이라면 power_supply_status 또는 current 값이 충전 여부를 반영해야 합니다.

15. 실행

1) TurtleBot3 Bringup 실행

ros2 launch turtlebot3_bringup robot.launch.py

2) 카메라 실행

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

카메라 해상도는 640×480을 선택해야 정상적인 실행결과를 얻을 수 있습니다.

3) 토픽 확인

ros2 topic list

다음 토픽이 있는지 확인합니다.

/scan
/camera/image_raw
/cmd_vel

4) 영상 확인

원격 PC에서 ROS_DOMAIN_ID와 네트워크 설정이 맞다면 다음 명령으로 영상을 볼 수 있습니다.

rqt_image_view

/camera/image_raw를 선택합니다.

5) 노드 실행

기본 실행은 다음과 같습니다.

ros2 launch tb3_charging_dock charging_dock.launch.py

카메라 토픽 이름이 다르면 다음처럼 실행합니다.

ros2 launch tb3_charging_dock charging_dock.launch.py \
  image_topic:=/picam2/image_raw \
  camera_info_topic:=/picam2/camera_info

마커 크기와 ID를 지정해서 실행합니다.

ros2 launch tb3_charging_dock charging_dock.launch.py \
  target_marker_id:=0 \
  marker_size_m:=0.10

실제 충전 상태 확인을 사용하는 경우 다음처럼 실행합니다.

ros2 launch tb3_charging_dock charging_dock.launch.py \
  use_battery_verify:=true

충전 상태 확인 하드웨어가 아직 없다면 실습용으로 다음처럼 실행합니다.

ros2 launch tb3_charging_dock charging_dock.launch.py \
 target_marker_id:=0 \
  marker_size_m:=0.10
  use_battery_verify:=false

주의할 점은 명확합니다.

실제 충전스테이션에서는 use_battery_verify:=true로 두고 충전 여부를 반드시 확인해야 합니다.
false는 알고리즘 테스트용입니다.

16. 결과 확인

1) 도킹 상태 확인

도킹 상태 토픽을 확인합니다.

ros2 topic echo /charging_dock/state

정상 동작하면 다음 상태들이 순서대로 출력됩니다.

SEARCH_MARKER
APPROACH_PRE_DOCK
ALIGN_DOCK_AXIS
FINAL_APPROACH
CONTACT_PUSH
CHARGE_VERIFY
DOCKED

실패하거나 복구가 필요한 경우 다음 상태가 출력될 수 있습니다.

RECOVERY_BACKUP
FAILED

2) 도킹 완료 확인

도킹 완료 토픽을 확인합니다.

ros2 topic echo /charging_dock/docked

도킹이 완료되면 다음처럼 출력됩니다.

data: true

실패 상태이거나 도킹 전이면 다음처럼 출력됩니다.

data: false

3) cmd_vel 확인

속도 명령을 확인합니다.

ros2 topic echo /cmd_vel

각 단계별로 대략 다음과 같은 동작이 나타납니다.

SEARCH_MARKER:
  angular.z만 발생

APPROACH_PRE_DOCK:
  linear.x와 angular.z가 함께 발생

ALIGN_DOCK_AXIS:
  작은 linear.x 또는 angular.z 발생

FINAL_APPROACH:
  낮은 linear.x와 작은 angular.z 발생

CONTACT_PUSH:
  아주 낮은 linear.x 발생

DOCKED:
  모든 속도 0

4) 디버그 영상 확인

원격 PC에서 rqt를 실행합니다.

rqt

Plugins → Visualization → Image View를 선택합니다.

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

/charging_dock/debug_image

디버그 영상에서 확인할 수 있는 정보는 다음과 같습니다.

  1. 검출된 ArUco 마커 외곽선
  2. 화면 중앙선
  3. 현재 상태
  4. 마커 ID
  5. 마커까지 거리
  6. 좌우 오차
  7. bearing 오차
  8. retry count

17. 주요 파라미터 정리

주요 파라미터는 다음과 같습니다.

image_topic
  카메라 이미지 토픽

camera_info_topic
  카메라 내부 파라미터 토픽

cmd_vel_topic
  속도 명령 토픽

battery_topic
  배터리 상태 토픽

target_marker_id
  충전스테이션에 붙인 ArUco 마커 ID

marker_size_m
  실제 마커 한 변 길이

pre_dock_distance_m
  최종 진입 전 대기 거리

final_dock_distance_m
  접촉 밀어넣기 전 최종 거리

lateral_tolerance_m
  도킹축 정렬 시 허용 좌우 오차

bearing_tolerance_rad
  도킹축 정렬 시 허용 방향 오차

final_lateral_limit_m
  최종 진입 중 허용되는 최대 좌우 오차

final_bearing_limit_rad
  최종 진입 중 허용되는 최대 방향 오차

final_approach_speed_mps
  최종 진입 속도

contact_push_speed_mps
  접촉 밀어넣기 속도

contact_push_time_sec
  접촉 밀어넣기 시간

use_battery_verify
  충전 상태 확인 사용 여부

max_retry_count
  최대 재시도 횟수

초기 추천값은 다음과 같습니다.

marker_size_m: 0.10
pre_dock_distance_m: 0.50
final_dock_distance_m: 0.15

lateral_tolerance_m: 0.060
bearing_tolerance_rad: 0.100

final_lateral_limit_m: 0.060
final_bearing_limit_rad: 0.100

max_approach_speed_mps: 0.070
final_approach_speed_mps: 0.025
contact_push_speed_mps: 0.010

contact_push_time_sec: 1.20
max_retry_count: 3

k_z: 0.20
k_lateral: 0.30
k_bearing: 0.80
search_angular_speed_rps: 0.10
marker_lost_timeout_sec: 0.80
stale_stop_timeout_sec: 0.15

18. 튜닝 방법

1) 마커 거리 값이 틀린 경우

marker_size_m을 먼저 확인합니다.

실제 마커 한 변이 10cm이면 다음 값을 사용해야 합니다.

marker_size_m:=0.10

실제 마커가 15cm이면 다음 값을 사용합니다.

marker_size_m:=0.15

마커 크기가 틀리면 전체 도킹 거리가 틀어집니다.

2) 진입 위치가 너무 가까운 경우

pre_dock_distance_m을 늘립니다.

pre_dock_distance_m:=0.70

도킹축 정렬을 충분히 할 공간이 필요하면 이 값을 키우는 것이 좋습니다.

3) 최종 진입이 불안정한 경우

final_approach_speed_mps를 낮춥니다.

final_approach_speed_mps = 0.015

실제 충전스테이션에서는 마지막 접근 속도가 낮을수록 안정적입니다.

4) 접점이 잘 붙지 않는 경우

contact_push_time_sec를 조금 늘립니다.

contact_push_time_sec = 1.5

또는 contact_push_speed_mps를 조금 올릴 수 있습니다.

contact_push_speed_mps = 0.012

단, 무리하게 올리면 충전스테이션이 밀리거나 로봇 구조물에 충격이 생길 수 있습니다.

5) 충전 확인이 실패하는 경우

다음을 확인합니다.

  1. /battery_state 토픽이 실제로 들어오는지 확인합니다.
  2. power_supply_status가 충전 중일 때 바뀌는지 확인합니다.
  3. current 값이 충전 중일 때 양수로 증가하는지 확인합니다.
  4. charge_current_threshold_a가 너무 높지 않은지 확인합니다.
  5. 충전 접점이 물리적으로 닿는지 확인합니다.


Leave a Comment