TurtleBot3 Burger :AMCL 현재 좌표 취득 후 경유점 주행

이 글에서의 실습목표는 다음과 같습니다.

TurtleBot3 Burger에서 Navigation2를 실행한다.
AMCL이 추정하는 현재 로봇 좌표를 확인한다.
로봇을 원하는 위치로 이동시킨 뒤 현재 좌표를 경유점으로 저장한다.
저장된 경유점 YAML 파일을 기존 waypoint follower 코드에 적용한다.
목적지 도착 후 gTTS 음성 출력까지 연결한다.

전체 구조는 다음과 같습니다.

TurtleBot3 Burger 실제 로봇
        ↓
SLAM 또는 저장된 map 사용
        ↓
Nav2 + AMCL 실행
        ↓
/amcl_pose 토픽에서 현재 좌표 취득
        ↓
YAML 파일로 경유점 저장
        ↓
waypoint_follower_yaml.py 실행
        ↓
경유점 순차 주행
        ↓
목적지 도착 후 음성 출력

1. 패키지 생성

작업 공간으로 이동합니다.

mkdir -p ~/turtlebot3_ws/src
cd ~/turtlebot3_ws/src

Python 패키지를 생성합니다.

ros2 pkg create tb3_waypoint_nav \
  --build-type ament_python \
  --dependencies rclpy geometry_msgs nav2_simple_commander PyYAML

패키지 구조는 다음과 같이 구성합니다.

tb3_waypoint_nav/
├── package.xml
├── setup.py
├── resource/
│   └── tb3_waypoint_nav
├── tb3_waypoint_nav/
│   ├── __init__.py
│   └── waypoint_follower_yaml.py
└── waypoints/
    └── tb3_waypoints.yaml

waypoints 폴더를 생성합니다.

cd ~/turtlebot3_ws/src/tb3_waypoint_nav
mkdir waypoints

2. 로봇 실행

1) TurtleBot3 SBC에서 bringup 실행

TurtleBot3 Burger 본체에 SSH 접속합니다.

ssh sjyong@192.168.210.12

TurtleBot3 bringup을 실행합니다.

ros2 launch turtlebot3_bringup robot.launch.py

이 터미널은 계속 켜둡니다.

2) Remote PC에서 Navigation2 실행

Remote PC에서 저장된 map을 사용해 Nav2를 실행합니다.

예시:

export TURTLEBOT3_MODEL=burger

ros2 launch turtlebot3_navigation2 navigation2.launch.py \
  map:=$HOME/map.yaml

맵 파일 경로는 본인의 맵 위치에 맞게 수정해야 합니다.

예를 들어 맵이 ~/maps/lab_map.yaml에 있다면 다음과 같이 실행합니다.

ros2 launch turtlebot3_navigation2 navigation2.launch.py \
  map:=$HOME/maps/lab_map.yaml

3. AMCL 초기 위치 설정

Nav2를 실행하면 RViz가 열립니다.

실제 로봇의 현재 위치를 RViz 맵 위에 맞춰야 합니다.

1) RViz에서 초기 위치 지정

RViz 상단 메뉴에서 2D Pose Estimate를 클릭합니다.

그 다음 실제 TurtleBot3 Burger가 있는 위치를 맵에서 클릭하고, 로봇이 바라보는 방향으로 드래그합니다.

RViz → 2D Pose Estimate → 실제 로봇 위치 클릭 → 바라보는 방향으로 드래그

이 작업이 정확해야 AMCL 좌표가 제대로 나옵니다.

2) 라이다 스캔과 맵 정렬 확인

RViz에서 LaserScan 점들이 맵의 벽과 잘 겹치는지 확인합니다.

정렬이 맞으면 좋습니다.

LaserScan 점 ≈ 맵의 벽 위치

정렬이 틀어졌다면 다시 2D Pose Estimate를 사용해서 위치를 맞춥니다.

이 단계가 대충 되면 이후 경유점 주행도 거의 실패합니다.

실제 로봇에서는 이 부분이 제일 중요합니다.

3) AMCL 현재 좌표 확인 방법

AMCL이 정상 동작하면 /amcl_pose 토픽이 나옵니다.

토픽 목록을 확인합니다.

ros2 topic list | grep amcl

정상이라면 다음과 비슷하게 나옵니다.

/amcl_pose

현재 AMCL 좌표를 직접 확인하려면 다음 명령을 사용합니다.

ros2 topic echo /amcl_pose

출력 예시는 다음과 같습니다.

header:
  stamp:
    sec: 123
    nanosec: 456
  frame_id: map
pose:
  pose:
    position:
      x: 1.245
      y: -0.532
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.382
      w: 0.924

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

position:
  x: 1.245
  y: -0.532
orientation:
  z: 0.382
  w: 0.924

하지만 기존 경유점 YAML은 yaw 값을 사용합니다.

따라서 quaternion 값을 yaw 값으로 변환해야 합니다.

직접 계산할 수도 있지만, 실습에서는 좌표 저장 노드를 만들어 자동으로 x, y, yaw를 저장하는 방식이 훨씬 편합니다.

4. AMCL 좌표를 경유점 YAML로 저장하는 노드 추가

기존 패키지에 새 Python 파일을 하나 추가합니다.

파일 이름은 다음과 같이 하겠습니다.

amcl_waypoint_recorder.py

역할은 간단합니다.

/amcl_pose 토픽을 구독한다.
현재 로봇의 x, y, yaw를 계산한다.
사용자가 Enter를 누를 때마다 현재 위치를 경유점으로 저장한다.
종료할 때 YAML 파일로 저장한다.

5. amcl_waypoint_recorder.py 소스 코드

파일을 생성합니다.

touch ~/turtlebot3_ws/src/tb3_waypoint_nav/tb3_waypoint_nav/amcl_waypoint_recorder.py

아래 코드를 입력합니다.

#!/usr/bin/env python3

import argparse
import math
import sys
import threading
from pathlib import Path

import yaml

import rclpy
from rclpy.node import Node
from rclpy.utilities import remove_ros_args
from geometry_msgs.msg import PoseWithCovarianceStamped


def quaternion_to_yaw(q):
    """
    Quaternion 값을 yaw 값으로 변환한다.

    TurtleBot3 Burger는 2D 평면에서 움직이므로
    roll, pitch는 거의 사용하지 않고 yaw만 사용한다.
    """
    siny_cosp = 2.0 * ((q.w * q.z) + (q.x * q.y))
    cosy_cosp = 1.0 - 2.0 * ((q.y * q.y) + (q.z * q.z))

    yaw = math.atan2(siny_cosp, cosy_cosp)
    return yaw


class AmclWaypointRecorder(Node):
    def __init__(self, output_file: str, frame_id: str):
        super().__init__("amcl_waypoint_recorder")

        self.output_file = Path(output_file).expanduser()
        self.frame_id = frame_id

        self.current_pose = None
        self.waypoints = []
        self.running = True

        self.create_subscription(
            PoseWithCovarianceStamped,
            "/amcl_pose",
            self.amcl_pose_callback,
            10
        )

        self.get_logger().info("AMCL waypoint recorder started.")
        self.get_logger().info("Subscribing: /amcl_pose")
        self.get_logger().info(f"Output YAML: {self.output_file}")
        self.get_logger().info("Move robot to target position, then press Enter to save waypoint.")
        self.get_logger().info("Type q and press Enter to save file and quit.")

        self.input_thread = threading.Thread(target=self.keyboard_loop)
        self.input_thread.daemon = True
        self.input_thread.start()

    def amcl_pose_callback(self, msg: PoseWithCovarianceStamped):
        """
        /amcl_pose 토픽에서 현재 로봇 위치를 계속 갱신한다.
        """
        pose = msg.pose.pose

        x = pose.position.x
        y = pose.position.y
        yaw = quaternion_to_yaw(pose.orientation)

        self.current_pose = {
            "x": float(x),
            "y": float(y),
            "yaw": float(yaw)
        }

    def keyboard_loop(self):
        """
        Enter 입력을 받을 때마다 현재 AMCL 위치를 waypoint로 저장한다.
        """
        while self.running:
            user_input = input()

            if user_input.lower() == "q":
                self.running = False
                self.save_yaml()
                rclpy.shutdown()
                break

            self.save_current_pose_as_waypoint()

    def save_current_pose_as_waypoint(self):
        """
        현재 AMCL pose를 waypoints 리스트에 추가한다.
        """
        if self.current_pose is None:
            self.get_logger().warn("No AMCL pose received yet. Check /amcl_pose.")
            return

        index = len(self.waypoints) + 1
        waypoint_name = f"wp{index}"

        waypoint = {
            "name": waypoint_name,
            "x": round(self.current_pose["x"], 3),
            "y": round(self.current_pose["y"], 3),
            "yaw": round(self.current_pose["yaw"], 3)
        }

        self.waypoints.append(waypoint)

        self.get_logger().info(
            f"Saved {waypoint_name}: "
            f"x={waypoint['x']}, y={waypoint['y']}, yaw={waypoint['yaw']}"
        )

    def save_yaml(self):
        """
        저장된 waypoint 목록을 YAML 파일로 저장한다.
        """
        if len(self.waypoints) == 0:
            self.get_logger().warn("No waypoints saved. YAML file will not be created.")
            return

        yaml_data = {
            "frame_id": self.frame_id,
            "initial_pose": {
                "x": self.waypoints[0]["x"],
                "y": self.waypoints[0]["y"],
                "yaw": self.waypoints[0]["yaw"]
            },
            "waypoints": self.waypoints
        }

        self.output_file.parent.mkdir(parents=True, exist_ok=True)

        with open(self.output_file, "w", encoding="utf-8") as file:
            yaml.dump(
                yaml_data,
                file,
                allow_unicode=True,
                sort_keys=False,
                default_flow_style=False
            )

        self.get_logger().info(f"Saved waypoint YAML: {self.output_file}")


def parse_arguments():
    argv = remove_ros_args(args=sys.argv)[1:]

    parser = argparse.ArgumentParser(
        description="Record TurtleBot3 AMCL pose as waypoint YAML"
    )

    parser.add_argument(
        "--output",
        default="~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml",
        help="Output waypoint YAML file path"
    )

    parser.add_argument(
        "--frame-id",
        default="map",
        help="Frame ID for waypoint YAML"
    )

    return parser.parse_args(argv)


def main():
    args = parse_arguments()

    rclpy.init()

    node = AmclWaypointRecorder(
        output_file=args.output,
        frame_id=args.frame_id
    )

    try:
        rclpy.spin(node)

    except KeyboardInterrupt:
        node.get_logger().warn("Keyboard interrupt received.")
        node.save_yaml()

    finally:
        node.running = False
        node.destroy_node()

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


if __name__ == "__main__":
    main()

실행 권한을 줍니다.

chmod +x ~/turtlebot3_ws/src/tb3_waypoint_nav/tb3_waypoint_nav/amcl_waypoint_recorder.py

1) import 부분 설명

소스의 첫 부분은 필요한 Python 모듈과 ROS 2 메시지를 불러오는 부분입니다.

#!/usr/bin/env python3

import argparse
import math
import sys
import threading
from pathlib import Path

import yaml

import rclpy
from rclpy.node import Node
from rclpy.utilities import remove_ros_args
from geometry_msgs.msg import PoseWithCovarianceStamped

#!/usr/bin/env python3는 이 파일을 Python 3로 실행하겠다는 의미입니다. ROS 2 Python 노드에서는 일반적으로 이 구문을 파일 맨 위에 넣습니다.

argparse는 실행할 때 옵션을 받기 위해 사용합니다. 이 코드에서는 YAML 저장 경로와 frame id를 명령어 옵션으로 받을 수 있습니다.

예를 들면 다음과 같은 실행 명령에서 사용됩니다.

ros2 run tb3_waypoint_nav amcl_waypoint_recorder \
  --output ~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml \
  --frame-id map

math는 quaternion을 yaw로 변환할 때 atan2() 계산에 사용됩니다.

sys는 실행 인자를 처리하기 위해 사용합니다.

threading은 키보드 입력을 별도의 thread에서 처리하기 위해 사용합니다. ROS 2 노드는 /amcl_pose를 계속 구독해야 하고, 동시에 사용자의 Enter 입력도 받아야 합니다. 이 두 작업을 하나의 흐름에서 처리하면 입력 대기 때문에 ROS 2 callback 처리가 막힐 수 있습니다. 그래서 키보드 입력은 별도 thread로 분리합니다.

Path는 파일 경로를 안전하게 다루기 위해 사용합니다. 특히 ~ 경로를 실제 home 경로로 변환할 때 유용합니다.

yaml은 waypoint 목록을 YAML 파일로 저장하기 위해 사용합니다.

remove_ros_args는 ROS 2 실행 인자와 Python argparse 인자를 분리하기 위해 사용합니다.

PoseWithCovarianceStamped는 /amcl_pose 토픽의 메시지 타입입니다. AMCL은 로봇의 추정 위치를 이 메시지 타입으로 발행합니다.

2) quaternion_to_yaw 함수 설명

def quaternion_to_yaw(q):
    """
    Quaternion 값을 yaw 값으로 변환한다.

    TurtleBot3 Burger는 2D 평면에서 움직이므로
    roll, pitch는 거의 사용하지 않고 yaw만 사용한다.
    """
    siny_cosp = 2.0 * ((q.w * q.z) + (q.x * q.y))
    cosy_cosp = 1.0 - 2.0 * ((q.y * q.y) + (q.z * q.z))

    yaw = math.atan2(siny_cosp, cosy_cosp)
    return yaw

ROS 2에서 로봇의 방향은 보통 quaternion 형식으로 표현됩니다.

하지만 waypoint YAML 파일에서는 사람이 이해하기 쉬운 yaw 값을 사용하는 것이 편합니다.

TurtleBot3 Burger는 바닥 위를 이동하는 2D 모바일 로봇입니다. 따라서 일반적으로 roll, pitch는 크게 중요하지 않고, 로봇이 평면에서 어느 방향을 바라보는지를 나타내는 yaw 값이 중요합니다.

AMCL pose 메시지 안에는 방향이 다음과 같이 들어 있습니다.

orientation:
  x: 0.0
  y: 0.0
  z: 0.707
  w: 0.707

이 값은 사람이 바로 해석하기 어렵습니다.

그래서 quaternion_to_yaw() 함수는 이 quaternion 값을 다음과 같은 yaw 값으로 변환합니다.

yaw: 1.571

yaw 값은 라디안 단위입니다.

코드에서 핵심 계산은 다음 부분입니다.

siny_cosp = 2.0 * ((q.w * q.z) + (q.x * q.y))
cosy_cosp = 1.0 - 2.0 * ((q.y * q.y) + (q.z * q.z))

yaw = math.atan2(siny_cosp, cosy_cosp)

이 계산을 통해 quaternion에서 yaw 성분만 추출합니다.

3) AmclWaypointRecorder 클래스 설명

class AmclWaypointRecorder(Node):

이 클래스가 실제 ROS 2 노드입니다.

Node를 상속받기 때문에 ROS 2 노드로 동작할 수 있습니다.

이 노드는 다음 역할을 합니다.

1. /amcl_pose 구독
2. 현재 pose 저장
3. 키보드 입력 감시
4. Enter 입력 시 waypoint 추가
5. q 입력 시 YAML 파일 저장

4) 저장 파일 경로와 frame_id 설정

self.output_file = Path(output_file).expanduser()
self.frame_id = frame_id

self.output_file은 최종 YAML 파일을 저장할 경로입니다.

예를 들어 실행 명령에서 다음과 같이 지정할 수 있습니다.

--output ~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml

Path(output_file).expanduser()는 ~를 실제 home 디렉터리 경로로 변환합니다.

예를 들면 다음과 같습니다.

~/turtlebot3_ws

이 경로가 내부적으로 다음처럼 변환됩니다.

/home/ubuntu/turtlebot3_ws

self.frame_id는 waypoint 좌표계입니다.

일반적으로 Nav2와 AMCL에서는 map 좌표계를 사용합니다.

frame_id: map

이 값은 최종 YAML 파일에도 저장됩니다.

5) 현재 pose와 waypoint 목록 변수

self.current_pose = None
self.waypoints = []
self.running = True

self.current_pose는 AMCL에서 가장 최근에 받은 현재 로봇 위치를 저장합니다.

처음에는 아직 /amcl_pose를 받지 못했기 때문에 None으로 시작합니다.

나중에 /amcl_pose를 받으면 다음과 같은 형태로 저장됩니다.

self.current_pose = {
    "x": 1.245,
    "y": -0.532,
    "yaw": 1.571
}

self.waypoints는 사용자가 Enter를 눌러 저장한 waypoint 목록입니다.

예를 들어 Enter를 세 번 누르면 내부적으로 다음과 비슷한 리스트가 됩니다.

self.waypoints = [
    {"name": "wp1", "x": 0.421, "y": -0.218, "yaw": 1.571},
    {"name": "wp2", "x": 1.254, "y": -0.231, "yaw": 0.018},
    {"name": "wp3", "x": 1.241, "y": 0.812, "yaw": 1.562}
]

self.running은 키보드 입력 thread를 계속 실행할지 여부를 판단하는 변수입니다.

6) /amcl_pose 구독 부분 설명

self.create_subscription(
    PoseWithCovarianceStamped,
    "/amcl_pose",
    self.amcl_pose_callback,
    10
)

이 부분은 /amcl_pose 토픽을 구독하는 코드입니다.

AMCL이 실행 중이면 /amcl_pose 토픽으로 현재 로봇의 추정 위치가 계속 발행됩니다.

구독 설정의 의미는 다음과 같습니다.

PoseWithCovarianceStamped  메시지 타입
"/amcl_pose"               구독할 토픽 이름
self.amcl_pose_callback    메시지를 받았을 때 실행할 함수
10                         QoS queue 크기

즉 /amcl_pose 메시지가 들어올 때마다 self.amcl_pose_callback() 함수가 자동으로 실행됩니다.

7) 실행 안내 로그 출력

self.get_logger().info("AMCL waypoint recorder started.")
self.get_logger().info("Subscribing: /amcl_pose")
self.get_logger().info(f"Output YAML: {self.output_file}")
self.get_logger().info("Move robot to target position, then press Enter to save waypoint.")
self.get_logger().info("Type q and press Enter to save file and quit.")

이 부분은 사용자가 현재 노드 상태를 알 수 있도록 터미널에 안내 메시지를 출력합니다.

실행하면 대략 다음과 같이 보입니다.

AMCL waypoint recorder started.
Subscribing: /amcl_pose
Output YAML: /home/ubuntu/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml
Move robot to target position, then press Enter to save waypoint.
Type q and press Enter to save file and quit.

실제 실습자는 이 메시지를 보고 로봇을 원하는 위치로 이동시킨 후 Enter를 누르면 됩니다.

8) 키보드 입력 thread 시작

self.input_thread = threading.Thread(target=self.keyboard_loop)
self.input_thread.daemon = True
self.input_thread.start()

이 코드는 키보드 입력을 받는 별도 thread를 시작합니다.

ROS 2 노드는 rclpy.spin(node)으로 callback을 계속 처리합니다. 그런데 같은 thread에서 input()으로 키보드 입력을 기다리면 ROS 2 callback 처리가 막힐 수 있습니다.

그래서 이 코드에서는 키보드 입력 처리를 별도 thread로 분리했습니다.

구조는 다음과 같습니다.

메인 thread
  → rclpy.spin(node)
  → /amcl_pose callback 처리

입력 thread
  → keyboard_loop()
  → Enter 또는 q 입력 처리

self.input_thread.daemon = True는 메인 프로그램이 종료될 때 이 thread도 함께 종료되도록 설정하는 것입니다.

9) amcl_pose_callback 함수 설명

def amcl_pose_callback(self, msg: PoseWithCovarianceStamped):
    """
    /amcl_pose 토픽에서 현재 로봇 위치를 계속 갱신한다.
    """
    pose = msg.pose.pose

    x = pose.position.x
    y = pose.position.y
    yaw = quaternion_to_yaw(pose.orientation)

    self.current_pose = {
        "x": float(x),
        "y": float(y),
        "yaw": float(yaw)
    }

이 함수는 /amcl_pose 메시지를 받을 때마다 실행됩니다.

AMCL 메시지 구조에서 실제 위치 정보는 다음 경로에 있습니다.

msg.pose.pose.position.x
msg.pose.pose.position.y
msg.pose.pose.orientation

코드에서는 먼저 pose를 꺼냅니다.

pose = msg.pose.pose

그 다음 x, y 좌표를 읽습니다.

x = pose.position.x
y = pose.position.y

방향은 quaternion 형태이므로 yaw로 변환합니다.

yaw = quaternion_to_yaw(pose.orientation)

마지막으로 현재 pose를 dictionary 형태로 저장합니다.

self.current_pose = {
    "x": float(x),
    "y": float(y),
    "yaw": float(yaw)
}

여기서 중요한 점은 이 함수가 waypoint를 바로 저장하지 않는다는 것입니다.

이 함수는 단지 최신 AMCL 위치를 계속 갱신합니다.

실제 waypoint 저장은 사용자가 Enter를 눌렀을 때 수행됩니다.

즉, 역할이 분리되어 있습니다.

/amcl_pose callback
  → 현재 위치 갱신만 수행

Enter 입력
  → 현재 위치를 waypoint로 저장

이 구조가 실제 사용에 적합합니다.

왜냐하면 로봇은 계속 움직일 수 있고, 사용자는 원하는 순간의 위치만 골라서 저장하면 되기 때문입니다.

10) keyboard_loop 함수 설명

def keyboard_loop(self):
    """
    Enter 입력을 받을 때마다 현재 AMCL 위치를 waypoint로 저장한다.
    """
    while self.running:
        user_input = input()

        if user_input.lower() == "q":
            self.running = False
            self.save_yaml()
            rclpy.shutdown()
            break

        self.save_current_pose_as_waypoint()

이 함수는 키보드 입력을 처리합니다.

while self.running: 조건이 참인 동안 계속 입력을 기다립니다.

user_input = input()

사용자가 Enter를 누르면 user_input은 빈 문자열이 됩니다.

빈 문자열인 경우에는 아래 조건에 걸리지 않습니다.

if user_input.lower() == "q":

따라서 다음 코드가 실행됩니다.

self.save_current_pose_as_waypoint()

즉, Enter를 누르면 현재 AMCL 위치가 waypoint로 저장됩니다.

반대로 사용자가 q를 입력하고 Enter를 누르면 다음 코드가 실행됩니다.

self.running = False
self.save_yaml()
rclpy.shutdown()
break

이때 저장된 waypoint 목록을 YAML 파일로 저장하고 ROS 2 노드를 종료합니다.

실제 사용 방법은 다음과 같습니다.

Enter 입력
  → 현재 위치 waypoint 저장

q 입력 후 Enter
  → YAML 저장 후 종료

11) save_current_pose_as_waypoint 함수 설명

def save_current_pose_as_waypoint(self):
    """
    현재 AMCL pose를 waypoints 리스트에 추가한다.
    """
    if self.current_pose is None:
        self.get_logger().warn("No AMCL pose received yet. Check /amcl_pose.")
        return

이 함수는 현재 AMCL 위치를 waypoint 목록에 추가합니다.

먼저 self.current_pose가 있는지 확인합니다.

만약 아직 /amcl_pose를 한 번도 받지 못했다면 self.current_pose는 None입니다.

그 상태에서 Enter를 누르면 저장할 좌표가 없기 때문에 다음 경고가 출력됩니다.

No AMCL pose received yet. Check /amcl_pose.

이 경우 확인해야 할 것은 다음입니다.

ros2 topic list | grep amcl

또는 다음 명령으로 /amcl_pose가 실제로 나오는지 확인합니다.

ros2 topic echo /amcl_pose

AMCL이 정상적으로 실행되고 초기 위치가 설정되어 있어야 /amcl_pose가 나옵니다.

12) waypoint 이름 생성

index = len(self.waypoints) + 1
waypoint_name = f"wp{index}"

저장된 waypoint 개수를 기준으로 이름을 자동 생성합니다.

처음 저장하면 wp1입니다.

두 번째 저장하면 wp2입니다.

세 번째 저장하면 wp3입니다.

예를 들면 다음과 같습니다.

첫 번째 Enter  → wp1
두 번째 Enter  → wp2
세 번째 Enter  → wp3

13) waypoint 데이터 생성

waypoint = {
    "name": waypoint_name,
    "x": round(self.current_pose["x"], 3),
    "y": round(self.current_pose["y"], 3),
    "yaw": round(self.current_pose["yaw"], 3)
}

현재 AMCL pose를 waypoint 형태로 만듭니다.

저장되는 값은 다음 네 가지입니다.

name  waypoint 이름
x     map 기준 x 좌표
y     map 기준 y 좌표
yaw   map 기준 로봇 방향

round(..., 3)은 소수점 세 자리까지만 저장하기 위한 코드입니다.

예를 들어 AMCL 값이 다음과 같다면

x = 1.245762341
y = -0.532881912
yaw = 1.57092312

YAML에는 다음처럼 저장됩니다.

x: 1.246
y: -0.533
yaw: 1.571

14) waypoint 리스트에 추가

self.waypoints.append(waypoint)

생성한 waypoint를 리스트에 추가합니다.

이 리스트는 나중에 YAML 파일의 waypoints 항목으로 저장됩니다.

15) 저장 로그 출력

self.get_logger().info(
    f"Saved {waypoint_name}: "
    f"x={waypoint['x']}, y={waypoint['y']}, yaw={waypoint['yaw']}"
)

사용자가 Enter를 누를 때마다 저장된 좌표를 터미널에 출력합니다.

예시:

Saved wp1: x=0.421, y=-0.218, yaw=1.571
Saved wp2: x=1.254, y=-0.231, yaw=0.018
Saved wp3: x=1.241, y=0.812, yaw=1.562

이 출력은 매우 중요합니다.

실습 중 좌표가 이상하게 저장되는지 바로 확인할 수 있기 때문입니다.

예를 들어 로봇을 거의 움직이지 않았는데 x, y 값이 크게 바뀐다면 AMCL localization이 불안정한 상태입니다.

16) save_yaml 함수 설명

def save_yaml(self):
    """
    저장된 waypoint 목록을 YAML 파일로 저장한다.
    """
    if len(self.waypoints) == 0:
        self.get_logger().warn("No waypoints saved. YAML file will not be created.")
        return

이 함수는 지금까지 저장한 waypoint 목록을 YAML 파일로 저장합니다.

먼저 waypoint가 하나라도 있는지 확인합니다.

만약 waypoint가 하나도 없다면 YAML 파일을 만들지 않습니다.

이때 다음 경고가 출력됩니다.

No waypoints saved. YAML file will not be created.

즉, Enter를 한 번도 누르지 않고 q를 입력하면 파일이 생성되지 않습니다.

17) YAML 데이터 구조 생성

yaml_data = {
    "frame_id": self.frame_id,
    "initial_pose": {
        "x": self.waypoints[0]["x"],
        "y": self.waypoints[0]["y"],
        "yaw": self.waypoints[0]["yaw"]
    },
    "waypoints": self.waypoints
}

이 부분에서 최종 YAML 파일 구조를 만듭니다.

생성되는 YAML은 다음 구조입니다.

frame_id: map

initial_pose:
  x: 0.421
  y: -0.218
  yaw: 1.571

waypoints:
  - name: wp1
    x: 0.421
    y: -0.218
    yaw: 1.571

  - name: wp2
    x: 1.254
    y: -0.231
    yaw: 0.018

frame_id는 좌표 기준입니다.

보통 Nav2 경유점 주행에서는 map을 사용합니다.

initial_pose는 첫 번째 waypoint와 같은 위치로 저장됩니다.

이유는 간단합니다. 일반적인 실습에서는 첫 번째로 저장한 위치가 출발 위치인 경우가 많기 때문입니다.

다만 실제 주행에서는 반드시 이 위치에서 출발해야 하는 것은 아닙니다.

RViz에서 2D Pose Estimate로 현재 위치를 수동으로 맞춘다면 initial_pose는 사용하지 않아도 됩니다.

waypoints는 실제 경유점 주행에 사용할 좌표 목록입니다.

18) 저장 폴더 생성

self.output_file.parent.mkdir(parents=True, exist_ok=True)

YAML 파일을 저장할 폴더가 없으면 자동으로 생성합니다.

예를 들어 저장 경로가 다음과 같다고 하겠습니다.

~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml

만약 waypoints 폴더가 없다면 이 코드가 자동으로 만들어 줍니다.

parents=True는 상위 폴더까지 필요하면 함께 생성하라는 뜻입니다.

exist_ok=True는 이미 폴더가 있어도 에러를 내지 말라는 뜻입니다.

19) YAML 파일 쓰기

with open(self.output_file, "w", encoding="utf-8") as file:
    yaml.dump(
        yaml_data,
        file,
        allow_unicode=True,
        sort_keys=False,
        default_flow_style=False
    )

이 부분이 실제로 YAML 파일을 저장하는 코드입니다.

open(..., "w")는 쓰기 모드로 파일을 엽니다.

encoding="utf-8"은 한글이 깨지지 않도록 하기 위한 설정입니다.

yaml.dump()는 Python dictionary를 YAML 형식으로 변환해서 파일에 저장합니다.

옵션의 의미는 다음과 같습니다.

allow_unicode=True       한글 저장 허용
sort_keys=False          dictionary 순서 유지
default_flow_style=False 보기 좋은 YAML 블록 형식 사용

sort_keys=False를 넣지 않으면 YAML 키 순서가 바뀔 수 있습니다.

우리는 사람이 보기 좋은 순서로 저장하고 싶기 때문에 이 옵션을 넣었습니다.

20) 저장 완료 로그

self.get_logger().info(f"Saved waypoint YAML: {self.output_file}")

YAML 저장이 끝나면 저장 경로를 출력합니다.

예시:

Saved waypoint YAML: /home/ubuntu/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml

이 파일을 이후 waypoint follower에서 사용합니다.

21) parse_arguments 함수 설명

def parse_arguments():
    argv = remove_ros_args(args=sys.argv)[1:]

    parser = argparse.ArgumentParser(
        description="Record TurtleBot3 AMCL pose as waypoint YAML"
    )

이 함수는 실행 옵션을 처리합니다.

ROS 2 명령어는 일반 Python 인자 외에도 ROS 전용 인자를 포함할 수 있습니다.

그래서 remove_ros_args()를 사용해 ROS 2 인자를 제거하고, 우리가 사용할 인자만 argparse로 처리합니다.

예를 들어 다음 명령을 실행할 수 있습니다.

ros2 run tb3_waypoint_nav amcl_waypoint_recorder \
  --output ~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml \
  --frame-id map

여기서 이 코드가 처리하는 인자는 다음 두 개입니다.

--output
--frame-id

22)–output 옵션 설명

parser.add_argument(
    "--output",
    default="~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml",
    help="Output waypoint YAML file path"
)

--output은 생성할 YAML 파일 경로입니다.

지정하지 않으면 기본값으로 다음 경로에 저장됩니다.

~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml

다른 이름으로 저장하고 싶으면 실행할 때 다음처럼 바꾸면 됩니다.

ros2 run tb3_waypoint_nav amcl_waypoint_recorder \
  --output ~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/delivery_route_01.yaml

이렇게 하면 delivery_route_01.yaml이라는 파일로 저장됩니다.

23) –frame-id 옵션 설명

parser.add_argument(
    "--frame-id",
    default="map",
    help="Frame ID for waypoint YAML"
)

--frame-id는 waypoint 좌표 기준 프레임입니다.

Nav2 경유점 주행에서는 일반적으로 map을 사용합니다.

따라서 대부분의 경우 기본값을 그대로 사용하면 됩니다.

--frame-id map

특별한 이유가 없다면 odom이나 base_link를 사용하지 않는 것이 좋습니다.

경유점 주행 목표 좌표는 보통 map 기준으로 관리해야 합니다.

24) main 함수 설명

def main():
    args = parse_arguments()

    rclpy.init()

main() 함수는 프로그램이 실제로 시작되는 부분입니다.

먼저 실행 인자를 읽습니다.

args = parse_arguments()

그 다음 ROS 2 Python 시스템을 초기화합니다.

rclpy.init()

ROS 2 노드를 만들기 전에 반드시 rclpy.init()을 호출해야 합니다.

25) 노드 객체 생성

node = AmclWaypointRecorder(
    output_file=args.output,
    frame_id=args.frame_id
)

여기서 AmclWaypointRecorder 노드 객체를 생성합니다.

실행 옵션으로 받은 output_file과 frame_id를 노드에 전달합니다.

예를 들어 사용자가 다음과 같이 실행했다면

ros2 run tb3_waypoint_nav amcl_waypoint_recorder \
  --output ~/waypoints/test.yaml \
  --frame-id map

노드 내부에는 다음 값이 들어갑니다.

output_file = ~/waypoints/test.yaml
frame_id = map

26) rclpy.spin 설명

try:
    rclpy.spin(node)

rclpy.spin(node)는 ROS 2 노드를 계속 실행하면서 callback을 처리하는 함수입니다.

이 코드에서는 /amcl_pose 메시지가 들어올 때마다 amcl_pose_callback()이 실행되어야 합니다.

그 작업을 가능하게 하는 것이 rclpy.spin(node)입니다.

즉, 이 코드가 실행되는 동안 노드는 계속 살아 있고 /amcl_pose를 계속 받습니다.

27) KeyboardInterrupt 처리

except KeyboardInterrupt:
    node.get_logger().warn("Keyboard interrupt received.")
    node.save_yaml()

사용자가 Ctrl + C를 누르면 KeyboardInterrupt가 발생합니다.

이때 프로그램이 바로 종료되면 저장한 waypoint가 사라질 수 있습니다.

그래서 이 코드에서는 Ctrl + C가 들어와도 먼저 node.save_yaml()을 호출해서 저장된 waypoint를 YAML 파일로 남깁니다.

즉, 종료 방법은 두 가지입니다.

q 입력 후 Enter
  → YAML 저장 후 종료

Ctrl + C
  → YAML 저장 시도 후 종료

실습에서는 q 입력 방식을 추천합니다.

28) finally 블록 설명

finally:
    node.running = False
    node.destroy_node()

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

finally 블록은 정상 종료든, 에러 종료든 마지막에 실행됩니다.

먼저 키보드 입력 thread를 멈추기 위해 self.running을 False로 만듭니다.

node.running = False

그 다음 ROS 2 노드를 제거합니다.

node.destroy_node()

마지막으로 ROS 2 시스템을 종료합니다.

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

rclpy.ok()로 아직 ROS 2가 종료되지 않았는지 확인한 뒤 shutdown을 호출합니다.

6. setup.py 수정

새 노드를 실행할 수 있도록 setup.py의 console_scripts에 추가합니다.

nano ~/turtlebot3_ws/src/tb3_waypoint_nav/setup.py

아래처럼 수정합니다.

from setuptools import setup
import os
from glob import glob

package_name = 'tb3_waypoint_nav'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'waypoints'),
            glob('waypoints/*.yaml')),
    ],
    install_requires=['setuptools', 'PyYAML'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='user@example.com',
    description='TurtleBot3 waypoint navigation using Nav2 and YAML',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'amcl_waypoint_recorder = tb3_waypoint_nav.amcl_waypoint_recorder:main',
        ],
    },
)

중요하게 추가된 부분은 이것입니다.

'amcl_waypoint_recorder = tb3_waypoint_nav.amcl_waypoint_recorder:main',

이제 다음 명령으로 좌표 기록 노드를 실행할 수 있습니다.

ros2 run tb3_waypoint_nav amcl_waypoint_recorder

7. 빌드

패키지를 다시 빌드합니다.

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

8. AMCL 좌표로 경유점 저장하기

1) TurtleBot3 bringup 실행

TurtleBot3 Burger 본체에서 실행합니다.

ros2 launch turtlebot3_bringup robot.launch.py

2) Remote PC에서 Nav2 실행

Remote PC에서 실행합니다.

export TURTLEBOT3_MODEL=burger

ros2 launch turtlebot3_navigation2 navigation2.launch.py \
  map:=$HOME/maps/lab_map.yaml

3) RViz에서 초기 위치 설정

RViz에서 2D Pose Estimate를 사용해 실제 로봇 위치를 맞춥니다.

라이다 점이 맵과 잘 겹치는지 확인합니다.

4) 좌표 저장 노드 실행

새 터미널에서 실행합니다.

cd ~/turtlebot3_ws
source install/setup.bash

ros2 run tb3_waypoint_nav amcl_waypoint_recorder \
  --output ~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml

실행하면 다음과 같은 안내가 나옵니다.

AMCL waypoint recorder started.
Subscribing: /amcl_pose
Move robot to target position, then press Enter to save waypoint.
Type q and press Enter to save file and quit.

9. 실제 경유점 취득 방법

경유점 취득은 다음 방식으로 진행합니다.

1) 로봇을 첫 번째 위치로 이동

키보드 조종을 실행합니다.

ros2 run turtlebot3_teleop teleop_keyboard

로봇을 원하는 첫 번째 경유점 위치로 이동시킵니다.

예를 들어 배송 시작 위치, 복도 입구, 특정 방 앞 등으로 이동합니다.

2) AMCL 위치 안정화 확인

로봇이 멈춘 뒤 1~2초 정도 기다립니다.

AMCL pose가 안정화된 뒤 저장하는 것이 좋습니다.

좌표가 계속 흔들리면 다음을 확인해야 합니다.

ros2 topic echo /amcl_pose

position.x, position.y 값이 크게 흔들리면 localization이 불안정한 상태입니다.

3) Enter 입력으로 현재 좌표 저장

amcl_waypoint_recorder 터미널에서 Enter를 누릅니다.

그러면 현재 위치가 저장됩니다.

예시 출력:

Saved wp1: x=0.421, y=-0.218, yaw=1.571

4) 다음 위치로 이동 후 반복 저장

다시 teleop으로 로봇을 두 번째 경유점으로 이동시킵니다.

그리고 recorder 터미널에서 Enter를 누릅니다.

Saved wp2: x=1.254, y=-0.231, yaw=0.018

이 과정을 원하는 경유점 수만큼 반복합니다.

5) 저장 종료

모든 경유점을 저장했다면 recorder 터미널에서 q를 입력하고 Enter를 누릅니다.

그러면 YAML 파일이 생성됩니다.

Saved waypoint YAML: /home/user/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml

10. 생성된 YAML 파일 확인

생성된 파일을 확인합니다.

cat ~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml

예시는 다음과 같습니다.

frame_id: map
initial_pose:
  x: 0.421
  y: -0.218
  yaw: 1.571
waypoints:
- name: wp1
  x: 0.421
  y: -0.218
  yaw: 1.571
- name: wp2
  x: 1.254
  y: -0.231
  yaw: 0.018
- name: wp3
  x: 1.241
  y: 0.812
  yaw: 1.562
- name: wp4
  x: 0.418
  y: 0.796
  yaw: 3.124

이 파일은 기존 waypoint_follower_yaml.py에서 그대로 사용할 수 있습니다.

11. Python 경유점 주행 소스 코드

다음 파일을 생성합니다.

touch ~/turtlebot3_ws/src/tb3_waypoint_nav/tb3_waypoint_nav/waypoint_follower_yaml.py

아래 코드를 입력합니다.

#!/usr/bin/env python3

import argparse
import math
import sys
from pathlib import Path

import yaml

import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
from rclpy.utilities import remove_ros_args


def yaw_to_quaternion(yaw: float):
    """
    2D yaw 값을 quaternion z, w 값으로 변환한다.

    TurtleBot3는 평면 주행 로봇이므로 roll, pitch는 0으로 두고
    yaw 회전만 quaternion으로 변환한다.
    """
    qz = math.sin(yaw * 0.5)
    qw = math.cos(yaw * 0.5)
    return qz, qw


def create_pose(navigator: BasicNavigator, frame_id: str, x: float, y: float, yaw: float) -> PoseStamped:
    """
    Nav2에 전달할 PoseStamped 메시지를 생성한다.
    """
    pose = PoseStamped()
    pose.header.frame_id = frame_id
    pose.header.stamp = navigator.get_clock().now().to_msg()

    pose.pose.position.x = float(x)
    pose.pose.position.y = float(y)
    pose.pose.position.z = 0.0

    qz, qw = yaw_to_quaternion(float(yaw))
    pose.pose.orientation.z = qz
    pose.pose.orientation.w = qw

    return pose


def load_waypoint_yaml(yaml_path: str) -> dict:
    """
    YAML 파일을 읽어서 dict 형태로 반환한다.
    """
    path = Path(yaml_path).expanduser()

    if not path.exists():
        raise FileNotFoundError(f"YAML file not found: {path}")

    with open(path, "r", encoding="utf-8") as file:
        data = yaml.safe_load(file)

    if data is None:
        raise ValueError("YAML file is empty.")

    if "waypoints" not in data:
        raise ValueError("YAML file must contain 'waypoints' field.")

    if not isinstance(data["waypoints"], list):
        raise ValueError("'waypoints' must be a list.")

    return data


def parse_arguments():
    """
    ROS 2 인자와 사용자 인자를 분리해서 처리한다.
    """
    argv = remove_ros_args(args=sys.argv)[1:]

    parser = argparse.ArgumentParser(
        description="TurtleBot3 Nav2 waypoint follower using YAML file"
    )

    parser.add_argument(
        "--waypoints",
        required=True,
        help="Path to waypoint YAML file"
    )

    parser.add_argument(
        "--use-initial-pose",
        action="store_true",
        help="Set initial pose from YAML before navigation"
    )

    return parser.parse_args(argv)


def main():
    args = parse_arguments()

    rclpy.init()

    navigator = BasicNavigator()

    try:
        yaml_data = load_waypoint_yaml(args.waypoints)

        frame_id = yaml_data.get("frame_id", "map")

        # 1. 초기 위치 설정
        if args.use_initial_pose and "initial_pose" in yaml_data:
            init = yaml_data["initial_pose"]

            initial_pose = create_pose(
                navigator=navigator,
                frame_id=frame_id,
                x=init.get("x", 0.0),
                y=init.get("y", 0.0),
                yaw=init.get("yaw", 0.0),
            )

            navigator.setInitialPose(initial_pose)
            navigator.info("Initial pose has been set from YAML.")

        # 2. Nav2 활성화 대기
        navigator.info("Waiting for Nav2 to become active...")
        navigator.waitUntilNav2Active()
        navigator.info("Nav2 is active.")

        # 3. YAML 경유점 목록을 PoseStamped 목록으로 변환
        goal_poses = []

        for wp in yaml_data["waypoints"]:
            name = wp.get("name", "noname")
            x = wp["x"]
            y = wp["y"]
            yaw = wp.get("yaw", 0.0)

            pose = create_pose(
                navigator=navigator,
                frame_id=frame_id,
                x=x,
                y=y,
                yaw=yaw,
            )

            goal_poses.append(pose)
            navigator.info(f"Loaded waypoint: {name}, x={x}, y={y}, yaw={yaw}")

        if len(goal_poses) == 0:
            raise ValueError("No waypoint loaded from YAML.")

        # 4. 경유점 주행 시작
        navigator.info(f"Starting waypoint navigation. Total waypoints: {len(goal_poses)}")
        navigator.followWaypoints(goal_poses)

        # 5. 주행 상태 모니터링
        while not navigator.isTaskComplete():
            feedback = navigator.getFeedback()

            if feedback:
                current_wp = feedback.current_waypoint + 1
                total_wp = len(goal_poses)
                navigator.info(f"Moving to waypoint {current_wp}/{total_wp}")

        # 6. 결과 확인
        result = navigator.getResult()

        if result == TaskResult.SUCCEEDED:
            navigator.info("Waypoint navigation succeeded.")

        elif result == TaskResult.CANCELED:
            navigator.warn("Waypoint navigation was canceled.")

        elif result == TaskResult.FAILED:
            navigator.error("Waypoint navigation failed.")

        else:
            navigator.warn("Waypoint navigation finished with unknown result.")

    except KeyboardInterrupt:
        navigator.warn("Keyboard interrupt received. Canceling navigation...")
        navigator.cancelTask()

    except Exception as e:
        navigator.error(f"Error: {str(e)}")
        navigator.cancelTask()

    finally:
        navigator.lifecycleShutdown()
        rclpy.shutdown()


if __name__ == "__main__":
    main()

실행 권한을 부여합니다.

chmod +x ~/turtlebot3_ws/src/tb3_waypoint_nav/tb3_waypoint_nav/waypoint_follower_yaml.py

프로그램 설명은 아래의 글을 참고하세요

https://humanoidsystem.kr/wp-admin/post.php?post=1165&action=edit

12. setup.py 수정

setup.py 파일을 수정합니다.

nano ~/turtlebot3_ws/src/tb3_waypoint_nav/setup.py

아래와 같이 작성합니다.

from setuptools import setup
import os
from glob import glob

package_name = 'tb3_waypoint_nav'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'waypoints'),
            glob('waypoints/*.yaml')),
    ],
    install_requires=['setuptools', 'PyYAML'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='user@example.com',
    description='TurtleBot3 waypoint navigation using Nav2 and YAML',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'amcl_waypoint_recorder = tb3_waypoint_nav.amcl_waypoint_recorder:main',
            'waypoint_follower_yaml = tb3_waypoint_nav.waypoint_follower_yaml:main',
        ],
    },
)

13. package.xml 확인

package.xml 파일에서 의존성을 확인합니다.

nano ~/turtlebot3_ws/src/tb3_waypoint_nav/package.xml

아래 항목이 포함되어 있어야 합니다.

<?xml version="1.0"?>
<package format="3">
  <name>tb3_waypoint_nav</name>
  <version>0.0.0</version>
  <description>TurtleBot3 waypoint navigation using Nav2 and YAML</description>
  <maintainer email="user@example.com">user</maintainer>
  <license>Apache-2.0</license>

  <depend>rclpy</depend>
  <depend>geometry_msgs</depend>
  <depend>nav2_simple_commander</depend>

  <exec_depend>python3-yaml</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>

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

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

Python 패키지에서는 ament_python이 들어가야 합니다.

14. 빌드

작업 공간으로 이동합니다.

cd ~/turtlebot3_ws

빌드합니다.

colcon build --packages-select tb3_waypoint_nav

환경 설정을 적용합니다.

source install/setup.bash

15. 기존 경유점 주행 코드에 적용하기

기존 실행 명령에서 YAML 파일 경로만 바꾸면 됩니다.

ros2 run tb3_waypoint_nav waypoint_follower_yaml \
  --waypoints ~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml \
  --use-initial-pose

단, 실제 로봇에서는 --use-initial-pose 사용 여부를 조심해야 합니다.

실제 현장에서는 보통 다음 방식이 더 안전합니다.

ros2 run tb3_waypoint_nav waypoint_follower_yaml \
  --waypoints ~/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/recorded_waypoints.yaml

왜냐하면 실제 로봇에서는 RViz에서 2D Pose Estimate로 현재 위치를 이미 맞춘 상태에서 주행하는 경우가 많기 때문입니다.

--use-initial-pose를 사용하면 YAML의 initial_pose 값으로 초기 위치를 다시 설정합니다.

로봇이 실제로 그 위치에 있지 않다면 localization이 오히려 틀어질 수 있습니다.

정리하면 다음과 같습니다.

로봇이 YAML initial_pose 위치에서 출발한다
→ --use-initial-pose 사용 가능

로봇 위치를 RViz에서 직접 맞췄다
→ --use-initial-pose 생략 추천

16. 실제 적용 순서

실제 TurtleBot3 Burger에서 경유점 주행을 적용하는 순서는 다음과 같습니다.

1) 1단계: 맵 준비

SLAM으로 맵을 만들거나 기존 맵을 준비합니다.

맵 파일은 보통 다음 두 개가 한 쌍입니다.

lab_map.yaml
lab_map.pgm

2) 2단계: TurtleBot3 bringup

TurtleBot3 본체에서 실행합니다.

ros2 launch turtlebot3_bringup robot.launch.py

3) 3단계: Nav2 실행