Nav2 개요 - 지도·위치추정·경로계획의 흐름

이 장에서 배우는 것

앞 장에서 두리의 링크와 관절을 기술하고 로봇의 형상을 좌표계에 연결했다. 이제 두리가 지도 위에서 자신의 위치를 추정하고, 건물 입구까지 장애물을 피하며 이동하도록 구성한다. 내비게이션은 목표 좌표를 바퀴 속도로 바꾸는 하나의 함수가 아니다. 지도, 센서 관측, 위치 추정, 경로계획, 주행 제어가 서로 다른 주기로 협력하는 과정이다.

Nav2는 ROS 2에서 이 과정을 구성하는 서버와 도구를 제공한다. 이 장에서는 저장된 지도를 이용하는 구성을 기준으로 각 부분의 책임과 데이터 흐름을 살펴본다. ROS 없이 실행하는 모형으로 계획 실패와 복구를 확인한 뒤, 같은 목표 전달 방식을 실제 Nav2 액션 클라이언트로 연결한다.

• 행동 트리가 계획·주행·복구를 어떤 순서로 조정하는지 설명한다.
• 저장된 지도와 전역·지역 코스트맵의 목적을 구분한다.
• AMCL이 추정하는 위치와 좌표 변환의 관계를 이해한다.
• 지도 좌표계의 목표 자세를 Nav2에 보내고 최종 결과를 판별한다.

문제 상황

두리는 관리실에서 출발해 실외 보행로를 따라 배달함으로 이동해야 한다. 저장된 지도에는 건물 벽과 화단이 있지만, 오늘 세워진 안내판과 지나가는 사람은 없다. 바퀴 회전량으로 구한 위치에는 오차가 누적되고, 센서에는 가끔 실제로 없는 장애물처럼 보이는 관측도 들어온다.

처음에는 출발점과 배달함을 직선으로 잇고 그 방향으로 속도를 내보냈다. 그러나 직선이 화단을 가로지르는 문제가 생겼다. 우회 경로를 계산한 다음에는 사람이 길을 막았을 때 멈춘 뒤 무엇을 해야 하는지 결정하지 못했다. 목표에 도착했다는 판단도 액션 요청을 보냈다는 사실과 혼동했다.

이 문제를 풀려면 책임을 나눠야 한다. 위치 추정은 현재 자세의 불확실성을 다루고, 코스트맵은 지나가기 어려운 공간을 표현한다. 계획기는 이동할 경로를 찾고, 제어기는 현재 상태에서 그 경로를 따라갈 속도를 계산한다. 행동 트리는 이 작업들의 진행과 실패를 조정한다. 배달 프로그램은 이 내부 작업을 직접 구현하기보다 목적지와 작업 결과를 관리하는 쪽에 집중한다.

행동 트리가 계획과 주행을 조정한다

행동 트리(behavior tree)는 작은 작업과 조건을 트리로 연결해 실행 순서를 표현한다. 각 노드를 주기적으로 평가하는 동작을 틱이라고 한다. 노드는 작업이 끝났으면 SUCCESS, 실패했으면 FAILURE, 아직 진행 중이면 RUNNING을 반환한다. RUNNING은 실패도 성공도 아니다. 다음 평가에서 작업 상태를 다시 확인하라는 뜻이다.

Nav2의 BT Navigator는 선택된 트리에 따라 경로계획이나 경로 추종 같은 작업을 관련 서버에 요청한다. 계획 서버는 시작과 목표를 연결하는 경로를 계산하고, 제어 서버는 경로와 현재 자세, 주변 장애물을 이용해 속도 명령을 계산한다. 복구에 필요한 회전이나 후진 같은 동작은 동작 서버에 요청할 수 있다. 코스트맵 초기화처럼 별도 서비스로 수행하는 복구도 있다.

단순한 순차 구조에서는 앞 작업이 성공해야 다음 작업으로 넘어간다. 그러나 실제 주행에서는 경로를 따라가는 동안 일정한 조건이나 주기에 따라 경로를 다시 계산할 필요가 있다. Nav2의 트리는 이런 진행 방식을 표현할 수 있다. 따라서 행동 트리를 “계획을 한 번 계산하고 끝까지 따라가는 목록”으로만 이해하면 재계획의 역할을 놓치게 된다.

복구는 원인을 해결할 가능성이 있을 때 선택해야 한다. 오래된 센서 관측이 통로를 막았다면 코스트맵의 해당 관측을 초기화하는 것이 도움이 될 수 있다. 반면 실제 벽 때문에 연결되지 않는 두 영역은 초기화로 연결되지 않는다. 복구 횟수와 재시도 조건을 제한하지 않으면 두리는 같은 실패를 계속 반복한다.

그림은 책임의 흐름을 단순화한 것이며 특정 Nav2 기본 트리의 노드 구성을 그대로 나타내지 않는다. 사용할 트리, 플러그인, 복구 조건은 설정에 따라 달라진다. 뒤의 순수 Python 예제도 이 구조를 이해하기 위한 작은 모형이며 Nav2 트리 실행기를 대체하지 않는다.

내비게이션 구성 요소별 입력과 책임

구성 요소	주요 입력	책임 또는 출력
BT Navigator	목표 자세와 작업 결과	계획·주행·복구의 진행 조정
계획 서버	시작 자세, 목표 자세, 전역 코스트맵	이동 경로 계산
제어 서버	경로, 현재 상태, 지역 코스트맵	주행 속도 명령 계산
동작 서버	트리가 요청한 동작	회전·후진 등의 동작 수행

지도는 기준이고 코스트맵은 이동 비용이다

저장된 점유 격자 지도는 공간을 셀로 나누고 각 셀의 점유 상태를 나타낸다. 지도 서버는 지도 설명 파일과 연결된 이미지에서 이 정보를 읽어 제공할 수 있다. 지도는 위치 추정의 기준이자 전역 코스트맵의 정적 정보가 된다. 지도 파일을 읽었다는 사실만으로 두리의 현재 위치까지 정해지는 것은 아니다.

코스트맵(costmap)은 로봇의 이동 판단에 사용할 비용을 셀에 저장한다. 장애물이 있는 셀뿐 아니라 장애물에 가까운 셀에도 높은 비용을 부여할 수 있다. 그러면 계획기는 이동 거리와 장애물 근접 비용을 함께 고려하게 된다. 비용의 의미는 점유 확률과 같지 않으며, 점유 지도 값을 그대로 주행 비용으로 해석해서는 안 된다.

코스트맵은 여러 계층의 정보를 결합한다. 정적 계층은 저장된 지도를 반영한다. 장애물 계층은 레이저 스캔 같은 관측으로 장애물을 표시하고, 관측 광선이 지나간 공간을 이용해 기존 표시를 지울 수 있다. 사용하는 센서와 설정에 따라 복셀 계층 등을 구성하기도 한다. 팽창 계층은 장애물 주변에 비용을 퍼뜨려 경로가 벽에 지나치게 가까워지는 것을 줄인다.

두리의 몸체 외곽선도 필요하다. 중심점 하나가 빈 셀 위에 있다는 사실만으로 몸체 전체가 통과할 수 있는 것은 아니다. 몸체 외곽선이나 반경 설정이 실제 크기보다 작으면 문틀이나 화단 모서리에 가까운 경로가 허용될 수 있다. 팽창 비용과 로봇의 충돌 검사 형상은 함께 조정해야 한다.

전역 코스트맵은 목적지까지 이어지는 경로를 찾는 데 사용한다. 저장된 지도 기반 구성에서는 보통 map 좌표계를 기준으로 넓은 범위를 다룬다. 지역 코스트맵은 로봇 주변의 주행 제어에 사용하며 보통 odom 좌표계에서 로봇을 따라 움직이는 제한된 창으로 구성한다. 이는 흔한 구성이지 이름만으로 강제되는 규칙은 아니다.

전역 코스트맵에도 동적 장애물 관측을 넣을 수 있다. 지역 코스트맵이 있다고 해서 전역 계획기가 오늘 놓인 안내판을 자동으로 아는 것은 아니다. 어떤 계층을 어느 코스트맵에 연결했는지 확인해야 한다. 관측 거리, 갱신 주기, 지도 해상도와 미지 영역 처리 정책도 결과에 영향을 준다.

실외에서는 보행로 밖의 잔디나 낮은 턱처럼 센서가 잘 구분하지 못하는 공간도 있다. 코스트맵이 비어 있다는 사실은 그곳의 주행 가능성을 충분히 관측했다는 뜻과 다를 수 있다. 이 장에서는 이미 준비된 지도와 장애물 관측을 전제로 흐름을 설명한다. 실제 주행 범위는 센서 구성과 공간의 통행 조건을 반영해 정해야 한다.

AMCL로 위치를 추정하고 목표 자세를 보낸다

적응형 몬테카를로 위치 추정(Adaptive Monte Carlo Localization, AMCL)은 저장된 지도 안에서 로봇의 평면 자세를 추정한다. 여러 위치·방향 후보를 입자로 유지하고, 이동 정보로 후보를 예측한 뒤 레이저 관측과 지도의 일치 정도로 가중치를 갱신한다. 이후 재표본화 등을 거쳐 후보 집합을 조정한다. AMCL은 지도를 만드는 기능이 아니다.

바퀴 주행계가 제공하는 odom → base_link 변환은 짧은 시간 동안 연속적인 움직임을 표현하지만 오차가 누적될 수 있다. AMCL은 지도와 관측을 비교해 전역적인 위치를 보정하고, 일반적인 설정에서 map → odom 변환을 제공한다. 두 변환을 연결하면 지도 기준의 로봇 자세를 얻는다. 센서 좌표계까지 이어지는 변환도 있어야 관측을 로봇과 지도에 맞출 수 있다.

초기 자세는 후보를 어디에 배치할지 정하는 출발 정보다. RViz에서 초기 위치와 방향을 지정하면 AMCL이 탐색할 범위를 좁힐 수 있다. 초기 자세의 공분산은 불확실성의 크기를 표현한다. 실제 위치와 다른 자세를 작은 불확실성으로 전달하면 추정이 올바른 위치로 모이는 데 어려움을 줄 수 있다.

두리가 비슷한 벽이 반복되는 길에 있거나 관측 가능한 구조물이 적은 곳에 있으면 여러 후보를 구분하기 어렵다. 추정값 하나가 출력된다는 이유만으로 위치가 충분히 확인됐다고 판단하지 않는다. 지도와 센서 관측의 정렬, 자세의 안정성, 추정 불확실성을 함께 살펴야 한다.

목표는 위치 두 값만이 아니라 좌표계와 방향을 포함하는 자세다. 이 장에서는 map 좌표계의 x, y와 평면 방향각을 입력받아 NavigateToPose 액션의 pose 필드에 넣는다. 평면 방향각 θ에 대한 쿼터니언은 x = 0, y = 0, z = sin(θ/2), w = cos(θ/2)로 구성한다. 방향은 도착 후 배달함을 어느 쪽으로 바라볼지 지정하는 데 필요하다.

액션 서버가 요청을 수락한 시점과 목표 도착 시점은 구분한다. 요청 수락 뒤에도 주행이 중단되거나 취소될 수 있다. 최종 응답의 상태가 성공인지 확인해야 배달 작업을 다음 단계로 넘길 수 있다. 피드백의 남은 거리는 진행을 관찰하는 값이며, 값이 작다는 이유로 최종 성공을 대신 판정하지 않는다.

구성과 인터페이스의 사실 확인에는 Nav2 개념 안내, AMCL 설정 안내, Jazzy NavigateToPose 인터페이스를 참고할 수 있다. 아래 문장과 코드는 이 장의 두리 예제를 위해 구성했다.

완성 코드

첫 프로그램은 표준 라이브러리만 사용한다. 세 위치 후보의 가중치를 관측으로 갱신하고, 격자에서 비용이 작은 경로를 찾는다. 유일한 통로에 오래된 동적 관측을 놓아 첫 계획을 실패시킨 뒤, 한 번의 복구로 그 관측을 제거하고 다시 계획한다. 제어 모형은 틱마다 경로의 다음 셀로 이동한다.

위치 후보의 갱신은 AMCL의 일부 아이디어만 보여 준다. 이동 예측, 방향, 재표본화, 적응적인 입자 수 조정은 구현하지 않는다. 비용 계산도 Nav2의 실제 비용값 체계와 다르다. 장애물 옆 셀에 추가 비용 4를 주는 자체 규칙이며, 셀 하나의 이동을 거리나 시간의 실제 단위로 해석하지 않는다.

nav2_flow_demo.py

from heapq import heappop, heappush
from itertools import count

WIDTH = 7
HEIGHT = 5
WALLS = {(3, y) for y in range(4)}
GOAL = (5, 2)


def estimate_position():
    candidates = [(0, 2), (1, 2), (2, 2)]
    prior = [0.2, 0.6, 0.2]
    likelihood = [0.1, 0.8, 0.1]
    raw = [p * q for p, q in zip(prior, likelihood)]
    total = sum(raw)
    if total <= 0.0:
        raise ValueError("관측으로 후보를 평가할 수 없다")
    weights = [value / total for value in raw]
    best = max(range(len(weights)), key=weights.__getitem__)
    return candidates[best], weights[best]


def neighbors(cell):
    x, y = cell
    for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1)):
        nx, ny = x + dx, y + dy
        if 0 <= nx < WIDTH and 0 <= ny < HEIGHT:
            yield nx, ny


def entry_cost(cell, blocked):
    x, y = cell
    near_obstacle = any(
        abs(x - bx) + abs(y - by) == 1
        for bx, by in blocked
    )
    return 1 + (4 if near_obstacle else 0)


def plan(start, goal, blocked):
    if start in blocked or goal in blocked:
        return None
    serial = count()
    queue = [(0, next(serial), start)]
    costs = {start: 0}
    previous = {}

    while queue:
        cost, _, current = heappop(queue)
        if cost != costs[current]:
            continue
        if current == goal:
            path = [goal]
            while path[-1] != start:
                path.append(previous[path[-1]])
            path.reverse()
            return path, cost

        for candidate in neighbors(current):
            if candidate in blocked:
                continue
            new_cost = cost + entry_cost(candidate, blocked)
            if new_cost < costs.get(candidate, float("inf")):
                costs[candidate] = new_cost
                previous[candidate] = current
                heappush(
                    queue, (new_cost, next(serial), candidate)
                )
    return None


class NavigationTree:
    def __init__(self, start):
        self.position = start
        self.dynamic = {(3, 4)}
        self.path = None
        self.index = 0
        self.recovered = False
        self.terminal = None

    def tick(self):
        if self.terminal is not None:
            return self.terminal

        if self.path is None:
            answer = plan(
                self.position, GOAL, WALLS | self.dynamic
            )
            if answer is None:
                print("[트리] 계획 실패")
                if self.recovered:
                    self.terminal = "FAILURE"
                    return self.terminal
                removed = len(self.dynamic)
                self.dynamic.clear()
                self.recovered = True
                print(f"[복구] 동적 관측 {removed}개 제거")
                return "RUNNING"

            self.path, cost = answer
            print(
                f"[계획] 이동 {len(self.path) - 1}회, 비용 {cost}"
            )

        if self.index < len(self.path) - 1:
            self.index += 1
            self.position = self.path[self.index]

        if self.position == GOAL:
            print(f"[제어] 목표 셀 {self.position} 도착")
            self.terminal = "SUCCESS"
            return self.terminal
        return "RUNNING"


def main():
    start, weight = estimate_position()
    print(f"[위치] 선택 {start}, 가중치 {weight:.3f}")
    tree = NavigationTree(start)
    status = "RUNNING"
    for _ in range(30):
        status = tree.tick()
        if status != "RUNNING":
            break
    else:
        raise RuntimeError("틱 예산을 초과했다")
    print(f"[결과] {status}")


if __name__ == "__main__":
    main()

두 번째 프로그램은 실제 Nav2에 목표 하나를 보낸다. 지도 서버, AMCL, 내비게이션 서버가 이미 실행되고 활성화되어 있어야 한다. 초기 자세 지정, 센서 입력, 주행계와 좌표 변환도 준비되어 있어야 한다. 서버가 발견된다는 사실만으로 이 조건이 모두 충족되는 것은 아니다.

Nav2 자체의 구동 명령은 로봇별 센서와 지도, 플러그인 설정에 따라 달라지므로 여기서 가상의 공통 실행 명령을 만들지 않는다. 아래 클라이언트는 준비된 Jazzy 환경에서 실행한다. 기본 액션 이름은 상대 이름 navigate_to_pose이며, 별도의 이름공간을 사용하는 시스템에서는 명령행으로 액션 이름을 지정할 수 있다.

send_nav_goal.py

import argparse
import math

import rclpy
from action_msgs.msg import GoalStatus
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
from rclpy.node import Node


def finite_float(text):
    value = float(text)
    if not math.isfinite(value):
        raise argparse.ArgumentTypeError("유한한 숫자가 필요하다")
    return value


class GoalSender(Node):
    def __init__(self, action_name):
        super().__init__("duri_goal_sender")
        self.client = ActionClient(
            self, NavigateToPose, action_name
        )

    def send(self, x, y, yaw_degrees):
        goal = NavigateToPose.Goal()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x = x
        goal.pose.pose.position.y = y
        yaw = math.radians(yaw_degrees)
        goal.pose.pose.orientation.z = math.sin(yaw / 2.0)
        goal.pose.pose.orientation.w = math.cos(yaw / 2.0)
        goal.behavior_tree = ""
        return self.client.send_goal_async(goal)


def wait_value(node, future):
    rclpy.spin_until_future_complete(node, future)
    if not future.done():
        raise RuntimeError("응답을 기다리는 중 실행이 중단됐다")
    value = future.result()
    if value is None:
        raise RuntimeError("응답이 비어 있다")
    return value


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--x", type=finite_float, required=True)
    parser.add_argument("--y", type=finite_float, required=True)
    parser.add_argument("--yaw-deg", type=finite_float, default=0.0)
    parser.add_argument("--action", default="navigate_to_pose")
    args, ros_args = parser.parse_known_args()

    rclpy.init(args=ros_args)
    node = None
    try:
        node = GoalSender(args.action)
        if not node.client.wait_for_server(timeout_sec=10.0):
            print("[결과] 액션 서버를 찾지 못했다")
            return 2

        handle = wait_value(
            node, node.send(args.x, args.y, args.yaw_deg)
        )
        if not handle.accepted:
            print("[결과] 목표가 거절됐다")
            return 3

        print("[요청] 목표 수락")
        result = wait_value(node, handle.get_result_async())
        if result.status == GoalStatus.STATUS_SUCCEEDED:
            print("[결과] 도착 성공")
            return 0
        if result.status == GoalStatus.STATUS_CANCELED:
            print("[결과] 취소")
            return 4
        print("[결과] 성공하지 못했다")
        return 5
    except KeyboardInterrupt:
        print("[클라이언트] 대기를 중단했다")
        return 130
    finally:
        if node is not None:
            node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == "__main__":
    raise SystemExit(main())

빈 behavior_tree 값은 서버에 설정된 기본 트리를 사용하도록 한다. 이 클라이언트는 최종 결과를 기다리는 예제이므로 피드백 출력과 사용자 취소 요청은 추가하지 않았다. Ctrl+C는 클라이언트의 대기를 중단하며, 서버의 목표를 취소했다고 보장하지 않는다. 주행 중단이 필요한 경우에는 별도로 액션 취소 요청과 응답을 처리해야 한다.

줄별 해설

첫 파일의 WIDTH와 HEIGHT는 격자의 범위를 정한다. WALLS는 x가 3이고 y가 0부터 3인 벽이다. 왼쪽과 오른쪽을 오가려면 아래쪽의 (3, 4)를 지나야 한다. GOAL은 오른쪽 영역에 두어 그 통로가 계획 성공 여부를 결정하도록 했다.

estimate_position의 prior는 관측 이전 가중치이고 likelihood는 관측이 각 후보와 맞는 정도다. 두 값을 곱한 raw는 합이 1이 아니므로 total로 나눠 정규화한다. 가운데 후보의 정규화된 가중치는 0.48 / 0.52다. 가장 큰 가중치의 후보를 선택하지만, 실제 AMCL의 자세 추정 전체를 이 선택 한 줄과 동일시해서는 안 된다.

neighbors는 네 방향으로만 이웃을 만든다. 경계 조건을 통과한 좌표만 yield하므로 계획기가 지도 바깥 셀을 방문하지 않는다. entry_cost는 들어갈 셀이 장애물과 상하좌우로 맞닿으면 기본 이동 비용 1에 추가 비용 4를 더한다. 대각선 거리나 로봇의 실제 외곽선은 이 모형에 포함하지 않는다.

plan은 누적 비용이 작은 후보부터 꺼내는 다익스트라 탐색이다. queue에는 비용, 순번, 좌표를 넣는다. 순번은 같은 비용의 항목에 일정한 처리 순서를 부여한다. costs에 저장된 값과 꺼낸 비용이 다르면 더 좋은 경로가 이미 발견된 항목이므로 건너뛴다.

목표를 꺼냈을 때 previous를 거꾸로 따라가면 목표부터 시작점까지의 셀 목록을 얻는다. reverse로 순서를 뒤집어 제어 모형이 따라갈 경로를 만든다. 탐색할 후보가 없어질 때까지 목표를 찾지 못하면 None을 반환한다. 빈 경로와 실패를 혼동하지 않도록 실패값을 명시했다.

NavigationTree의 dynamic은 오래된 관측 하나를 담는다. 첫 tick에서는 벽과 동적 관측을 합친 집합이 아래 통로까지 막는다. 계획이 실패하면 그 관측을 제거하고 recovered를 참으로 바꾼다. 여기서는 관측이 잘못됐다는 사실을 예제 작성자가 알고 있다. 실제 시스템에서는 센서가 다시 관측한 장애물이 곧바로 코스트맵에 표시될 수 있다.

다음 tick은 바뀐 조건으로 경로를 계산한다. 경로가 생기면 index를 하나씩 늘려 위치를 옮긴다. 도착 전까지 RUNNING을 반환하고 도착하면 SUCCESS를 저장한다. terminal은 완료된 트리를 다시 평가해도 같은 결과를 돌려주도록 한다. main의 30회 제한은 모형의 반복 횟수 제한이며 실제 주행 시간 제한이 아니다.

두 번째 파일에서 finite_float는 목표 좌표에 무한대나 NaN이 들어오는 것을 막는다. parse_known_args는 예제용 인자를 읽고 남은 인자를 ROS 초기화에 넘긴다. 이 방식으로 use_sim_time 같은 ROS 파라미터를 명령행에서 함께 전달할 수 있다.

GoalSender는 NavigateToPose 형식의 ActionClient를 만든다. send에서는 목표 좌표계와 노드 시계의 현재 시각을 먼저 기록한다. 위치는 미터, 입력 방향은 도 단위로 받고, 삼각함수 계산 직전에 라디안으로 변환한다. 메시지의 기본값으로 남는 위치 z와 쿼터니언 x, y는 0이다.

wait_value는 실행기를 돌려 비동기 응답이 처리되도록 한다. 이 함수는 main에서 호출한다. 콜백 내부에서 같은 방식으로 기다리는 코드로 옮기면 실행기 구성에 따라 응답 처리가 막힐 수 있으므로, 콜백 기반 프로그램에서는 비동기 완료 콜백으로 흐름을 이어가는 편이 적절하다.

main의 첫 대기는 서버 발견을 위한 10초 제한이다. 목표 수락 응답과 최종 주행 결과에는 이 제한이 적용되지 않는다. accepted를 검사한 다음 get_result_async로 최종 응답을 요청하고, 응답의 status를 비교한다. 반환값은 프로세스 종료 코드가 되므로 호출한 프로그램도 성공과 실패를 구분할 수 있다.

실행 결과

두 파일을 같은 디렉터리에 저장한다. 다음 문법 검사 명령은 ROS 모듈을 가져오거나 Nav2 서버를 실행하지 않는다. 경고를 오류로 취급하여 두 파일을 컴파일하며, 성공하면 출력이 없다. 이 검사는 인터페이스 연결이나 실제 로봇 동작을 검증하는 것은 아니다.

python3 -W error -m py_compile nav2_flow_demo.py send_nav_goal.py

macOS와 Linux에서 ROS 설치 없이 첫 프로그램을 실행할 수 있다.

python3 nav2_flow_demo.py

예상 출력은 다음과 같다.

[위치] 선택 (1, 2), 가중치 0.923
[트리] 계획 실패
[복구] 동적 관측 1개 제거
[계획] 이동 8회, 비용 12
[제어] 목표 셀 (5, 2) 도착
[결과] SUCCESS

경로는 벽 아래로 돌아가므로 이동이 8회다. 통로 셀 (3, 4)는 벽의 끝인 (3, 3)과 이웃이어서 추가 비용 4가 붙는다. 따라서 누적 비용은 12다. 비용과 이동 횟수가 다른 것이 이 예제에서 확인할 핵심이다.

두 번째 프로그램은 rclpy와 nav2_msgs를 사용할 수 있는 Jazzy 환경에서 실행한다. macOS에서 순수 Python 모형을 실행했다고 해서 ROS 클라이언트의 의존성까지 준비되는 것은 아니다. Nav2를 Linux 가상 머신이나 별도 컴퓨터에서 운영한다면 해당 ROS 환경에서 클라이언트를 실행하는 구성이 명확하다.

지도 기준 (2.0, 1.0) 미터가 접근 가능한 목표이고 초기 위치 추정과 주행 구성이 준비되었다고 가정한다.

python3 send_nav_goal.py --x 2.0 --y 1.0 --yaw-deg 90

목표가 수락되고 주행이 성공했을 때 이 프로그램이 출력하는 줄은 다음과 같다. 별도로 실행 중인 Nav2 서버의 로그는 포함하지 않는다.

[요청] 목표 수락
[결과] 도착 성공

서버를 발견하지 못하면 10초 대기 뒤 아래 한 줄을 출력하고 종료 코드 2로 끝난다.

[결과] 액션 서버를 찾지 못했다

시뮬레이션 시간을 사용하는 시스템에서는 클라이언트도 같은 시계를 사용한다. 이때 시뮬레이터가 시각을 공급하고 있어야 한다.

python3 send_nav_goal.py --x 2.0 --y 1.0 --yaw-deg 90 --ros-args -p use_sim_time:=true

실무에서 자주 틀리는 것

지도 좌표를 로봇 좌표라고 표시한다

다음 코드는 지도에서 읽은 배달함 좌표에 base_link라는 이름을 붙인다. 좌표계 이름을 바꾸는 작업은 좌표 변환 계산이 아니다. 서버가 변환할 수 있더라도 원래 의도와 다른 위치를 목표로 해석할 수 있다.

# 틀린 코드: x와 y는 지도에서 읽은 값이다.
goal.pose.header.frame_id = "base_link"
goal.pose.pose.position.x = 2.0
goal.pose.pose.position.y = 1.0

지도에서 얻은 좌표에는 지도 좌표계를 기록하고 시각도 설정한다. 다른 좌표계에서 측정한 값을 사용할 때는 실제 변환 계산을 거쳐야 한다.

# 고친 코드
goal.pose.header.frame_id = "map"
goal.pose.header.stamp = node.get_clock().now().to_msg()
goal.pose.pose.position.x = 2.0
goal.pose.pose.position.y = 1.0

방향을 비워 둔 채 목표를 보낸다

아래 네 성분은 길이가 0인 쿼터니언이므로 유효한 회전을 표현하지 않는다. 방향을 중요하게 생각하지 않는 상황에서도 유효한 목표 방향은 필요하다.

# 틀린 코드
goal.pose.pose.orientation.x = 0.0
goal.pose.pose.orientation.y = 0.0
goal.pose.pose.orientation.z = 0.0
goal.pose.pose.orientation.w = 0.0

지도 x축 방향을 바라보게 하려면 단위 회전을 설정한다. 임의의 평면 방향은 완성 코드처럼 반각의 사인과 코사인으로 계산한다.

# 고친 코드: 방향각 0인 회전
goal.pose.pose.orientation.x = 0.0
goal.pose.pose.orientation.y = 0.0
goal.pose.pose.orientation.z = 0.0
goal.pose.pose.orientation.w = 1.0

요청 수락을 도착 성공으로 처리한다

accepted는 서버가 작업을 받아들였는지 나타낸다. 이 시점에 배달함 잠금을 해제하거나 다음 목적지로 넘어가면 아직 이동 중인 작업과 후속 작업이 겹친다.

# 틀린 코드
if handle.accepted:
    print("도착 성공")

수락 여부를 먼저 확인한 다음 최종 상태를 기다린다. 아래 wait_value는 완성 코드의 함수를 사용한다.

# 고친 코드
if handle.accepted:
    wrapped = wait_value(node, handle.get_result_async())
    if wrapped.status == GoalStatus.STATUS_SUCCEEDED:
        print("도착 성공")
    else:
        print("도착하지 못했다")
else:
    print("목표가 거절됐다")

복구하면서 정적 장애물까지 지운다

순수 Python 모형에서 모든 장애물을 지우면 경로는 쉽게 생기지만 벽을 통과하게 된다. 계산 성공을 위해 환경의 제약을 삭제한 셈이다.

# 틀린 코드: 모형의 정적 벽까지 제거한다.
WALLS.clear()
self.dynamic.clear()
answer = plan(self.position, GOAL, WALLS | self.dynamic)

복구 대상은 원인과 연결해야 한다. 이 모형에서는 잘못된 동적 관측만 제거하고 벽을 유지한다. 실제 코스트맵 초기화는 물체를 없애는 동작이 아니며, 초기화 범위와 계층별 반영 방식도 설정에 따라 확인해야 한다.

# 고친 코드: 모형에서 오래된 동적 관측만 제거한다.
self.dynamic.clear()
answer = plan(self.position, GOAL, WALLS | self.dynamic)

한눈에 보기

두리의 목표 주행에서 확인할 정보와 판단 기준

대상	표현하는 것	확인할 점
저장된 지도	기준 공간의 점유 상태	실제 환경과 지도 형상이 맞는가
AMCL	지도 기준의 추정 자세	초기 자세와 관측이 타당한가
전역 코스트맵	경로계획용 공간 비용	목표까지 연결된 경로가 있는가
지역 코스트맵	주행 제어용 주변 비용	현재 장애물 관측이 반영되는가
행동 트리	작업 진행과 재시도 정책	복구 조건과 횟수가 제한되는가
목표 자세	도착 위치와 방향	좌표계·시각·쿼터니언이 유효한가
최종 액션 상태	요청된 주행의 종료 결과	수락과 성공을 구분했는가

배달 프로그램은 목적지 전달과 작업 결과에 집중하고, 내비게이션 구성은 위치·환경·경로·속도를 연결한다. 문제가 생기면 이 구분에 따라 어떤 입력과 결과부터 확인할지 정할 수 있다. 다음에는 이러한 기대 동작을 자동으로 확인하는 테스트를 다룬다.

연습 문제

1. 순수 Python 모형의 동적 관측을 처음부터 빈 집합으로 바꾸라. 복구 관련 출력과 최종 경로 비용이 어떻게 달라지는지 설명하라.
2. 정적 벽이 (3, 4)까지 포함되도록 바꾸라. 프로그램이 성공하는지 확인하고, 동적 관측을 지워도 결과가 달라지지 않는 이유를 설명하라.
3. 목표 방향을 -90도로 보낼 때 쿼터니언의 z와 w를 계산하라. 쿼터니언 네 성분의 제곱합도 구하라.
4. 목표 수락 뒤 최종 상태가 STATUS_ABORTED였다. 배달 완료로 기록해도 되는지 판단하고, 위치 추정과 코스트맵 관점에서 확인할 항목을 각각 하나 이상 제시하라.

정답과 해설

1. NavigationTree의 생성자에서 self.dynamic = set()으로 바꾼다. 첫 계획부터 통로가 열려 있으므로 계획 실패와 복구 출력이 사라진다. 벽과 목표는 같으므로 이동 8회, 비용 12이며 최종 결과는 SUCCESS다. 빈 집합을 만들 때 {}를 사용하면 사전이 되므로 set()을 사용한다.

2. WALLS = {(3, y) for y in range(5)}로 바꾼다. 첫 실패 후 동적 관측을 제거하지만 정적 벽이 통로를 계속 막는다. 다음 계획도 실패하고 recovered가 이미 참이므로 FAILURE로 종료한다. 격자 밖으로 나갈 수 없고 벽을 지나는 이웃도 허용되지 않으므로 연결 경로가 없다. 복구 횟수를 늘려도 이 공간 구조는 달라지지 않는다.

3. θ는 -π/2이므로 z = sin(-π/4) ≈ -0.70710678, w = cos(-π/4) ≈ 0.70710678이다. x와 y는 0이다. 제곱합은 반올림 오차를 제외하면 1이다. 위치가 같더라도 방향각이 다르면 최종 목표 자세는 달라진다.

4. STATUS_ABORTED는 성공이 아니므로 배달 완료로 기록하면 안 된다. 위치 추정에서는 초기 자세와 센서 관측이 지도에 정렬되는지, 필요한 좌표 변환이 공급되는지 확인한다. 코스트맵에서는 목표가 장애물 안에 놓였는지, 통로가 로봇 크기에 비해 좁은지, 오래된 관측이 남아 있는지 확인한다. 이 상태값 하나만으로 실패 원인을 확정할 수 없으므로 서버의 결과 정보와 로그를 함께 살펴야 한다.