Devin.KR
통합 검색 Ctrl K

turtlesim 미니 프로젝트 - 경로 따라 움직이기

개발자KR 조회 9

이 장에서 배우는 것

좌표와 타임스탬프를 다룬 앞 장에서는 두리가 자기 위치와 시간을 표현하는 법을 살펴봤다. 이번 장에서는 그 위치 정보를 실제로 움직임에 연결한다. turtlesim은 실제 두리 하드웨어 없이도 "위치를 구독하고, 오차를 계산하고, 속도를 발행한다"는 제어 루프의 뼈대를 검증할 수 있는 가장 가벼운 무대다. 토픽 발행·구독을 다룬 장에서 배운 내용을 그대로 활용해, 목표 좌표까지 스스로 이동하는 노드를 완성한다.

  • turtlesim_node가 발행·구독하는 토픽의 이름과 메시지 타입을 정리한다
  • 하나의 노드 안에서 발행과 구독을 함께 쓰는 구조를 만든다
  • 거리 오차와 각도 오차에 비례하는 속도를 계산하는 비례 제어를 구현한다
  • 목표 도달을 판정하고 로봇을 실제로 멈추는 처리를 한다
  • ROS 없이도 같은 제어 원리를 순수 파이썬으로 확인한다

문제 상황

두리 팀의 다음 목표는 "창고 입구에서 선반 앞까지 좌표만 주면 알아서 이동하기"다. 지금까지는 teleop_twist_keyboard로 키를 눌러 수동으로 움직이는 것만 확인했다. 팀장이 "두리가 스스로 목적지까지 갈 수 있어?"라고 물었을 때, 아직 자동 이동 로직은 코드로 존재하지 않는다. 실제 로봇에 바로 새 제어 로직을 올리기는 부담스럽다. 바퀴 두 개짜리 시뮬레이터인 turtlesim에서 먼저 검증하면, 로직의 뼈대가 맞는지 빠르게 확인할 수 있다. 이 장에서는 turtlesim의 거북이를 목표 좌표까지 이동시키는 노드를 만들어, 다음 장에서 실제 로봇이나 다른 시뮬레이터로 옮길 준비를 한다.

turtlesim 노드가 주고받는 토픽

ros2 run turtlesim turtlesim_node로 실행되는 turtlesim_node는 창 하나에 거북이 한 마리를 띄우고, 두 개의 토픽으로 외부와 통신한다. /turtle1/cmd_vel은 geometry_msgs/msg/Twist 타입으로, 이 노드가 구독해서 거북이를 움직이는 데 쓴다. /turtle1/pose는 turtlesim/msg/Pose 타입으로, x, y, theta(방향), linear_velocity, angular_velocity 다섯 개 필드를 담아 turtlesim_node가 발행한다. 우리가 만들 노드는 이 두 토픽에 대해 turtlesim_node와 정반대 역할을 맡는다. /turtle1/pose를 구독해서 현재 위치를 받고, /turtle1/cmd_vel을 발행해서 속도 명령을 보낸다.

공식 문서에서 메시지 필드를 다시 확인하고 싶다면 ROS 2 Jazzy 공식 문서를 참고한다.

비례 제어로 목표점에 다가가기

목표점까지 이동하는 문제는 두 가지 오차로 나뉜다. 하나는 거리 오차(현재 위치와 목표점 사이의 직선 거리)이고, 다른 하나는 각도 오차(현재 방향과 목표점을 바라보는 방향의 차이)다. 비례 제어(P 제어)는 이 오차에 상수를 곱해서 그대로 명령 속도로 쓰는 방식이다. 오차가 크면 빠르게, 오차가 작으면 천천히 움직이므로 목표에 가까워질수록 자연스럽게 속도가 줄어든다.

두리가 목표점을 향하도록 거리 오차와 각도 오차를 함께 계산해야 방향과 속도를 동시에 맞출 수 있다

목표를 바라보는 각도는 atan2(목표y - 현재y, 목표x - 현재x)로 구한다. 여기서 현재 방향(theta)을 빼면 각도 오차가 나오는데, 이 값을 그대로 쓰면 문제가 생긴다.

각도 오차 정규화

theta는 -π에서 π 사이 값으로 순환한다. 예를 들어 theta가 3.0 라디안이고 목표 방향이 -3.0 라디안이면, 단순히 빼면 -6.0에 가까운 값이 나와 거의 한 바퀴를 도는 것처럼 보인다. 실제로 두 방향 사이의 최단 회전은 2π - 6.0, 즉 약 0.28 라디안에 불과하다. 이 문제를 피하려면 오차를 atan2(sin(오차), cos(오차))에 다시 통과시켜 -π에서 π 사이로 정규화한다.

완성 코드

go_to_goal.py (ROS 노드)

import math

import rclpy
from geometry_msgs.msg import Twist
from rclpy.node import Node
from turtlesim.msg import Pose

GOAL_X = 9.0
GOAL_Y = 9.0
DISTANCE_TOLERANCE = 0.15
KP_LINEAR = 1.2
KP_ANGULAR = 4.0
MAX_LINEAR_SPEED = 2.0


class GoToGoalNode(Node):

    def __init__(self):
        super().__init__('go_to_goal')
        self.pose = None
        self.goal_reached = False
        self.cmd_pub = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
        self.pose_sub = self.create_subscription(
            Pose, '/turtle1/pose', self.on_pose, 10)
        self.timer = self.create_timer(0.1, self.control_loop)

    def on_pose(self, msg):
        self.pose = msg

    def control_loop(self):
        if self.pose is None or self.goal_reached:
            return

        dx = GOAL_X - self.pose.x
        dy = GOAL_Y - self.pose.y
        distance = math.sqrt(dx * dx + dy * dy)

        cmd = Twist()

        if distance < DISTANCE_TOLERANCE:
            self.cmd_pub.publish(cmd)
            self.goal_reached = True
            self.get_logger().info(
                f'목표점에 도착했다: 남은 거리 {distance:.3f}')
            return

        angle_to_goal = math.atan2(dy, dx)
        angle_error = angle_to_goal - self.pose.theta
        angle_error = math.atan2(
            math.sin(angle_error), math.cos(angle_error))

        cmd.linear.x = min(KP_LINEAR * distance, MAX_LINEAR_SPEED)
        cmd.angular.z = KP_ANGULAR * angle_error

        self.cmd_pub.publish(cmd)
        self.get_logger().info(
            f'거리 {distance:.2f}, 각도오차 {angle_error:.2f}, '
            f'선속도 {cmd.linear.x:.2f}, 각속도 {cmd.angular.z:.2f}')


def main(args=None):
    rclpy.init(args=args)
    node = GoToGoalNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.cmd_pub.publish(Twist())
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

pose_control_sim.py (ROS 없이 실행하는 보조 예제)

class Topic:
    def __init__(self, name):
        self.name = name
        self.subscribers = []

    def subscribe(self, callback):
        self.subscribers.append(callback)

    def publish(self, message):
        for callback in self.subscribers:
            callback(message)


def run_simulation():
    pose_topic = Topic('/turtle1/pose')
    cmd_topic = Topic('/turtle1/cmd_vel')

    goal_x = 10.0
    tolerance = 0.05
    kp = 0.5
    position = [0.0]
    step_count = [0]

    def on_cmd_vel(speed):
        position[0] += speed
        pose_topic.publish(position[0])

    def on_pose(x):
        distance = goal_x - x
        print(f'step={step_count[0]} x={x:.7f} distance={distance:.7f}')
        if distance < tolerance:
            print('목표 도달')
            return
        step_count[0] += 1
        speed = kp * distance
        cmd_topic.publish(speed)

    cmd_topic.subscribe(on_cmd_vel)
    pose_topic.subscribe(on_pose)

    pose_topic.publish(position[0])


if __name__ == '__main__':
    run_simulation()

줄별 해설

go_to_goal.py — GOAL_X, GOAL_Y는 목표 좌표, DISTANCE_TOLERANCE는 도착으로 인정할 거리, KP_LINEAR와 KP_ANGULAR는 각각 거리·각도 오차에 곱할 비례 상수다. on_pose는 /turtle1/pose 메시지를 받을 때마다 최신 위치를 self.pose에 저장만 하고, 실제 계산은 0.1초마다 도는 control_loop에서 한다. 위치를 아직 못 받았거나 이미 도착했으면 바로 반환한다. 도착 판정에 걸리면 빈 Twist()(모든 값이 0)를 발행해 실제로 멈추고 goal_reached를 세워 이후 호출을 막는다. 그렇지 않으면 atan2로 목표 방향을 구하고, 정규화한 각도 오차와 거리에 비례 상수를 곱해 cmd.linear.x와 cmd.angular.z를 채운다. min(...)으로 선속도 상한을 둔 이유는 아래 "실무에서 자주 틀리는 것"에서 다룬다.

pose_control_sim.py — Topic 클래스는 rclpy의 발행·구독 구조를 아주 단순하게 흉내 낸 것으로, 구독자 목록을 들고 있다가 publish가 호출되면 모두에게 메시지를 전달한다. on_cmd_vel은 turtlesim_node 대신 위치를 갱신하고 새 위치를 pose_topic에 발행하는 "가짜 turtlesim" 역할이다. on_pose는 거리를 계산해 출력하고, 도착 전이면 비례 제어로 속도를 계산해 cmd_topic에 발행한다. 마지막 줄 pose_topic.publish(position[0])가 최초의 위치 메시지를 흘려보내면서 구독-계산-발행-이동이 반복되는 연쇄가 시작된다.

위치 구독과 속도 발행이 0.1초 주기로 반복되면서 오차가 줄어들수록 명령 속도도 함께 줄어든다

실행 결과

터미널 하나에 turtlesim을 띄운다.

$ ros2 run turtlesim turtlesim_node

다른 터미널에서 노드를 실행하면, 거북이는 보통 (5.54, 5.54) 부근에서 시작해서 (9, 9)를 향해 방향을 튼 다음 서서히 속도를 줄이며 다가간다. 로그는 대략 이렇게 보인다.

$ ros2 run doori_turtle_demo go_to_goal
[INFO] [go_to_goal]: 거리 4.89, 각도오차 0.79, 선속도 2.00, 각속도 3.14
[INFO] [go_to_goal]: 거리 3.95, 각도오차 0.41, 선속도 2.00, 각속도 1.63
[INFO] [go_to_goal]: 거리 2.87, 각도오차 0.18, 선속도 2.00, 각속도 0.71
[INFO] [go_to_goal]: 거리 1.42, 각도오차 0.05, 선속도 1.71, 각속도 0.19
[INFO] [go_to_goal]: 거리 0.31, 각도오차 0.01, 선속도 0.37, 각속도 0.03
[INFO] [go_to_goal]: 목표점에 도착했다: 남은 거리 0.092

ROS 없이 순수 파이썬 보조 예제를 돌리면 값이 정확히 재현된다.

$ python3 pose_control_sim.py
step=0 x=0.0000000 distance=10.0000000
step=1 x=5.0000000 distance=5.0000000
step=2 x=7.5000000 distance=2.5000000
step=3 x=8.7500000 distance=1.2500000
step=4 x=9.3750000 distance=0.6250000
step=5 x=9.6875000 distance=0.3125000
step=6 x=9.8437500 distance=0.1562500
step=7 x=9.9218750 distance=0.0781250
step=8 x=9.9609375 distance=0.0390625
목표 도달
비례 제어에서는 거리가 매 주기 절반 가까이 줄어들어 목표 근처에서 매우 작은 값으로 수렴한다

실무에서 자주 틀리는 것

cmd_vel을 한 번만 발행하고 spin에 맡기기

오차와 무관하게 속도를 한 번만 보내면 turtlesim_node는 새 메시지가 올 때까지 마지막 속도를 계속 쓰기 때문에, 목표에 가까워져도 속도가 줄지 않고 지나쳐 버린다.

cmd = Twist()
cmd.linear.x = 2.0
self.cmd_pub.publish(cmd)
rclpy.spin(self)

self.timer = self.create_timer(0.1, self.control_loop)

각도 오차를 정규화하지 않기

theta가 -π/π 경계를 넘나드는 순간 단순 뺄셈 결과가 실제 최단 회전량보다 훨씬 커져서 로봇이 불필요하게 크게 회전한다.

angle_error = angle_to_goal - self.pose.theta
cmd.angular.z = KP_ANGULAR * angle_error

angle_error = angle_to_goal - self.pose.theta
angle_error = math.atan2(math.sin(angle_error), math.cos(angle_error))
cmd.angular.z = KP_ANGULAR * angle_error

도착 후 정지 명령을 보내지 않기

플래그만 세우고 반환하면 turtlesim_node는 직전에 받은 속도를 계속 유지하므로 거북이가 도착 판정 이후에도 계속 움직인다.

if distance < DISTANCE_TOLERANCE:
    self.goal_reached = True
    return

if distance < DISTANCE_TOLERANCE:
    self.cmd_pub.publish(Twist())
    self.goal_reached = True
    return

선속도에 상한을 두지 않기

거리가 클 때 비례 상수를 그대로 곱하면 지나치게 빠른 속도가 나가 목표를 훌쩍 지나쳐 반대편에서 다시 진동하게 된다.

cmd.linear.x = KP_LINEAR * distance

cmd.linear.x = min(KP_LINEAR * distance, MAX_LINEAR_SPEED)

한눈에 보기

turtlesim 노드가 쓰는 토픽
토픽 이름메시지 타입이 노드 기준 방향역할
/turtle1/cmd_velgeometry_msgs/msg/Twist발행선속도·각속도 명령 전달
/turtle1/poseturtlesim/msg/Pose구독현재 x, y, theta 수신
비례 제어에 쓰인 값
이름의미코드에서의 역할조정 시 영향
KP_LINEAR거리 오차에 곱하는 비례 상수선속도 계산크게 하면 빠르게 접근하지만 지나칠 위험이 커진다
KP_ANGULAR각도 오차에 곱하는 비례 상수각속도 계산크게 하면 방향을 빨리 맞추지만 떨림이 커진다
DISTANCE_TOLERANCE도착으로 인정하는 최소 거리정지 판정너무 작으면 도착 판정이 안 나고 떨림이 지속된다
MAX_LINEAR_SPEED선속도 상한속도 clamp없으면 먼 거리에서 과속으로 목표를 지나친다

연습 문제

  1. DISTANCE_TOLERANCE를 0.01로 낮추면 어떤 현상이 나타날 가능성이 높은지 이유와 함께 설명하라.
  2. angle_error를 atan2(sin, cos)로 정규화하지 않으면 어떤 상황에서 문제가 되는지, theta가 3.0 라디안이고 목표 방향이 -3.0 라디안인 경우를 예로 들어 설명하라.
  3. MAX_LINEAR_SPEED를 제거하면 초기 거리가 클 때 어떤 문제가 생기는지, 그리고 KP_LINEAR를 줄이는 것과 MAX_LINEAR_SPEED로 clamp하는 것의 차이를 설명하라.
  4. pose_control_sim.py에서 kp 값을 0.5에서 0.9로 바꾸면 목표 도달까지 필요한 step 수가 어떻게 달라지는지, 그리고 kp가 1.0 이상이 되면 어떤 위험이 있는지 서술하라.

정답과 해설

1. 도착 판정 범위가 너무 좁아지면 로봇이 그 안에 정확히 멈추기 어려워, 목표 근처에서 미세하게 속도 명령을 주고받는 진동이 이어질 수 있다. 비례 제어는 오차를 완전히 0으로 만들기 어렵기 때문에 적당한 허용 범위가 필요하다.

2. 단순히 빼면 -3.0 - 3.0 = -6.0에 가까운 값이 나와 거의 한 바퀴를 도는 것처럼 보인다. 하지만 두 방향 사이의 실제 최단 회전은 2π - 6.0, 즉 약 0.28 라디안에 불과하다. 정규화하지 않으면 로봇이 불필요하게 큰 회전 명령을 받아 반대 방향으로 크게 돈다.

3. clamp가 없으면 초기 거리(예: 8~9 정도)에 KP_LINEAR(1.2)를 곱한 큰 속도가 그대로 나가 목표를 지나칠 수 있다. KP_LINEAR를 줄이면 먼 거리에서도 느려지지만 가까운 거리에서도 똑같이 느려져 전체 응답이 둔해진다. MAX_LINEAR_SPEED로 clamp하면 먼 거리에서만 속도를 제한하고, 가까운 거리에서는 비례 제어가 그대로 작동해 정교하게 감속한다.

4. kp가 커지면 매 단계 거리가 (1 - kp)배로 줄어드는 폭이 커져서, 더 적은 step 만에 tolerance 아래로 내려간다. 다만 이 1차원 모델에서 kp가 1.0을 넘으면 (1 - kp)가 음수가 되어 거리가 목표를 지나쳐 반대편으로 넘어가고, 다음 step에서 다시 반대로 넘어가는 진동이 나타날 수 있다. 실제 2D 제어에서도 KP_LINEAR가 지나치게 크면 같은 종류의 오버슈트가 발생한다.

댓글 0

아직 댓글이 없습니다. 첫 댓글을 남겨 보세요.

댓글을 남기려면 로그인이 필요합니다.