경유점 적용 소스 설명

1) 사용되는 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

  - name: wp3
    x: 1.241
    y: 0.812
    yaw: 1.562

frame_id는 좌표 기준 프레임입니다.

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

initial_pose는 로봇의 초기 위치를 의미합니다.

waypoints는 실제로 이동할 경유점 목록입니다.

각 waypoint는 name, x, y, yaw 값을 가집니다.

여기서 x, y는 map 좌표계 기준 위치이고, yaw는 로봇이 해당 위치에 도착했을 때 바라볼 방향입니다.

2) import 부분 설명

#!/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

#!/usr/bin/env python3는 이 파일을 Python 3로 실행하겠다는 의미입니다.

ROS 2 Python 실행 파일에서는 보통 파일 맨 위에 이 구문을 넣습니다.

argparse는 터미널 실행 시 옵션을 받기 위해 사용합니다.

예를 들어 이 소스는 다음과 같이 실행합니다.

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

여기서 --waypoints 옵션을 처리하는 데 argparse가 사용됩니다.

math는 yaw 값을 quaternion으로 변환할 때 사용합니다.

sys는 Python 실행 인자를 가져올 때 사용합니다.

Path는 YAML 파일 경로를 처리할 때 사용합니다. 특히 ~ 경로를 실제 home 경로로 변환할 때 유용합니다.

yaml은 YAML 파일을 읽기 위해 사용합니다.

PoseStamped는 Nav2에 목표 위치를 전달할 때 사용하는 메시지 타입입니다.

BasicNavigator는 Nav2를 Python 코드에서 쉽게 사용할 수 있도록 해주는 클래스입니다.

TaskResult는 Nav2 주행 결과를 확인할 때 사용합니다.

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

3) yaw_to_quaternion 함수 설명

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

YAML 파일에는 방향값이 yaw로 저장되어 있습니다.

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

yaw: 1.571

하지만 ROS 2의 PoseStamped 메시지에서는 방향을 yaw 값 그대로 넣는 것이 아니라 quaternion 형식으로 넣어야 합니다.

TurtleBot3는 바닥 위를 이동하는 2D 모바일 로봇입니다.

따라서 roll과 pitch는 거의 사용하지 않고 yaw 방향만 중요합니다.

이 함수는 yaw 값을 quaternion의 z, w 값으로 변환합니다.

계산식은 다음과 같습니다.

qz = math.sin(yaw * 0.5)
qw = math.cos(yaw * 0.5)

반환값은 다음 두 개입니다.

return qz, qw

나중에 PoseStamped 메시지에 다음처럼 들어갑니다.

pose.pose.orientation.z = qz
pose.pose.orientation.w = qw

4) create_pose 함수 설명

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

이 함수는 Nav2에 보낼 목표 위치 메시지를 생성합니다.

Nav2는 단순히 x, y, yaw 값만 받는 것이 아니라 PoseStamped 메시지 형태의 목표점을 받습니다.

그래서 YAML에서 읽은 좌표를 PoseStamped로 변환해야 합니다.

함수 인자는 다음과 같습니다.

navigator  BasicNavigator 객체
frame_id   좌표 기준 프레임, 일반적으로 map
x          목표 위치의 x 좌표
y          목표 위치의 y 좌표
yaw        목표 위치에서 로봇이 바라볼 방향

먼저 빈 PoseStamped 메시지를 만듭니다.

pose = PoseStamped()

그 다음 좌표 기준 프레임을 지정합니다.

pose.header.frame_id = frame_id

일반적으로 frame_id는 map입니다.

즉, 이 목표점이 map 좌표계 기준이라는 뜻입니다.

그 다음 timestamp를 현재 시간으로 설정합니다.

pose.header.stamp = navigator.get_clock().now().to_msg()

Nav2에 목표점을 보낼 때는 메시지의 시간이 필요합니다.

여기서는 BasicNavigator의 clock을 사용해서 현재 시간을 넣습니다.

위치값은 다음과 같이 넣습니다.

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

TurtleBot3는 2D 평면 주행 로봇이기 때문에 z는 0으로 둡니다.

방향은 앞에서 만든 yaw_to_quaternion() 함수를 사용합니다.

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

마지막으로 완성된 pose를 반환합니다.

return pose

이 함수는 초기 위치를 만들 때도 사용되고, 각 waypoint 목표점을 만들 때도 사용됩니다.

5) load_waypoint_yaml 함수 설명

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

이 함수는 waypoint YAML 파일을 읽는 역할을 합니다.

먼저 문자열 경로를 Path 객체로 변환합니다.

path = Path(yaml_path).expanduser()

expanduser()를 사용하기 때문에 다음과 같은 ~ 경로도 사용할 수 있습니다.

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

그 다음 파일이 실제로 존재하는지 확인합니다.

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

파일이 없으면 바로 에러를 발생시킵니다.

이 처리를 하지 않으면 파일이 없는 상태에서 실행했을 때 원인을 찾기 어렵습니다.

다음으로 YAML 파일을 읽습니다.

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

encoding="utf-8"은 한글이 들어가도 문제없이 읽기 위한 설정입니다.

yaml.safe_load()는 YAML 파일을 Python dictionary 형태로 변환합니다.

예를 들어 YAML 파일이 다음과 같다면

frame_id: map
waypoints:
  - name: wp1
    x: 0.5
    y: 0.0
    yaw: 0.0

Python에서는 다음과 비슷한 dict가 됩니다.

{
    "frame_id": "map",
    "waypoints": [
        {
            "name": "wp1",
            "x": 0.5,
            "y": 0.0,
            "yaw": 0.0
        }
    ]
}

그 다음 YAML 내용이 비어 있는지 확인합니다.

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

그리고 반드시 waypoints 항목이 있는지 확인합니다.

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

이 소스는 waypoint 주행용 코드이므로 waypoints 항목이 반드시 필요합니다.

마지막으로 waypoints가 리스트인지 확인합니다.

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

정상이라면 읽은 데이터를 반환합니다.

return data

6) parse_arguments 함수 설명

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)

이 함수는 터미널에서 받은 실행 옵션을 처리합니다.

ROS 2에서는 실행할 때 ROS 전용 인자가 섞일 수 있습니다.

그래서 먼저 remove_ros_args()를 사용해 ROS 2 인자를 제거합니다.

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

그 다음 argparse.ArgumentParser()를 사용해 일반 Python 옵션을 처리합니다.

이 소스에서 사용하는 옵션은 두 개입니다.

--waypoints
--use-initial-pose

7) –waypoints 옵션 설명

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

--waypoints는 사용할 YAML 파일 경로입니다.

required=True가 들어가 있기 때문에 이 옵션은 반드시 입력해야 합니다.

실행 예시는 다음과 같습니다.

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

만약 이 옵션을 빼고 실행하면 argparse가 에러를 출력하고 프로그램은 실행되지 않습니다.

8) –use-initial-pose 옵션 설명

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

--use-initial-pose는 YAML 파일에 있는 initial_pose를 Nav2에 설정할지 결정하는 옵션입니다.

이 옵션을 넣으면 True가 됩니다.

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

이 옵션을 넣지 않으면 False입니다.

실제 TurtleBot3 Burger에서는 보통 RViz에서 2D Pose Estimate로 현재 위치를 직접 맞춘 뒤 실행하는 경우가 많습니다.

그 경우에는 --use-initial-pose를 생략하는 편이 안전합니다.

반대로 로봇이 항상 같은 출발 위치에서 시작하고, YAML의 initial_pose가 정확하다면 이 옵션을 사용할 수 있습니다.

9) try 블록 구조

try:
    yaml_data = load_waypoint_yaml(args.waypoints)

주행 중에는 여러 가지 문제가 발생할 수 있습니다.

예를 들면 YAML 파일이 없거나, waypoint 형식이 잘못되었거나, Nav2가 실행되지 않았을 수 있습니다.

그래서 주요 코드는 try 블록 안에 넣고, 문제가 생기면 except에서 처리하도록 구성했습니다.

첫 번째로 하는 일은 YAML 파일을 읽는 것입니다.

yaml_data = load_waypoint_yaml(args.waypoints)

args.waypoints에는 사용자가 --waypoints 옵션으로 입력한 파일 경로가 들어 있습니다.

10) frame_id 읽기

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

YAML 파일에서 frame_id 값을 읽습니다.

만약 YAML 파일에 frame_id가 없다면 기본값으로 map을 사용합니다.

즉, 다음 YAML은

frame_id: map

명시적으로 map을 지정한 것이고, 만약 이 줄이 없어도 코드에서는 기본적으로 map을 사용합니다.

실제 Nav2 waypoint 주행에서는 특별한 이유가 없다면 map을 사용하는 것이 맞습니다.

11) 초기 위치 설정 부분

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.")

이 부분은 YAML 파일의 initial_pose를 Nav2에 설정하는 코드입니다.

조건은 두 가지입니다.

1. 실행할 때 --use-initial-pose 옵션이 있어야 한다.
2. YAML 파일 안에 initial_pose 항목이 있어야 한다.

두 조건이 모두 만족되면 초기 위치를 설정합니다.

먼저 YAML에서 initial_pose 값을 꺼냅니다.

init = yaml_data["initial_pose"]

예를 들어 YAML이 다음과 같다면

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

init에는 이 값들이 들어갑니다.

그 다음 create_pose() 함수를 사용해 PoseStamped 메시지로 변환합니다.

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

init.get("x", 0.0)은 YAML에 x 값이 있으면 그 값을 사용하고, 없으면 0.0을 사용하라는 의미입니다.

마지막으로 Nav2에 초기 위치를 설정합니다.

navigator.setInitialPose(initial_pose)

이 작업은 RViz에서 2D Pose Estimate를 찍는 것과 비슷한 역할을 합니다.

하지만 실제 로봇에서는 주의해야 합니다.

로봇이 실제로 그 위치에 있지 않은데 initial_pose를 강제로 넣으면 AMCL 위치가 틀어질 수 있습니다.

따라서 실제 TurtleBot3 Burger 주행에서는 다음 기준으로 사용하면 됩니다.

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

RViz에서 이미 2D Pose Estimate로 위치를 맞췄다
  --use-initial-pose 생략 추천

12) Nav2 활성화 대기

navigator.info("Waiting for Nav2 to become active...")
navigator.waitUntilNav2Active()
navigator.info("Nav2 is active.")

Nav2는 여러 lifecycle node로 구성됩니다.

예를 들어 planner server, controller server, behavior server, bt navigator 등이 준비되어야 정상적으로 목표점을 받을 수 있습니다.

waitUntilNav2Active()는 Nav2가 주행 명령을 받을 수 있는 상태가 될 때까지 기다립니다.

이 과정 없이 바로 waypoint를 보내면 Nav2가 아직 준비되지 않아서 실패할 수 있습니다.

13) YAML 경유점 목록을 PoseStamped 목록으로 변환

goal_poses = []

Nav2의 followWaypoints() 함수에는 PoseStamped 메시지 목록을 넣어야 합니다.

YAML 파일에는 단순히 x, y, yaw 값이 들어 있으므로 이를 하나씩 PoseStamped로 변환해야 합니다.

변환된 목표점들을 저장할 리스트가 goal_poses입니다.

14) waypoint 반복 처리

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

YAML의 waypoints 항목을 하나씩 꺼내서 처리합니다.

각 waypoint는 다음과 같은 구조입니다.

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

name은 waypoint 이름입니다.

name = wp.get("name", "noname")

이름이 없으면 "noname"으로 표시합니다.

x와 y는 필수값으로 처리합니다.

x = wp["x"]
y = wp["y"]

즉 YAML에 x 또는 y가 없으면 에러가 발생합니다.

yaw는 없을 수도 있으므로 기본값 0.0을 사용합니다.

yaw = wp.get("yaw", 0.0)

15) waypoint를 PoseStamped로 변환

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

YAML에서 읽은 x, y, yaw 값을 PoseStamped 메시지로 변환합니다.

이 변환이 필요한 이유는 Nav2가 목표점을 PoseStamped 형식으로 받기 때문입니다.

생성된 pose는 goal_poses 리스트에 추가됩니다.

goal_poses.append(pose)

그리고 로그로 어떤 waypoint가 로드되었는지 출력합니다.

16) try 블록 구조

try:
    yaml_data = load_waypoint_yaml(args.waypoints)

주행 중에는 여러 가지 문제가 발생할 수 있습니다.

예를 들면 YAML 파일이 없거나, waypoint 형식이 잘못되었거나, Nav2가 실행되지 않았을 수 있습니다.

그래서 주요 코드는 try 블록 안에 넣고, 문제가 생기면 except에서 처리하도록 구성했습니다.

첫 번째로 하는 일은 YAML 파일을 읽는 것입니다.

yaml_data = load_waypoint_yaml(args.waypoints)

args.waypoints에는 사용자가 --waypoints 옵션으로 입력한 파일 경로가 들어 있습니다.

17) frame_id 읽기

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

YAML 파일에서 frame_id 값을 읽습니다.

만약 YAML 파일에 frame_id가 없다면 기본값으로 map을 사용합니다.

즉, 다음 YAML은

frame_id: map

명시적으로 map을 지정한 것이고, 만약 이 줄이 없어도 코드에서는 기본적으로 map을 사용합니다.

실제 Nav2 waypoint 주행에서는 특별한 이유가 없다면 map을 사용하는 것이 맞습니다.

18) 초기 위치 설정 부분

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.")

이 부분은 YAML 파일의 initial_pose를 Nav2에 설정하는 코드입니다.

조건은 두 가지입니다.

1. 실행할 때 --use-initial-pose 옵션이 있어야 한다.
2. YAML 파일 안에 initial_pose 항목이 있어야 한다.

두 조건이 모두 만족되면 초기 위치를 설정합니다.

먼저 YAML에서 initial_pose 값을 꺼냅니다.

init = yaml_data["initial_pose"]

예를 들어 YAML이 다음과 같다면

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

init에는 이 값들이 들어갑니다.

그 다음 create_pose() 함수를 사용해 PoseStamped 메시지로 변환합니다.

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

init.get("x", 0.0)은 YAML에 x 값이 있으면 그 값을 사용하고, 없으면 0.0을 사용하라는 의미입니다.

마지막으로 Nav2에 초기 위치를 설정합니다.

navigator.setInitialPose(initial_pose)

이 작업은 RViz에서 2D Pose Estimate를 찍는 것과 비슷한 역할을 합니다.

하지만 실제 로봇에서는 주의해야 합니다.

로봇이 실제로 그 위치에 있지 않은데 initial_pose를 강제로 넣으면 AMCL 위치가 틀어질 수 있습니다.

따라서 실제 TurtleBot3 Burger 주행에서는 다음 기준으로 사용하면 됩니다.

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

RViz에서 이미 2D Pose Estimate로 위치를 맞췄다
  --use-initial-pose 생략 추천

19) Nav2 활성화 대기

navigator.info("Waiting for Nav2 to become active...")
navigator.waitUntilNav2Active()
navigator.info("Nav2 is active.")

Nav2는 여러 lifecycle node로 구성됩니다.

예를 들어 planner server, controller server, behavior server, bt navigator 등이 준비되어야 정상적으로 목표점을 받을 수 있습니다.

waitUntilNav2Active()는 Nav2가 주행 명령을 받을 수 있는 상태가 될 때까지 기다립니다.

이 과정 없이 바로 waypoint를 보내면 Nav2가 아직 준비되지 않아서 실패할 수 있습니다.

실행하면 터미널에 다음과 비슷한 로그가 나옵니다.

Waiting for Nav2 to become active...
Nav2 is active.

이 메시지가 나온 뒤부터 실제 waypoint 주행 명령을 보내는 것이 안전합니다.

20) YAML 경유점 목록을 PoseStamped 목록으로 변환

goal_poses = []

Nav2의 followWaypoints() 함수에는 PoseStamped 메시지 목록을 넣어야 합니다.

YAML 파일에는 단순히 x, y, yaw 값이 들어 있으므로 이를 하나씩 PoseStamped로 변환해야 합니다.

변환된 목표점들을 저장할 리스트가 goal_poses입니다.

21) waypoint 반복 처리

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

YAML의 waypoints 항목을 하나씩 꺼내서 처리합니다.

각 waypoint는 다음과 같은 구조입니다.

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

name은 waypoint 이름입니다.

name = wp.get("name", "noname")

이름이 없으면 "noname"으로 표시합니다.

x와 y는 필수값으로 처리합니다.

x = wp["x"]
y = wp["y"]

즉 YAML에 x 또는 y가 없으면 에러가 발생합니다.

yaw는 없을 수도 있으므로 기본값 0.0을 사용합니다.

yaw = wp.get("yaw", 0.0)

22) waypoint를 PoseStamped로 변환

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

YAML에서 읽은 x, y, yaw 값을 PoseStamped 메시지로 변환합니다.

이 변환이 필요한 이유는 Nav2가 목표점을 PoseStamped 형식으로 받기 때문입니다.

생성된 pose는 goal_poses 리스트에 추가됩니다.

goal_poses.append(pose)

그리고 로그로 어떤 waypoint가 로드되었는지 출력합니다.

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

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

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

이 로그를 보면 YAML 파일이 제대로 읽혔는지 바로 확인할 수 있습니다.

23) waypoint 개수 확인

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

만약 YAML 파일은 읽혔지만 waypoint가 하나도 없다면 주행할 목표가 없습니다.

이 경우 에러를 발생시킵니다.

예를 들어 다음과 같은 YAML은 문제가 됩니다.

frame_id: map
waypoints: []

이 경우 주행할 좌표가 없기 때문에 프로그램을 중단하는 것이 맞습니다.

24) 경유점 주행 시작

navigator.info(f"Starting waypoint navigation. Total waypoints: {len(goal_poses)}")
navigator.followWaypoints(goal_poses)

이 부분이 실제 경유점 주행을 시작하는 핵심 코드입니다.

followWaypoints() 함수에 goal_poses 리스트를 전달하면 Nav2가 첫 번째 waypoint부터 마지막 waypoint까지 순서대로 주행합니다.

예를 들어 goal_poses에 세 개의 목표점이 있다면 다음 순서로 이동합니다.

wp1 → wp2 → wp3

여기서 중요한 점은 waypoint 순서입니다.

YAML 파일에 적힌 순서대로 이동합니다.

따라서 경유점 순서를 바꾸고 싶으면 Python 코드를 수정할 필요 없이 YAML 파일에서 순서만 바꾸면 됩니다.

예를 들어 다음 YAML은 wp1, wp2, wp3 순서로 이동합니다.

waypoints:
  - name: wp1
    x: 0.4
    y: 0.0
    yaw: 0.0

  - name: wp2
    x: 1.0
    y: 0.0
    yaw: 0.0

  - name: wp3
    x: 1.0
    y: 1.0
    yaw: 1.571

만약 wp3를 먼저 가고 싶다면 YAML에서 순서만 바꾸면 됩니다.

25) 주행 상태 모니터링

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}")

이 부분은 waypoint 주행이 끝날 때까지 상태를 확인하는 코드입니다.

navigator.isTaskComplete()는 Nav2 작업이 끝났는지 확인합니다.

작업이 아직 끝나지 않았으면 False입니다.

따라서 다음 반복문은 주행이 끝날 때까지 계속 실행됩니다.

while not navigator.isTaskComplete():

반복문 안에서는 feedback을 가져옵니다.

feedback = navigator.getFeedback()

feedback 안에는 현재 몇 번째 waypoint로 이동 중인지 정보가 들어 있습니다.

current_wp = feedback.current_waypoint + 1

코드에서 + 1을 하는 이유는 내부 index는 보통 0부터 시작하기 때문입니다.

즉, 내부적으로 첫 번째 waypoint는 0번이지만 사람이 보기에는 1번 waypoint입니다.

그래서 다음처럼 변환합니다.

내부 index 0 → waypoint 1
내부 index 1 → waypoint 2
내부 index 2 → waypoint 3

전체 waypoint 개수는 다음과 같이 구합니다.

total_wp = len(goal_poses)

마지막으로 현재 이동 상태를 로그로 출력합니다.

navigator.info(f"Moving to waypoint {current_wp}/{total_wp}")

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

Moving to waypoint 1/3
Moving to waypoint 2/3
Moving to waypoint 3/3

다만 이 코드는 반복문이 빠르게 돌기 때문에 같은 로그가 많이 출력될 수 있습니다.

실제 블로그 실습용으로는 이해하기 쉽지만, 로그가 너무 많이 나온다면 나중에 출력 주기를 제한하는 방식으로 개선할 수 있습니다.

26) 결과 확인

result = navigator.getResult()

주행이 끝나면 Nav2 작업 결과를 확인합니다.

결과는 크게 세 가지로 나눌 수 있습니다.

TaskResult.SUCCEEDED
TaskResult.CANCELED
TaskResult.FAILED

27) 주행 성공 처리

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

모든 waypoint를 정상적으로 주행했다면 TaskResult.SUCCEEDED가 반환됩니다.

이 경우 터미널에 성공 로그를 출력합니다.

Waypoint navigation succeeded.

이 위치에 gTTS 음성 출력 코드를 추가하면 최종 목적지 도착 후 안내 음성을 출력할 수 있습니다.

예를 들어 다음과 같은 코드를 넣을 수 있습니다.

if result == TaskResult.SUCCEEDED:
    navigator.info("Waypoint navigation succeeded.")
    speak_with_gtts("목적지에 도착했습니다.")

28) 주행 취소 처리

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

사용자가 작업을 취소했거나 코드에서 cancelTask()가 호출되면 TaskResult.CANCELED가 나올 수 있습니다.

이 경우 주행이 정상 완료된 것이 아닙니다.

29) 주행 실패 처리

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

Nav2가 경로를 만들지 못했거나, 장애물 때문에 이동하지 못했거나, localization 문제가 생기면 실패할 수 있습니다.

실패 시 확인해야 할 것은 다음과 같습니다.

1. waypoint가 맵 안에 있는지 확인
2. waypoint가 벽이나 장애물 위에 있지 않은지 확인
3. AMCL 초기 위치가 정확한지 확인
4. LaserScan이 맵과 잘 겹치는지 확인
5. costmap이 정상적으로 생성되는지 확인
6. Nav2 lifecycle node들이 active 상태인지 확인

30) 알 수 없는 결과 처리

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

정의된 성공, 취소, 실패 외의 결과가 나올 경우 경고를 출력합니다.

일반적인 상황에서는 자주 발생하지 않지만, 예외적인 상태를 확인하기 위해 넣어 둔 코드입니다.

31) KeyboardInterrupt 처리

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

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

이때 그냥 프로그램을 종료하면 Nav2 작업이 남아 있을 수 있습니다.

그래서 navigator.cancelTask()를 호출하여 현재 주행 작업을 취소합니다.

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

로봇이 이동 중일 때 프로그램이 갑자기 종료되면 위험할 수 있기 때문입니다.

32) 일반 예외 처리

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

YAML 파일 오류, waypoint 필드 오류, Nav2 통신 오류 등이 발생하면 이 부분에서 처리됩니다.

에러 내용을 로그로 출력하고, 안전하게 현재 작업을 취소합니다.

예를 들어 YAML 파일 경로가 잘못되면 다음과 같은 에러가 출력될 수 있습니다.

Error: YAML file not found: /home/ubuntu/turtlebot3_ws/src/tb3_waypoint_nav/waypoints/test.yaml

33) finally 블록 설명

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

finally 블록은 성공, 실패, 예외 발생 여부와 관계없이 마지막에 실행됩니다.

navigator.lifecycleShutdown()은 Nav2 lifecycle node 종료 요청을 수행합니다.

실습 코드에서는 주행이 끝난 뒤 Nav2 관련 lifecycle을 정리하기 위해 사용했습니다.

다만 실제 로봇에서 Nav2를 계속 켜둔 상태로 여러 번 waypoint 주행을 반복하고 싶다면 이 부분은 주의해야 합니다.

lifecycleShutdown()을 호출하면 Nav2가 종료될 수 있기 때문에, 반복 주행 테스트에서는 이 줄을 제거하거나 상황에 맞게 조정하는 것이 좋습니다.

예를 들어 Nav2를 계속 켜두고 follower 노드만 종료하고 싶다면 다음처럼 바꿀 수 있습니다.

finally:
    rclpy.shutdown()

처음 실습에서는 기존 코드 그대로 사용해도 됩니다.

하지만 실제 로봇에서 여러 번 반복 실행하는 구조라면 lifecycleShutdown() 사용 여부를 반드시 확인해야 합니다.