CH10ROS 2 SLAM 과 Nav2 자율주행
Gmapping 으로 실제 지도를 만들어 저장하고, Nav2 로 위치 추정(AMCL) · 목표 주행 · 웨이포인트 순회를 구성하며 파라미터가 주행에 주는 영향을 확인합니다.
학습 목표#
- 공식 순서(bringup →
slam_gmapping→ teleop →map_saver_cli)로 지도를 만들어 저장하고,map.yaml의 resolution · origin · occupied_thresh · free_thresh 가 지도 해석을 어떻게 바꾸는지 시뮬레이터로 예측 · 확인할 수 있습니다. - pgm 이미지를 칠해 금지 구역을 만들고, 픽셀값 · 임계값 · 행 순서(위가 행 0)를 지켜 지도를 직접 고칠 수 있습니다.
- AMCL 이 파티클을 이동 → 가중치 → 리샘플링 하며 한 점으로 모으는 원리를 설명하고, 2D Pose Estimate 가 틀렸을 때 · 특징 없는 복도 · 납치된 로봇에서 무슨 일이 생기는지 관찰할 수 있습니다. AMCL 이 map → odom 을 발행하는 이유를 TF 로 설명할 수 있습니다.
navigation2_active.launch.py의 지도 파일을 바꿔 빌드 · 실행하고, Nav2 의 BT Navigator · 전역 계획기 · 지역 컨트롤러 · 코스트맵 · 복구 행동을 ch08 의 개념과 짝지으며, RViz 의 Nav2 Goal · 웨이포인트 모드와nav2_simple_commander로 목표를 보낼 수 있습니다.max_vel_x·max_vel_y·inflation_radius·xy_goal_tolerance·yaw_goal_tolerance를 바꾸며 목표 앞 맴돌기 · 좁은 통로 실패 · 과속 코너를 재현하고, 증상에서 조절할 파라미터를 고를 수 있습니다. Rtabmap(3D) 매핑과 Isaac Sim 시뮬레이션이 무엇인지 소개할 수 있습니다.
이 페이지에는 브라우저에서 바로 동작하는 시뮬레이터 5개가 있습니다. ROS 2 가 없는 컴퓨터에서도 지도 작성 · 저장, map.yaml 해석, AMCL 파티클, Nav2 웨이포인트 순회, Nav2 파라미터 튜닝을 직접 조작할 수 있습니다. 조작하기 전에 결과를 먼저 예측해 적고, 확인하세요. 시뮬레이터 1 에서 저장한 지도는 시뮬레이터 2 가 그대로 읽습니다. Playground 실습(표준 라이브러리 + numpy)은 시뮬레이터와 같은 규칙으로 만들었습니다. 실제 ROS 2 코드는 "실물 전용" 블록으로 따로 둡니다.
공식 자료 기준 (myagvPlus_docs 6.2.4 · 6.2.5 · 6.2.6 · 6.3): 지도 작성은 ros2 launch myagv_plus_bringup myagv_plus_bringup.launch.py → ros2 launch slam_gmapping slam_gmapping.launch.py → ros2 run teleop_twist_keyboard teleop_twist_keyboard, 저장은 ros2 run nav2_map_server map_saver_cli -f ~/myagv_plus_ros2/src/myagv_plus_navigation2/map/map_test. 내비게이션은 navigation2_active.launch.py 의 map_file 을 /map/map_test.yaml 로 바꾸고 colcon build --packages-select myagv_plus_navigation2 → source install/setup.bash → bringup → ros2 launch myagv_plus_navigation2 navigation2_active.launch.py. RViz 의 2D Pose Estimate, Nav2 Goal, Waypoint / Nav Through Poses Mode → Start Waypoint Following. Rtabmap 은 Orbbec 카메라(astra_pro2.launch.py)와 rtabmap_demos, 지도는 ~/.ros/rtabmap.db. Isaac Sim 은 RTX 4080 이상 · Isaac Sim 6.0.1 · Ubuntu 22.04 + Humble. 참고한 공개 설정 (이전 모델용): myagv_ros2 (galactic-JN) 의 myagv.yaml — AMCL max_particles 2000 · min_particles 500 · likelihood_field, controller_frequency 5.0, DWB max_vel_x 0.26 · max_vel_y 0.0 · max_vel_theta 0.5, xy_goal_tolerance 0.25 · yaw_goal_tolerance 0.25, inflation_radius 0.25 · cost_scaling_factor 5.0, robot_radius 0.22, 진행 검사 0.5 m / 10 s. myAGV Plus 의 실제 파라미터 파일 이름과 값은 확인 필요입니다(로봇의 myagv_plus_navigation2/param 폴더에서 확인). 교육용 가정: 시뮬레이터의 창고 지도, 오도메트리 오차(거리 +3 %), 지도 작성 자세 오차 모델, AMCL 의 가중치 식(로그 가능도 × 0.2), 경로 추종기(5 Hz, 명령 지연 1 주기, 최소 회전 속도 0.4 rad/s, 제자리 회전 각가속도 1 rad/s²), 코스트맵 cost_scaling_factor 3.0.
1교시 · Gmapping 으로 지도 만들고 저장하기#
오늘의 큰 그림: 지도 → 저장 → 위치 추정 → 주행#
ch07 에서는 SLAM 의 원리(스캔 매칭 · 파티클 · 루프 닫기)를, ch08 에서는 지도 위 경로 계획을, ch09 에서는 ROS 2 의 노드 · 토픽 · 빌드를 배웠습니다. 이 차시는 셋을 myAGV Plus 의 실제 ROS 2 패키지로 이어 붙입니다.
핵심은 "지도는 한 번 만들고, 주행할 때는 지도를 고정한 채 위치만 찾는다" 는 것입니다. SLAM 은 지도와 위치를 동시에 추정하느라 계산이 많고 지도가 계속 바뀝니다. 물류 현장에서는 지도를 잘 만들어 저장해 두고, 매일의 주행은 그 지도 위에서 AMCL(2교시)로 위치만 찾습니다.
공식 순서: bringup → slam_gmapping → teleop#
공식 문서 6.2.4 의 순서입니다. 터미널을 세 개 엽니다(Ctrl + Alt + T).
# 터미널 1: 로봇 기동 (LiDAR + 하위 보드 통신)
ros2 launch myagv_plus_bringup myagv_plus_bringup.launch.py
# 터미널 2: Gmapping 시작 (RViz 가 함께 뜨며 /map 이 자라는 모습이 보임)
ros2 launch slam_gmapping slam_gmapping.launch.py
# 터미널 3: 키보드 조종 - 이 터미널을 클릭해 "선택된 터미널" 로 둔 채 키를 누름
ros2 run teleop_twist_keyboard teleop_twist_keyboard- 키 배치는 ch09 와 같습니다:
i전진,,후진,j/l제자리 회전,u/o/m/.대각,k정지, Shift +J/L옆으로 평행 이동,q/z속도 ±10 %. - 공식 문서의 권고는 "기본 속도를 쓰라, 낮은 속도가 대체로 더 좋은 지도를 만든다" 입니다(ch07 4교시에서 이유를 확인했습니다).
- 지도가 자라는 동안 RViz 에서 회색(미확인) → 흰색(빈 칸) · 검정(벽) 으로 바뀌는 영역을 봅니다. 선반 사이 통로는 정면으로 한 번씩 들여다봐야 채워집니다.
지도를 만드는 동안에는 지도가 계속 바뀌고, 로봇 위치도 SLAM 이 직접 정합니다(map → odom TF 를 Gmapping 이 발행). 이때 AMCL 까지 켜면 두 노드가 같은 TF 를 서로 다르게 발행해 로봇이 지도 위에서 튑니다. 지도 작성 중에는 slam_gmapping, 주행 중에는 AMCL 하나만 map → odom 을 냅니다.
시뮬레이터 1 · 선반 통로 지도 작성과 map_saver_cli#
ch07 의 "선반 통로" 창고(12 m × 8 m)입니다. 키 버튼(teleop 과 같은 배치) 또는 ▶ 자동 경로로 돌며 지도를 만들고, 💾 map_saver_cli 로 저장합니다. 이 시뮬레이터의 Gmapping 은 "빠르게 움직일수록 스캔을 붙이는 자세에 오차가 남는다" 는 성질만 단순하게 흉내 냅니다(교육용 가정 — 0.3 m/s 에서 거의 0, 1.0 m/s 에서 약 10 cm).
- 기본 속도(0.3 m/s)로 ▶ 자동 경로로 한 바퀴 를 끝까지 돌립니다. 통로 안쪽 · 선반 뒤쪽 중 어디가 마지막까지 회색으로 남을까요? 끝난 뒤 💾 로 저장하고 "알려진 빈 칸" 과 "유령 벽" 을 적습니다.
- ↺ 처음부터 → teleop 속도를 1.0 m/s 로 올리고 다시 한 바퀴 돕니다. "실제 창고 윤곽 겹쳐 보기" 를 켜고 벽이 어떻게 달라졌는지 봅니다. 벽으로 확정된 셀 수는 늘까요, 줄까요?
- 키 버튼으로 직접 조종해 봅니다.
j/l로 제자리에서 빠르게 돌 때와i로 직진할 때, 어느 쪽에서 지도가 더 번지나요? - 지도를 반쯤만 만든 상태에서 💾 를 눌러 봅니다. 저장된 pgm 의 회색 영역은 다음 시뮬레이터에서 어떻게 해석될까요?
- 0.3 m/s 한 바퀴(약 39 m)면 실제 빈 칸의 약 96 % 가 알려지고 유령 벽은 0 입니다. 선반 안쪽은 LiDAR 가 들어갈 수 없으므로 끝까지 회색이 정상입니다.
- 1.0 m/s 에서는 스캔을 붙이는 자세가 10 cm 안팎 흔들려, 한 번 벽으로 찍힌 칸을 다음 스캔이 "빈 칸" 으로 지우기도 합니다. 그래서 벽 셀 수가 절반 가까이 줄고(벽에 구멍), 제자리가 아닌 유령 벽이 수십 개 생깁니다. 구멍 난 벽은 2교시의 AMCL 이 붙잡을 특징을 잃게 하고, 3교시의 계획기가 벽을 뚫는 경로를 만들게 할 수 있습니다.
- 저장된 yaml 의 origin 은 [-1.000, -0.700, 0] 입니다. Gmapping 의
map원점은 지도 작성을 시작한 로봇 자세이고, 이미지 왼쪽 아래 모서리가 그 원점에서 (−1.0, −0.7) m 떨어져 있다는 뜻입니다. 실제 Gmapping 지도는 여백을 두고 더 크게 만들어지므로 origin 값이 이보다 훨씬 큰 음수인 경우가 많습니다.
지도 저장: map_saver_cli#
지도가 마음에 들면 Gmapping 을 끄기 전에 네 번째 터미널에서 저장합니다.
ros2 run nav2_map_server map_saver_cli -f ~/myagv_plus_ros2/src/myagv_plus_navigation2/map/map_test
ls ~/myagv_plus_ros2/src/myagv_plus_navigation2/map/
# map_test.pgm map_test.yaml (원래 있던 map.pgm · map.yaml 도 보임)-f뒤에는 확장자 없이 경로 + 파일 이름을 씁니다. 같은 이름이 있으면 덮어쓰니, 날짜를 붙인 이름(map_0315)으로 따로 남겨 두면 좋습니다.- 저장한 곳이
src/.../map이라는 점에 주목합니다. 소스 폴더이므로 3교시에서 다시 빌드해야 설치 폴더(install/)로 복사됩니다.
map.yaml 읽기#
| 필드 | 뜻 | 잘못 적으면 |
|---|---|---|
resolution | 픽셀 한 변의 길이(m) | 지도 전체가 늘어나거나 줄어듦. 원점 근처만 맞고 멀수록 어긋남 |
origin | 이미지 왼쪽 아래 모서리의 map 좌표 [x, y, yaw] | 지도 전체가 평행 이동. 스캔이 벽과 일정한 거리만큼 어긋남 |
occupied_thresh | 이 확률보다 크면 점유(벽) | 너무 높이면 진회색으로 칠한 벽이 "미확인" 으로 읽힘 |
free_thresh | 이 확률보다 작으면 빈 칸 | 0.196 보다 크게 하면 회색(205) 미확인 칸이 빈 칸으로 읽힘 |
negate | 1 이면 흰색이 점유 | 지도가 통째로 반전 |
mode | trinary(점유 · 빈 칸 · 미확인) 등 | — |
- 확률 p = (255 − 픽셀값) / 255 (negate 0). 0 → 1.00, 205 → 0.196, 254 → 0.004.
- origin 의 yaw 는 지원되지 않거나 무시되는 경우가 많습니다(Nav2 map_server 문서 확인). 지도를 돌리고 싶으면 이미지를 돌립니다.
- 픽셀 (열 c, 행 r) 의 중심: x = origin_x + (c + 0.5) · res, y = origin_y + (높이 − 1 − r + 0.5) · res (ch07 과 같음).
pgm 을 칠해 금지 구역 만들기#
지도는 그냥 이미지이므로 그림판 · GIMP 로 고칠 수 있습니다. 포장대 앞처럼 사람이 일하는 구역, LiDAR 높이 아래의 팔레트 · 선반 다리 처럼 지도에 안 보이는 장애물을 검정(0)으로 칠하면 계획기가 그곳을 벽으로 봅니다.
- 반드시 순수한 검정(0) 으로 칠합니다. 진회색(예: 100 → p 0.61)은 occupied_thresh 0.65 보다 작아 미확인으로 읽힙니다.
- 원본을 복사해 두고 고칩니다(
cp map_test.pgm map_test_orig.pgm). 저장할 때 형식(pgm, 8 bit 회색조)과 크기를 바꾸지 않습니다. 크기가 바뀌면 origin · resolution 과 맞지 않습니다. - 칠한 금지 구역은 지도에 영구히 들어갑니다. 시간대마다 바뀌는 구역은 Nav2 의 Keepout Filter(코스트맵 필터 — 별도 마스크 지도)를 쓰는 방법도 있습니다(Nav2 문서 참고).
시뮬레이터 2 · map.yaml 해석기: origin · resolution · 임계값 · 칠하기#
시뮬레이터 1 에서 저장한 지도(저장 전이면 자동으로 만든 지도)를 RViz 처럼 map 프레임에 그립니다. 로봇은 지도 좌표 (5.0, 2.0) 에 있고, 위치는 정확히 알고 있다고 둡니다. 그래서 빨간 스캔 점은 실제 벽 자리에 찍힙니다. yaml 값을 바꾸면 이미지만 움직입니다. 파란 선은 그 지도로 계획한 출고 목표까지의 경로입니다(로봇 반경 0.22 m · 팽창 0.35 m, 미확인 칸은 못 지나간다고 가정).
- origin x 를 −1.0 → −0.5 로 바꾸기 전에 예측합니다: 지도는 어느 쪽으로 움직이고, 빨간 스캔은 벽에서 얼마나 떨어져 보일까요? "스캔 일치율" 과 경로 칸도 봅니다.
- 기본값으로 돌리고 resolution 을 0.05 → 0.1 로 바꿉니다. 지도 좌표 원점 근처와 먼 곳 중 어디가 더 많이 어긋나나요?
- free_thresh 를 0.196 → 0.25 로 올립니다. 해석 결과의 "미확인" 칸 수가 어떻게 되나요? 지도를 덜 만든(반쯤 저장한) 지도라면 계획 경로가 어디로 지나갈 수 있을까요?
- 기본값에서 🖌 검정 0 으로 로봇 오른쪽 통로(x 6 ~ 7 m)를 가로질러 칠합니다. 경로 길이는 어떻게 바뀌나요? 같은 자리를 진회색 100 으로 칠하면? 그 상태에서 occupied_thresh 를 0.55 로 낮추면?
- negate 를 1 로 바꿔 봅니다. 왜 "시작점이 장애물" 이 될까요?
- origin 을 +0.5 m 바꾸면 지도 그림이 통째로 +x 로 0.5 m 밀리고, 스캔은 실제 자리에 그대로 있으므로 모든 벽에서 똑같이 0.5 m 어긋납니다. 일치율이 뚝 떨어지고, 로봇이 이미지 속 선반 위에 놓여 "Starting point in lethal space" 가 날 수도 있습니다. 실물에서는 AMCL 이 스캔을 지도에 맞추려고 로봇 위치를 0.5 m 옮겨 버리므로 목표 지점이 모두 0.5 m 씩 틀어집니다.
- resolution 을 두 배로 하면 지도가 origin 을 중심으로 두 배 커집니다. origin 에서 1 m 떨어진 벽은 1 m, 8 m 떨어진 벽은 8 m 어긋나 멀수록 더 틀립니다. 원점 이동(일정한 어긋남)과 구별하는 단서입니다.
- free_thresh 를 0.197 보다 크게 하면 회색(205) 칸이 모두 빈 칸으로 바뀝니다. 다 못 만든 지도라면 계획기가 가 보지도 않은 곳으로 경로를 냅니다. 저장 도구가 만든 yaml 값을 그대로 쓰는 것이 안전합니다.
- 진회색 100 은 p = 0.61 이라 occupied_thresh 0.65 아래 → 미확인으로 읽힙니다(이 시뮬레이터는 미확인을 막힌 곳으로 보므로 경로는 피해 가지만, 실제 Nav2 에서 미확인을 지나갈 수 있게 설정했다면 그대로 지나갑니다). 금지 구역은 순수 검정으로 칠합니다.
실습 과제 1#
작은 방 지도(16 × 10 픽셀, 0.25 m)를 yaml 규칙으로 읽고, 실제 LiDAR 점이 지도 벽에 몇 % 맞는지 계산합니다. TODO 두 곳(픽셀 → 상태, 지도 좌표 → 이미지 행)을 채우세요. 예측 먼저: origin x 를 0.5 m 크게 적었을 때와 resolution 을 0.25 → 0.3 으로 적었을 때, 어느 쪽 일치율이 더 낮을까요?
import math
# 작은 방 지도 (map_saver 가 저장한 trinary 이미지라고 가정). 한 글자 = 한 픽셀
# '#' = 0 (점유) '.' = 254 (빈 칸) '?' = 205 (미확인)
IMG = [
"????????????????",
"?##############?",
"?#............#?",
"?#............#?",
"?#....####....#?",
"?#....####....#?",
"?#............#?",
"?#............#?",
"?##############?",
"????????????????",
]
PIX = {"#": 0, ".": 254, "?": 205}
YAML = {"resolution": 0.25, "origin": [-0.5, -0.5, 0.0], "negate": 0,
"occupied_thresh": 0.65, "free_thresh": 0.196}
HEIGHT, WIDTH = len(IMG), len(IMG[0])
def cell_state(v, y):
"""픽셀값 v -> 100(점유) / 0(빈 칸) / -1(미확인)"""
occ = v / 255 if y["negate"] else (255 - v) / 255
# TODO 1: occ 가 occupied_thresh 보다 크면 100, free_thresh 보다 작으면 0, 아니면 -1
return -1
def world_to_pixel(x, y, ym):
"""지도 좌표 (x, y) -> 이미지 (열 c, 행 r). 행 0 = 이미지 맨 위"""
res, (ox, oy, _) = ym["resolution"], ym["origin"]
c = math.floor((x - ox) / res)
r = math.floor((y - oy) / res) # TODO 2: 행 0 이 이미지 맨 위라는 것을 반영해 고치세요
return c, r
# 실제 방: 벽 안쪽 3.0 m x 1.5 m, 가운데 기둥 (지도 좌표, 단위 m)
# 로봇이 제자리에 있을 때 LiDAR 가 찍은 벽 · 기둥 위의 점 (교육용으로 칸 중심에 놓음)
scan = []
for k in range(14):
scan += [(-0.125 + 0.25 * k, -0.125), (-0.125 + 0.25 * k, 1.625)] # 아래 · 위 벽
for k in range(1, 7):
scan += [(-0.125, -0.125 + 0.25 * k), (3.125, -0.125 + 0.25 * k)] # 왼 · 오른 벽
scan += [(1.125, 0.625), (1.125, 0.875), (1.875, 0.625), (1.875, 0.875)] # 기둥
def match_ratio(ym):
hit = 0
for x, y in scan:
c, r = world_to_pixel(x, y, ym)
if 0 <= c < WIDTH and 0 <= r < HEIGHT and cell_state(PIX[IMG[r][c]], ym) == 100:
hit += 1
return 100 * hit / len(scan)
print("픽셀값 -> 상태 (기본 임계값)")
for v in (0, 100, 205, 254):
print(" %3d -> %4d" % (v, cell_state(v, YAML)))
print("free_thresh 0.25 이면 205 ->", cell_state(205, dict(YAML, free_thresh=0.25)))
print("occupied_thresh 0.6 이면 100 ->", cell_state(100, dict(YAML, occupied_thresh=0.6)))
print("\n설정 스캔 점이 벽에 맞는 비율")
for name, ym in [("올바른 yaml", YAML),
("origin x 를 0.5 m 크게", dict(YAML, origin=[0.0, -0.5, 0.0])),
("origin y 를 0.25 m 작게", dict(YAML, origin=[-0.5, -0.75, 0.0])),
("resolution 0.25 -> 0.3", dict(YAML, resolution=0.3))]:
print(" %-26s %5.1f %%" % (name, match_ratio(ym)))풀이 코드 보기
import math
# 작은 방 지도 (map_saver 가 저장한 trinary 이미지라고 가정). 한 글자 = 한 픽셀
# '#' = 0 (점유) '.' = 254 (빈 칸) '?' = 205 (미확인)
IMG = [
"????????????????",
"?##############?",
"?#............#?",
"?#............#?",
"?#....####....#?",
"?#....####....#?",
"?#............#?",
"?#............#?",
"?##############?",
"????????????????",
]
PIX = {"#": 0, ".": 254, "?": 205}
YAML = {"resolution": 0.25, "origin": [-0.5, -0.5, 0.0], "negate": 0,
"occupied_thresh": 0.65, "free_thresh": 0.196}
HEIGHT, WIDTH = len(IMG), len(IMG[0])
def cell_state(v, y):
"""픽셀값 v -> 100(점유) / 0(빈 칸) / -1(미확인)"""
occ = v / 255 if y["negate"] else (255 - v) / 255
if occ > y["occupied_thresh"]:
return 100
if occ < y["free_thresh"]:
return 0
return -1
def world_to_pixel(x, y, ym):
"""지도 좌표 (x, y) -> 이미지 (열 c, 행 r). 행 0 = 이미지 맨 위"""
res, (ox, oy, _) = ym["resolution"], ym["origin"]
c = math.floor((x - ox) / res)
r = HEIGHT - 1 - math.floor((y - oy) / res)
return c, r
# 실제 방: 벽 안쪽 3.0 m x 1.5 m, 가운데 기둥 (지도 좌표, 단위 m)
# 로봇이 제자리에 있을 때 LiDAR 가 찍은 벽 · 기둥 위의 점 (교육용으로 칸 중심에 놓음)
scan = []
for k in range(14):
scan += [(-0.125 + 0.25 * k, -0.125), (-0.125 + 0.25 * k, 1.625)] # 아래 · 위 벽
for k in range(1, 7):
scan += [(-0.125, -0.125 + 0.25 * k), (3.125, -0.125 + 0.25 * k)] # 왼 · 오른 벽
scan += [(1.125, 0.625), (1.125, 0.875), (1.875, 0.625), (1.875, 0.875)] # 기둥
def match_ratio(ym):
hit = 0
for x, y in scan:
c, r = world_to_pixel(x, y, ym)
if 0 <= c < WIDTH and 0 <= r < HEIGHT and cell_state(PIX[IMG[r][c]], ym) == 100:
hit += 1
return 100 * hit / len(scan)
print("픽셀값 -> 상태 (기본 임계값)")
for v in (0, 100, 205, 254):
print(" %3d -> %4d" % (v, cell_state(v, YAML)))
print("free_thresh 0.25 이면 205 ->", cell_state(205, dict(YAML, free_thresh=0.25)))
print("occupied_thresh 0.6 이면 100 ->", cell_state(100, dict(YAML, occupied_thresh=0.6)))
print("\n설정 스캔 점이 벽에 맞는 비율")
for name, ym in [("올바른 yaml", YAML),
("origin x 를 0.5 m 크게", dict(YAML, origin=[0.0, -0.5, 0.0])),
("origin y 를 0.25 m 작게", dict(YAML, origin=[-0.5, -0.75, 0.0])),
("resolution 0.25 -> 0.3", dict(YAML, resolution=0.3))]:
print(" %-26s %5.1f %%" % (name, match_ratio(ym)))실행 결과:
픽셀값 -> 상태 (기본 임계값)
0 -> 100
100 -> -1
205 -> -1
254 -> 0
free_thresh 0.25 이면 205 -> 0
occupied_thresh 0.6 이면 100 -> 100
설정 스캔 점이 벽에 맞는 비율
올바른 yaml 100.0 %
origin x 를 0.5 m 크게 59.1 %
origin y 를 0.25 m 작게 36.4 %
resolution 0.25 -> 0.3 50.0 %- 픽셀 100 은 p ≈ 0.61 이라 기본 임계값에서 미확인(−1) 입니다. occupied_thresh 를 0.6 으로 낮춰야 벽이 됩니다.
- free_thresh 를 0.25 로 올리면 205(p ≈ 0.196)가 빈 칸(0) 이 됩니다. 시뮬레이터 2 의 3번과 같은 결과입니다.
- origin 을 틀리면 지도 전체가 같은 거리만큼 밀려 일치율이 크게 떨어집니다. resolution 을 틀리면 원점 가까운 벽은 맞고 먼 벽만 틀려 "일부는 맞는" 지도가 되므로 더 알아채기 어렵습니다.
HEIGHT - 1 - floor(...)의 뒤집기를 빼먹으면(TODO 2) 올바른 yaml 에서도 일치율이 낮게 나옵니다. ch07 오류 6 과 같은 실수입니다.
선반 통로 지도를 0.1 m 이미지로 만들고, 포장대 앞 통로(x 5.5 ~ 6.5 m, y 2.0 ~ 3.45 m)를 금지 구역으로 칠한 뒤, 로봇(반경 0.2 m)의 A → B 최단 경로가 얼마나 늘어나는지 봅니다. 마지막에 P2(텍스트) pgm 과 yaml 을 만듭니다. TODO 두 곳을 채우세요.
from collections import deque
import numpy as np
# 선반 통로 맵 (12 m x 8 m, 교육용) 을 0.1 m 로 저장한 지도라고 가정
RES, W, H = 0.1, 12.0, 8.0
ORIGIN = (0.0, 0.0) # 이미지 왼쪽 아래 모서리의 지도 좌표
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8),
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), (10.6, 3.8, 11.0, 4.2)]
NX, NY = round(W / RES), round(H / RES)
# grid[j, i]: 행 j = 아래(y 작은 쪽)부터. 값은 pgm 픽셀값 (0 점유, 254 빈 칸)
grid = np.full((NY, NX), 254, dtype=np.uint8)
cx = ORIGIN[0] + (np.arange(NX) + 0.5) * RES
cy = ORIGIN[1] + (np.arange(NY) + 0.5) * RES
for x0, y0, x1, y1 in BOXES:
grid[np.ix_((cy > y0) & (cy < y1), (cx > x0) & (cx < x1))] = 0
def paint(img, x0, y0, x1, y1, value=0):
"""지도 좌표 사각형을 pgm 이미지(행 0 = 맨 위)에 칠함"""
h = img.shape[0]
c0 = int(np.floor((x0 - ORIGIN[0]) / RES))
c1 = int(np.ceil((x1 - ORIGIN[0]) / RES))
# TODO 1: y 가 클수록 이미지 위쪽(행 번호 작음)입니다. 지금은 아래에서부터 센 행 번호입니다
r_top = int(np.floor((y0 - ORIGIN[1]) / RES))
r_bot = int(np.ceil((y1 - ORIGIN[1]) / RES))
img[max(0, r_top):r_bot, max(0, c0):c1] = value
def path_len(img, s, g, robot_r=0.2):
"""로봇 반지름만큼 장애물을 키운 뒤 4방향 BFS 로 최단 경로 길이 (m)"""
occ = np.flipud(img) == 0 # 다시 행 0 = 아래 로
k = int(round(robot_r / RES))
big = occ.copy()
for dj in range(-k, k + 1):
for di in range(-k, k + 1):
if di * di + dj * dj <= k * k:
big |= np.roll(np.roll(occ, dj, 0), di, 1)
si, sj, gi, gj = int(s[0] / RES), int(s[1] / RES), int(g[0] / RES), int(g[1] / RES)
dist = np.full(big.shape, np.inf)
dist[sj, si] = 0
q = deque([(si, sj)])
while q: # 4방향 BFS: 칸 수 x RES = 길이 (맨해튼)
i, j = q.popleft()
for a, b in ((i + 1, j), (i - 1, j), (i, j + 1), (i, j - 1)):
if 0 <= a < NX and 0 <= b < NY and not big[b, a] and dist[b, a] == np.inf:
dist[b, a] = dist[j, i] + 1
q.append((a, b))
return dist[gj, gi] * RES
img = np.flipud(grid).copy() # 저장용: 행 0 = 이미지 맨 위
A, B = (1.6, 2.7), (9.0, 2.7)
print("금지 구역 전 A -> B 경로: %.1f m" % path_len(img, A, B))
paint(img, 5.5, 2.0, 6.5, 3.45) # 포장대 앞 통로 (x 5.5~6.5) 를 금지 구역으로
print("금지 구역 후 A -> B 경로: %.1f m" % path_len(img, A, B))
r, c = NY - 1 - int(2.7 / RES), int(6.0 / RES)
print("금지 구역 가운데 (6.0, 2.7) 픽셀: 행 %d, 열 %d, 값 %d" % (r, c, img[r, c]))
pgm = "P2\n" # TODO 2: 둘째 줄에 '폭 높이', 셋째 줄에 최댓값 255 를 넣으세요
pgm += "\n".join(" ".join(str(v) for v in row) for row in img)
yaml = ("image: map_keepout.pgm\nmode: trinary\nresolution: %.2f\norigin: [%.1f, %.1f, 0.0]\n"
"negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n" % (RES, ORIGIN[0], ORIGIN[1]))
print("\n--- map_keepout.pgm (앞 3줄) ---")
print("\n".join(pgm.split("\n")[:3]))
print("--- map_keepout.yaml ---")
print(yaml)풀이 코드 보기
from collections import deque
import numpy as np
# 선반 통로 맵 (12 m x 8 m, 교육용) 을 0.1 m 로 저장한 지도라고 가정
RES, W, H = 0.1, 12.0, 8.0
ORIGIN = (0.0, 0.0) # 이미지 왼쪽 아래 모서리의 지도 좌표
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8),
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), (10.6, 3.8, 11.0, 4.2)]
NX, NY = round(W / RES), round(H / RES)
# grid[j, i]: 행 j = 아래(y 작은 쪽)부터. 값은 pgm 픽셀값 (0 점유, 254 빈 칸)
grid = np.full((NY, NX), 254, dtype=np.uint8)
cx = ORIGIN[0] + (np.arange(NX) + 0.5) * RES
cy = ORIGIN[1] + (np.arange(NY) + 0.5) * RES
for x0, y0, x1, y1 in BOXES:
grid[np.ix_((cy > y0) & (cy < y1), (cx > x0) & (cx < x1))] = 0
def paint(img, x0, y0, x1, y1, value=0):
"""지도 좌표 사각형을 pgm 이미지(행 0 = 맨 위)에 칠함"""
h = img.shape[0]
c0 = int(np.floor((x0 - ORIGIN[0]) / RES))
c1 = int(np.ceil((x1 - ORIGIN[0]) / RES))
r_top = h - int(np.ceil((y1 - ORIGIN[1]) / RES)) # y 가 클수록 위 (행 번호 작음)
r_bot = h - int(np.floor((y0 - ORIGIN[1]) / RES))
img[max(0, r_top):r_bot, max(0, c0):c1] = value
def path_len(img, s, g, robot_r=0.2):
"""로봇 반지름만큼 장애물을 키운 뒤 4방향 BFS 로 최단 경로 길이 (m)"""
occ = np.flipud(img) == 0 # 다시 행 0 = 아래 로
k = int(round(robot_r / RES))
big = occ.copy()
for dj in range(-k, k + 1):
for di in range(-k, k + 1):
if di * di + dj * dj <= k * k:
big |= np.roll(np.roll(occ, dj, 0), di, 1)
si, sj, gi, gj = int(s[0] / RES), int(s[1] / RES), int(g[0] / RES), int(g[1] / RES)
dist = np.full(big.shape, np.inf)
dist[sj, si] = 0
q = deque([(si, sj)])
while q: # 4방향 BFS: 칸 수 x RES = 길이 (맨해튼)
i, j = q.popleft()
for a, b in ((i + 1, j), (i - 1, j), (i, j + 1), (i, j - 1)):
if 0 <= a < NX and 0 <= b < NY and not big[b, a] and dist[b, a] == np.inf:
dist[b, a] = dist[j, i] + 1
q.append((a, b))
return dist[gj, gi] * RES
img = np.flipud(grid).copy() # 저장용: 행 0 = 이미지 맨 위
A, B = (1.6, 2.7), (9.0, 2.7)
print("금지 구역 전 A -> B 경로: %.1f m" % path_len(img, A, B))
paint(img, 5.5, 2.0, 6.5, 3.45) # 포장대 앞 통로 (x 5.5~6.5) 를 금지 구역으로
print("금지 구역 후 A -> B 경로: %.1f m" % path_len(img, A, B))
r, c = NY - 1 - int(2.7 / RES), int(6.0 / RES)
print("금지 구역 가운데 (6.0, 2.7) 픽셀: 행 %d, 열 %d, 값 %d" % (r, c, img[r, c]))
pgm = "P2\n%d %d\n255\n" % (NX, NY) + "\n".join(" ".join(str(v) for v in row) for row in img)
yaml = ("image: map_keepout.pgm\nmode: trinary\nresolution: %.2f\norigin: [%.1f, %.1f, 0.0]\n"
"negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n" % (RES, ORIGIN[0], ORIGIN[1]))
print("\n--- map_keepout.pgm (앞 3줄) ---")
print("\n".join(pgm.split("\n")[:3]))
print("--- map_keepout.yaml ---")
print(yaml)실행 결과:
금지 구역 전 A -> B 경로: 7.4 m
금지 구역 후 A -> B 경로: 11.8 m
금지 구역 가운데 (6.0, 2.7) 픽셀: 행 52, 열 60, 값 0
--- map_keepout.pgm (앞 3줄) ---
P2
120 80
255
--- map_keepout.yaml ---
image: map_keepout.pgm
mode: trinary
resolution: 0.10
origin: [0.0, 0.0, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196- 지도 좌표의 y 가 클수록 이미지 위쪽(행 번호가 작은 쪽) 입니다.
r_top = h − ceil((y1 − oy) / res)처럼 높이에서 빼야 합니다. 빼먹으면 금지 구역이 위아래로 뒤집힌 자리(엉뚱한 통로)에 칠해져 경로가 그대로입니다. - 가운데 통로를 막으니 7.4 m 가 11.8 m 로 늘었습니다. 로봇은 선반 1 의 끝을 돌아 아래 통로로 우회합니다(4방향 이동 기준의 길이).
- 이미지 크기(120 × 80)와 origin 을 바꾸지 않았으므로 yaml 은 저장 도구가 만든 것과 같은 값입니다.
RViz 에서 스캔이 지도 벽과 원점 근처에서는 거의 맞고, 원점에서 멀수록 점점 더 많이 어긋나 보입니다. 가장 먼저 의심할 yaml 항목은?
- origin
- resolution
- free_thresh
- negate
정답 보기
② — 해상도를 틀리면 지도가 origin 을 중심으로 늘어나거나 줄어들어, 어긋남이 거리에 비례합니다. origin 을 틀리면 모든 곳에서 **같은 거리**만큼 어긋나고, negate 는 지도 전체가 반전, free_thresh 는 회색 칸의 해석만 바꿉니다.pgm 에 금지 구역을 칠했는데 RViz 에서 그 자리가 검정이 아니라 회색으로 보이고, 로봇이 그곳을 지나가는 경로를 냈습니다. yaml 은 occupied_thresh: 0.65, free_thresh: 0.196 입니다. 가장 가능성 높은 원인은?
- 금지 구역을 픽셀값 100 정도의 진회색으로 칠했다
- 금지 구역을 순수한 검정(0)으로 칠했다
- resolution 을 0.05 로 적었다
- 지도를 저장한 뒤 colcon build 를 했다
정답 보기
① — 100 은 p = (255 − 100) / 255 ≈ 0.61 로 occupied_thresh 0.65 보다 작아 **미확인**이 됩니다. 미확인을 지나갈 수 있게 설정된 계획기(Nav2 의 `allow_unknown`)는 그대로 지나갑니다. 금지 구역은 0 으로 칠합니다.시뮬레이터 1 에서 1.0 m/s 로 지도를 만들었더니 0.3 m/s 때보다 "벽으로 확정된 셀" 이 절반 가까이 줄었습니다. 그 이유로 가장 알맞은 것은?
- 빠르면 LiDAR 의 최대 거리가 줄어든다
- 스캔을 붙이는 자세 오차가 커져, 한 번 벽으로 찍힌 칸을 다음 스캔이 빈 칸으로 지운다
- 빠르면 지도 해상도가 자동으로 낮아진다
- map_saver_cli 가 빠른 지도는 일부만 저장한다
정답 보기
② — 같은 벽을 여러 스캔이 조금씩 다른 자리에 찍으면 로그 오즈가 "벽" 과 "빈 칸" 사이에서 오가 확정되지 못하거나 지워집니다(ch07 의 로그 오즈). 그래서 공식 문서도 낮은 속도를 권합니다.2교시 · 위치 추정: AMCL 과 2D Pose Estimate#
지도가 있으면 할 일은 "내가 어디 있나" 뿐#
저장한 지도를 불러오면 지도는 더 이상 바뀌지 않습니다. 남은 문제는 그 지도 위 어디에 로봇이 있는가 입니다. 오도메트리(ch06)만 쓰면 바퀴 미끄러짐이 쌓여 점점 틀어지므로, LiDAR 스캔을 지도와 비교해 계속 바로잡아야 합니다. Nav2 의 기본 위치 추정기가 AMCL(Adaptive Monte Carlo Localization, 적응형 몬테카를로 위치 추정)입니다.
AMCL 의 원리: 가설 로봇 수백 개#
- 흩뿌리기: "로봇이 여기 있을지도" 라는 가설(파티클)을 수백 ~ 수천 개 만듭니다. 각 파티클은 (x, y, θ) 와 가중치를 가집니다.
- 이동(예측): 오도메트리가 "0.1 m 앞으로 갔다" 고 하면 모든 파티클을 0.1 m 씩 옮기고, 바퀴를 다 믿을 수 없으니 잡음을 더해 조금 퍼뜨립니다.
- 가중치(보정): 각 파티클 자리에 서서 지금 스캔을 지도에 겹쳐 봅니다. 스캔 끝점이 지도의 벽에 가까울수록 높은 점수를 받습니다(likelihood field 모델 — 끝점에서 가장 가까운 벽까지 거리로 점수).
- 리샘플링: 점수가 높은 파티클은 여러 번 복사하고 낮은 것은 버립니다. 이를 되풀이하면 파티클이 실제 위치 근처로 모입니다.
- AMCL 의 "적응형(Adaptive)" 은 파티클이 한곳에 모이면 수를 줄이고(
min_particles), 퍼지면 늘리는 것(max_particles)을 뜻합니다(KLD 샘플링). 이 시뮬레이터는 수를 고정합니다. - 파티클은 로봇이
update_min_d(이동 거리) ·update_min_a(회전각) 이상 움직였을 때만 갱신됩니다. 로봇이 가만히 있으면 수렴하지 않습니다. 초기 위치를 준 뒤 조금 움직여야 하는 이유입니다.
2D Pose Estimate 를 주는 이유와 "납치된 로봇"#
AMCL 은 처음에 파티클을 어디에 뿌릴지 모릅니다. RViz 의 2D Pose Estimate 버튼으로 지도 위 로봇 자리를 누르고(위치) 끌어(방향) 알려 주면, 그 주변에 파티클을 뿌립니다(표준편차 약 0.5 m · 15°). 공식 문서도 "초록 화살표 = 로봇 앞쪽, 스캔이 지도와 맞는지 확인" 을 안내합니다.
| 상황 | 무엇이 일어나나 |
|---|---|
| 정확히 줌 | 몇 번의 갱신 만에 한 점으로 모임 |
| 조금(0.5 ~ 1 m) 틀림 | 뿌린 범위 안에 정답이 있으므로 곧 바로잡힘 |
| 많이(2 m 이상) 틀림 | 정답 근처에 파티클이 없음. 잡음으로 조금씩 퍼지다 특징을 만나야 바로잡히거나, 엉뚱한 곳에 자신 있게 모임 |
| 납치된 로봇(kidnapped robot) | 전원을 켠 채 사람이 로봇을 들어 옮김. 파티클은 옛 자리에 모여 있어 스스로 못 돌아옴 |
납치 문제에 대비해 AMCL 은 점수가 갑자기 나빠지면 무작위 파티클을 섞어 넣는 기능(recovery_alpha_slow · recovery_alpha_fast)이 있지만 Nav2 기본값은 꺼짐(0)입니다. 현장에서는 보통 2D Pose Estimate 를 다시 주거나, 전역 위치 추정 서비스(/reinitialize_global_localization)로 지도 전체에 파티클을 다시 뿌립니다.
TF 로 보는 위치 보정: map → odom#
ch02 에서 TF 트리는 한 프레임에 부모가 하나뿐이라고 배웠습니다. odom → base_footprint 는 이미 오도메트리(ch06 EKF)가 발행하므로, AMCL 은 자기가 찾은 map → base_footprint 를 직접 내지 못하고 **둘의 차이 map → odom** 을 계산해 냅니다.
- 로봇 위치 = (map → odom) × (odom → base_footprint). 오도메트리가 매끄럽게 움직임을 채우고, AMCL 이 가끔 map → odom 을 고쳐 쌓인 흐름을 지웁니다.
- AMCL 이 아직 초기 위치를 못 받았으면 map → odom 이 없어 TF 트리가 끊깁니다. 이때 Nav2 가 내는 오류가 4교시의 "Timed out waiting for transform" 입니다.
# 로봇에서 확인 (ch09 의 도구)
ros2 run tf2_ros tf2_echo map odom # AMCL 이 발행하는 보정값
ros2 run tf2_ros tf2_echo map base_footprint # 지도 위 로봇 위치
ros2 topic echo /amcl_pose --once # 추정 위치 + 공분산시뮬레이터 3 · AMCL 파티클 보기#
로봇(초록)은 정해진 경로를 0.4 m/s 로 돕니다. 오도메트리는 거리를 3 % 길게 잽니다(교육용 가정). AMCL 은 0.1 m 이동 또는 0.1 rad 회전마다 36 개 빔으로 갱신합니다. 지도를 누른 채 끌면 RViz 처럼 2D Pose Estimate 를 줄 수 있습니다. 아래 그래프는 위치 오차(주황)와 파티클 퍼짐(파랑)입니다.
- 🎯 정확히 → ▶ 주행. 파티클은 몇 m 만에 한 점으로 모이나요? 표의 map → odom 값이 주행하는 동안 어떻게 변하는지 봅니다(오도메트리가 3 % 길게 재는 것을 무엇이 보정하나요?).
- ↔ 2 m 어긋나게 를 누르기 전에 예측을 적습니다: 몇 m 주행 후 수렴할까요? 확인한 뒤 📊 2 m 어긋남 수렴 거리 로 세 번 잽니다.
- 장소를 긴 복도로 바꿔 1 ~ 2 번을 되풀이합니다. 정확히 줬을 때도 파티클 모양이 어떻게 다른가요(어느 방향으로 길게 퍼지나요)?
- 파티클 수를 100 → 2000, LiDAR 잡음을 0.03 → 0.15 로 바꿔 📊 를 다시 잽니다. 수렴 거리가 크게 달라지나요?
- 수렴한 상태에서 🤖 납치 를 누릅니다. 그대로 두면? 🌐 전역 위치 추정 또는 지도를 끌어 다시 2D Pose Estimate 를 주면? "무작위 파티클 주입" 을 켜고 다시 납치해 봅니다.
- 정확히 주거나 1 m 이내로 틀리면 첫 갱신 몇 번(1 m 이내) 만에 수렴합니다. 2 m 틀리면 선반 통로에서 약 15 m 를 달린 뒤에야 바로잡힙니다(📊: 15.2 · 15.5 · 15.4 m). 처음 뿌린 범위(약 0.5 m) 안에 정답이 없기 때문입니다. 실물이라면 그동안 Nav2 가 틀린 위치로 주행합니다. 수렴할 때까지 기다리지 말고 2D Pose Estimate 를 다시 주는 것이 답입니다.
- 긴 복도에서는 옆벽만 보이므로 좌우(y) · 방향(θ)은 금방 맞지만 앞뒤(x)로 파티클이 길게 퍼집니다. 2 m 틀리면 복도 끝 벽이 보일 때(약 18 ~ 21 m)까지 못 고칩니다. 출발점 근처에 끝 벽이 보이면 1 m 오차는 바로 고쳐지는 것과 비교해 보세요.
- 파티클 수를 늘려도 "정답 근처에 파티클이 없는" 문제는 풀리지 않아 2 m 수렴 거리는 거의 같습니다(2000 개: 약 15 m). 파티클 수는 퍼진 상황을 얼마나 촘촘히 덮느냐의 문제이고, 계산량은 비례해서 늘어납니다.
- LiDAR 잡음이 크면 가중치가 덜 날카로워져(σ 를 키워야 하므로) 수렴한 뒤에도 퍼짐이 커집니다.
- 납치 뒤에는 파티클이 옛 자리에 "자신 있게" 모여 있어 퍼짐은 작은데 오차는 큽니다. 퍼짐이 작다고 위치가 맞는 것은 아닙니다. 무작위 주입은 도움이 될 때도 있지만, 비슷하게 생긴 통로가 많으면 엉뚱한 곳으로 튀기도 합니다(그래서 Nav2 기본값이 꺼짐).
실습 과제 2#
시뮬레이터 3 과 같은 규칙(likelihood field, 로그 가능도 × 0.2, 저분산 리샘플링, 오도메트리 +3 %)을 numpy 로 옮겼습니다(파티클 200 개, 빔 24 개, 0.1 m 격자). TODO 두 곳(빔 하나의 확률, 리샘플링)을 채우고, 두 장소 × 세 초기 오차 × 시드 3 개의 수렴 거리를 표로 만드세요. 실행에 5 ~ 15 초 걸립니다. 예측 먼저: 긴 복도에서 1 m 틀린 경우와 선반 통로에서 2 m 틀린 경우, 어느 쪽이 더 오래 걸릴까요?
import math
import numpy as np
# 교육용 맵. 사각형 (x0, y0, x1, y1)
MAPS = {
"선반 통로": dict(W=12, H=8, boxes=[(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8),
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), (10.6, 3.8, 11.0, 4.2)],
route=[(1.6, 2.7), (10.2, 2.7), (10.2, 6.5), (1.6, 6.5), (1.6, 0.75), (10.2, 0.75), (10.2, 2.7)]),
"긴 복도": dict(W=20, H=1.6, boxes=[(0, 0, 20, 0.1), (0, 1.5, 20, 1.6), (0, 0, 0.1, 1.6), (19.9, 0, 20, 1.6)],
route=[(1.0, 0.8), (19.0, 0.8), (1.0, 0.8)]),
}
RES, BEAMS, RMAX, SIGMA, TEMP, ALPHA = 0.1, 24, 8.0, 0.1, 0.2, 0.2
def build(m):
nx, ny = round(m["W"] / RES), round(m["H"] / RES)
occ = np.zeros((ny, nx), bool)
cx, cy = (np.arange(nx) + 0.5) * RES, (np.arange(ny) + 0.5) * RES
for x0, y0, x1, y1 in m["boxes"]:
occ[np.ix_((cy > y0) & (cy < y1), (cx > x0) & (cx < x1))] = True
oj, oi = np.nonzero(occ)
df = np.zeros((ny, nx), np.float32)
for j in range(ny): # 가장 가까운 점유 셀까지 거리 (likelihood field)
df[j] = np.sqrt(((np.arange(nx)[:, None] - oi) ** 2 + (j - oj) ** 2).min(1)) * RES
segs = []
for x0, y0, x1, y1 in m["boxes"]:
segs += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
return occ, np.minimum(df, 2.0), segs
def cast(segs, px, py, a):
dx, dy, best = math.cos(a), math.sin(a), RMAX + 1
for x0, y0, x1, y1 in segs:
sx, sy = x1 - x0, y1 - y0
den = dx * sy - dy * sx
if abs(den) < 1e-12:
continue
qx, qy = x0 - px, y0 - py
t, u = (qx * sy - qy * sx) / den, (qx * dy - qy * dx) / den
if 1e-9 < t < best and 0 <= u <= 1:
best = t
return best
ANG = -math.pi + np.arange(BEAMS) * 2 * math.pi / BEAMS
def weigh(P, ranges, occ, df):
"""파티클마다 스캔이 지도와 맞는 정도 -> 정규화된 가중치"""
ok = np.isfinite(ranges)
r, a = ranges[ok], ANG[ok]
ex = P[:, 0:1] + r * np.cos(P[:, 2:3] + a) # 빔 끝점 (지도 좌표)
ey = P[:, 1:2] + r * np.sin(P[:, 2:3] + a)
ny, nx = df.shape
i, j = np.floor(ex / RES).astype(int), np.floor(ey / RES).astype(int)
inside = (i >= 0) & (j >= 0) & (i < nx) & (j < ny)
z = np.where(inside, df[np.clip(j, 0, ny - 1), np.clip(i, 0, nx - 1)], 2.0)
pz = np.ones_like(z) # TODO 1: 끝점이 벽에서 z 만큼 떨어졌을 때 확률 0.95 exp(-z^2 / 2 SIGMA^2) + 0.05 / RMAX
logw = np.log(pz).sum(1)
ci = np.clip((P[:, 0] / RES).astype(int), 0, nx - 1)
cj = np.clip((P[:, 1] / RES).astype(int), 0, ny - 1)
logw[occ[cj, ci]] = -1e9 # 벽 · 선반 안 파티클은 탈락
w = np.exp(TEMP * (logw - logw.max()))
return w / w.sum()
def resample(P, w, rng):
"""저분산 리샘플링: 가중치가 큰 파티클을 여러 번 복사"""
n = len(P)
# TODO 2: 0 ~ 1/n 사이 난수 하나에서 시작해 1/n 간격으로 n 개의 바늘을 세우고,
# 누적 가중치 np.cumsum(w) 에서 각 바늘이 떨어지는 칸(np.searchsorted)을 고릅니다
return P.copy()
def run(name, offset, seed, n=200, max_d=30.0):
m = MAPS[name]
occ, df, segs = build(m)
rng = np.random.default_rng(seed)
R = m["route"]
x, y = R[0]
th = math.atan2(R[1][1] - y, R[1][0] - x)
P = np.column_stack([x + offset + 0.5 * rng.standard_normal(n), y + 0.5 * rng.standard_normal(n),
th + 0.26 * rng.standard_normal(n)]) # 2D Pose Estimate (offset 만큼 어긋남)
k, dist, good, conv, first = 1, 0.0, 0, None, 0.0
while dist < max_d:
gx, gy = R[k]
e = math.atan2(gy - y, gx - x) - th
e = (e + math.pi) % (2 * math.pi) - math.pi
if abs(e) > 0.01:
d, da = 0.0, max(-0.3, min(0.3, e))
else:
d, da = min(0.1, math.hypot(gx - x, gy - y)), 0.0
if math.hypot(gx - x, gy - y) - d < 1e-6:
k = k + 1 if k + 1 < len(R) else 1
th += da
x, y = x + d * math.cos(th), y + d * math.sin(th)
dist += d
od = d * 1.03 + 0.01 * d * rng.standard_normal() # 오도메트리 (거리 +3 %)
oa = da * 1.02
L = abs(od) # 파티클 이동 + 잡음
P[:, 2] += oa + (ALPHA * abs(oa) + 0.5 * ALPHA * L) * rng.standard_normal(n)
P[:, 0] += od * np.cos(P[:, 2]) + ALPHA * L * rng.standard_normal(n)
P[:, 1] += od * np.sin(P[:, 2]) + ALPHA * L * rng.standard_normal(n)
ranges = np.array([cast(segs, x, y, th + a) for a in ANG])
ranges[ranges > RMAX] = np.inf
ranges[np.isfinite(ranges)] += 0.03 * rng.standard_normal(np.isfinite(ranges).sum())
w = weigh(P, ranges, occ, df)
ex, ey = (w * P[:, 0]).sum(), (w * P[:, 1]).sum()
sd = math.sqrt((w * ((P[:, 0] - ex) ** 2 + (P[:, 1] - ey) ** 2)).sum())
err = math.hypot(ex - x, ey - y)
P = resample(P, w, rng)
if err < 0.3 and sd < 0.3: # 5번 연속 맞으면 수렴으로 봄
good += 1
if good == 1:
first = dist
if good == 5 and conv is None:
conv = first
else:
good = 0
if conv is not None and err > 1.0:
conv = None
return conv, err
print("맵 초기 오차 수렴까지 주행 거리 (시드 1, 2, 3)")
for name in MAPS:
for off in (0.0, 1.0, 2.0):
res = []
for seed in (1, 2, 3):
c, err = run(name, off, seed)
res.append("%5.1f m" % c if c is not None else " 실패 (오차 %.1f m)" % err)
print("%-10s %5.1f m %s" % (name, off, " ".join(res)))풀이 코드 보기
import math
import numpy as np
# 교육용 맵. 사각형 (x0, y0, x1, y1)
MAPS = {
"선반 통로": dict(W=12, H=8, boxes=[(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8),
(2.5, 1.4, 9.5, 2.0), (2.5, 3.45, 9.5, 4.05), (2.5, 4.55, 9.5, 5.15), (10.6, 3.8, 11.0, 4.2)],
route=[(1.6, 2.7), (10.2, 2.7), (10.2, 6.5), (1.6, 6.5), (1.6, 0.75), (10.2, 0.75), (10.2, 2.7)]),
"긴 복도": dict(W=20, H=1.6, boxes=[(0, 0, 20, 0.1), (0, 1.5, 20, 1.6), (0, 0, 0.1, 1.6), (19.9, 0, 20, 1.6)],
route=[(1.0, 0.8), (19.0, 0.8), (1.0, 0.8)]),
}
RES, BEAMS, RMAX, SIGMA, TEMP, ALPHA = 0.1, 24, 8.0, 0.1, 0.2, 0.2
def build(m):
nx, ny = round(m["W"] / RES), round(m["H"] / RES)
occ = np.zeros((ny, nx), bool)
cx, cy = (np.arange(nx) + 0.5) * RES, (np.arange(ny) + 0.5) * RES
for x0, y0, x1, y1 in m["boxes"]:
occ[np.ix_((cy > y0) & (cy < y1), (cx > x0) & (cx < x1))] = True
oj, oi = np.nonzero(occ)
df = np.zeros((ny, nx), np.float32)
for j in range(ny): # 가장 가까운 점유 셀까지 거리 (likelihood field)
df[j] = np.sqrt(((np.arange(nx)[:, None] - oi) ** 2 + (j - oj) ** 2).min(1)) * RES
segs = []
for x0, y0, x1, y1 in m["boxes"]:
segs += [(x0, y0, x1, y0), (x1, y0, x1, y1), (x1, y1, x0, y1), (x0, y1, x0, y0)]
return occ, np.minimum(df, 2.0), segs
def cast(segs, px, py, a):
dx, dy, best = math.cos(a), math.sin(a), RMAX + 1
for x0, y0, x1, y1 in segs:
sx, sy = x1 - x0, y1 - y0
den = dx * sy - dy * sx
if abs(den) < 1e-12:
continue
qx, qy = x0 - px, y0 - py
t, u = (qx * sy - qy * sx) / den, (qx * dy - qy * dx) / den
if 1e-9 < t < best and 0 <= u <= 1:
best = t
return best
ANG = -math.pi + np.arange(BEAMS) * 2 * math.pi / BEAMS
def weigh(P, ranges, occ, df):
"""파티클마다 스캔이 지도와 맞는 정도 -> 정규화된 가중치"""
ok = np.isfinite(ranges)
r, a = ranges[ok], ANG[ok]
ex = P[:, 0:1] + r * np.cos(P[:, 2:3] + a) # 빔 끝점 (지도 좌표)
ey = P[:, 1:2] + r * np.sin(P[:, 2:3] + a)
ny, nx = df.shape
i, j = np.floor(ex / RES).astype(int), np.floor(ey / RES).astype(int)
inside = (i >= 0) & (j >= 0) & (i < nx) & (j < ny)
z = np.where(inside, df[np.clip(j, 0, ny - 1), np.clip(i, 0, nx - 1)], 2.0)
pz = 0.95 * np.exp(-z ** 2 / (2 * SIGMA ** 2)) + 0.05 / RMAX
logw = np.log(pz).sum(1)
ci = np.clip((P[:, 0] / RES).astype(int), 0, nx - 1)
cj = np.clip((P[:, 1] / RES).astype(int), 0, ny - 1)
logw[occ[cj, ci]] = -1e9 # 벽 · 선반 안 파티클은 탈락
w = np.exp(TEMP * (logw - logw.max()))
return w / w.sum()
def resample(P, w, rng):
"""저분산 리샘플링: 가중치가 큰 파티클을 여러 번 복사"""
n = len(P)
u = rng.random() / n + np.arange(n) / n
idx = np.minimum(np.searchsorted(np.cumsum(w), u), n - 1)
return P[idx].copy()
def run(name, offset, seed, n=200, max_d=30.0):
m = MAPS[name]
occ, df, segs = build(m)
rng = np.random.default_rng(seed)
R = m["route"]
x, y = R[0]
th = math.atan2(R[1][1] - y, R[1][0] - x)
P = np.column_stack([x + offset + 0.5 * rng.standard_normal(n), y + 0.5 * rng.standard_normal(n),
th + 0.26 * rng.standard_normal(n)]) # 2D Pose Estimate (offset 만큼 어긋남)
k, dist, good, conv, first = 1, 0.0, 0, None, 0.0
while dist < max_d:
gx, gy = R[k]
e = math.atan2(gy - y, gx - x) - th
e = (e + math.pi) % (2 * math.pi) - math.pi
if abs(e) > 0.01:
d, da = 0.0, max(-0.3, min(0.3, e))
else:
d, da = min(0.1, math.hypot(gx - x, gy - y)), 0.0
if math.hypot(gx - x, gy - y) - d < 1e-6:
k = k + 1 if k + 1 < len(R) else 1
th += da
x, y = x + d * math.cos(th), y + d * math.sin(th)
dist += d
od = d * 1.03 + 0.01 * d * rng.standard_normal() # 오도메트리 (거리 +3 %)
oa = da * 1.02
L = abs(od) # 파티클 이동 + 잡음
P[:, 2] += oa + (ALPHA * abs(oa) + 0.5 * ALPHA * L) * rng.standard_normal(n)
P[:, 0] += od * np.cos(P[:, 2]) + ALPHA * L * rng.standard_normal(n)
P[:, 1] += od * np.sin(P[:, 2]) + ALPHA * L * rng.standard_normal(n)
ranges = np.array([cast(segs, x, y, th + a) for a in ANG])
ranges[ranges > RMAX] = np.inf
ranges[np.isfinite(ranges)] += 0.03 * rng.standard_normal(np.isfinite(ranges).sum())
w = weigh(P, ranges, occ, df)
ex, ey = (w * P[:, 0]).sum(), (w * P[:, 1]).sum()
sd = math.sqrt((w * ((P[:, 0] - ex) ** 2 + (P[:, 1] - ey) ** 2)).sum())
err = math.hypot(ex - x, ey - y)
P = resample(P, w, rng)
if err < 0.3 and sd < 0.3: # 5번 연속 맞으면 수렴으로 봄
good += 1
if good == 1:
first = dist
if good == 5 and conv is None:
conv = first
else:
good = 0
if conv is not None and err > 1.0:
conv = None
return conv, err
print("맵 초기 오차 수렴까지 주행 거리 (시드 1, 2, 3)")
for name in MAPS:
for off in (0.0, 1.0, 2.0):
res = []
for seed in (1, 2, 3):
c, err = run(name, off, seed)
res.append("%5.1f m" % c if c is not None else " 실패 (오차 %.1f m)" % err)
print("%-10s %5.1f m %s" % (name, off, " ".join(res)))실행 결과 (Pyodide · numpy 버전에 따라 난수가 달라 숫자가 조금 다를 수 있습니다):
맵 초기 오차 수렴까지 주행 거리 (시드 1, 2, 3)
선반 통로 0.0 m 0.1 m 0.1 m 0.1 m
선반 통로 1.0 m 0.3 m 8.4 m 0.4 m
선반 통로 2.0 m 12.3 m 18.1 m 12.3 m
긴 복도 0.0 m 0.1 m 0.1 m 0.5 m
긴 복도 1.0 m 21.8 m 22.2 m 23.0 m
긴 복도 2.0 m 실패 (오차 1.3 m) 실패 (오차 1.2 m) 29.3 m- 0 ~ 1 m 오차는 선반 통로에서 첫 1 m 안에 바로잡힙니다(시드 2 의 8.4 m 처럼 가끔 늦기도 합니다). 2 m 는 12 ~ 18 m 가 걸려 시뮬레이터 3 의 📊 와 같은 경향입니다.
- 긴 복도는 파티클 200 개일 때 1 m 오차도 20 m 넘게 걸리고, 2 m 는 30 m 안에 못 바로잡기도 합니다. 출발점이 복도 끝 벽 근처여도 끝 벽은 뒤쪽 몇 개의 빔에만 보이므로 앞뒤 정보가 약합니다. 특징 없는 환경에는 반사판 · 표지 · 기둥 같은 인공 특징을 두는 이유입니다.
- 리샘플링을 빼면(TODO 2) 가중치가 아무리 정확해도 파티클이 모이지 않아 모두 "실패" 가 됩니다. 가중치는 "어느 가설이 그럴듯한가" 를, 리샘플링은 "그럴듯한 가설에 자원을 몰아주기" 를 맡습니다.
AMCL 이 지도에서 찾은 로봇 위치(map → base)와 오도메트리(odom → base)가 주어졌을 때, AMCL 이 발행할 map → odom 을 구하세요. TODO 두 곳(역변환, 합성)을 채웁니다. 오른쪽 열은 계산한 map → odom 에 odom → base 를 다시 이어 붙인 값으로, map → base 와 같아야 합니다.
import math
def compose(a, b):
"""a 다음에 b: (x, y, th) 두 변환을 이어 붙임"""
c, s = math.cos(a[2]), math.sin(a[2])
return (a[0] + b[0] * c - b[1] * s, a[1] + b[0] * s + b[1] * c, a[2] + b[2])
def inverse(a):
# TODO 1: 역변환. 회전은 -th, 위치는 원래 위치를 -th 만큼 돌린 뒤 부호를 바꿈
return (0.0, 0.0, 0.0)
def fmt(p):
return "(%6.2f, %6.2f, %6.1f deg)" % (p[0], p[1], math.degrees(p[2]))
# 시간에 따라: 오도메트리가 본 자세 (odom -> base_footprint), AMCL 이 지도에서 찾은 자세 (map -> base_footprint)
log = [
("출발 (2D Pose Estimate 직후)", (0.00, 0.00, 0.00), (1.60, 2.70, 0.00)),
("8 m 직진", (8.24, 0.05, 0.02), (9.60, 2.70, 0.00)),
("왼쪽으로 돌아 3 m", (8.30, 3.20, 1.62), (9.62, 5.70, 1.57)),
("바퀴가 미끄러짐 (오도메트리만 0.3 m 더 감)", (8.30, 3.50, 1.62), (9.62, 5.70, 1.57)),
]
print("%-38s %-28s %-28s" % ("상황", "map -> odom (AMCL 이 발행)", "map -> base (로봇 위치)"))
for name, odom_base, map_base in log:
map_odom = (0.0, 0.0, 0.0) # TODO 2: map->odom = map->base x (odom->base)^-1
check = compose(map_odom, odom_base) # 다시 이어 붙이면 map->base 가 나와야 함
print("%-38s %s %s" % (name, fmt(map_odom), fmt(check)))풀이 코드 보기
import math
def compose(a, b):
"""a 다음에 b: (x, y, th) 두 변환을 이어 붙임"""
c, s = math.cos(a[2]), math.sin(a[2])
return (a[0] + b[0] * c - b[1] * s, a[1] + b[0] * s + b[1] * c, a[2] + b[2])
def inverse(a):
c, s = math.cos(a[2]), math.sin(a[2])
return (-a[0] * c - a[1] * s, a[0] * s - a[1] * c, -a[2])
def fmt(p):
return "(%6.2f, %6.2f, %6.1f deg)" % (p[0], p[1], math.degrees(p[2]))
# 시간에 따라: 오도메트리가 본 자세 (odom -> base_footprint), AMCL 이 지도에서 찾은 자세 (map -> base_footprint)
log = [
("출발 (2D Pose Estimate 직후)", (0.00, 0.00, 0.00), (1.60, 2.70, 0.00)),
("8 m 직진", (8.24, 0.05, 0.02), (9.60, 2.70, 0.00)),
("왼쪽으로 돌아 3 m", (8.30, 3.20, 1.62), (9.62, 5.70, 1.57)),
("바퀴가 미끄러짐 (오도메트리만 0.3 m 더 감)", (8.30, 3.50, 1.62), (9.62, 5.70, 1.57)),
]
print("%-38s %-28s %-28s" % ("상황", "map -> odom (AMCL 이 발행)", "map -> base (로봇 위치)"))
for name, odom_base, map_base in log:
map_odom = compose(map_base, inverse(odom_base)) # map->odom = map->base x (odom->base)^-1
check = compose(map_odom, odom_base) # 다시 이어 붙이면 map->base 가 나와야 함
print("%-38s %s %s" % (name, fmt(map_odom), fmt(check)))실행 결과:
상황 map -> odom (AMCL 이 발행) map -> base (로봇 위치)
출발 (2D Pose Estimate 직후) ( 1.60, 2.70, 0.0 deg) ( 1.60, 2.70, 0.0 deg)
8 m 직진 ( 1.36, 2.81, -1.1 deg) ( 9.60, 2.70, 0.0 deg)
왼쪽으로 돌아 3 m ( 1.17, 2.92, -2.9 deg) ( 9.62, 5.70, 90.0 deg)
바퀴가 미끄러짐 (오도메트리만 0.3 m 더 감) ( 1.16, 2.62, -2.9 deg) ( 9.62, 5.70, 90.0 deg)- 출발 직후에는 map → odom 이 곧 2D Pose Estimate 값(1.60, 2.70, 0°)입니다. odom 프레임은 로봇이 켜진 자리에서 시작하므로, 지도 위 그 자리를 알려 주는 것이 2D Pose Estimate 의 역할입니다.
- 8 m 를 가는 동안 오도메트리가 3 % 길게(8.24 m) 재고 1.1° 틀어졌습니다. AMCL 은 로봇 위치를 바꾸는 대신 map → odom 을 (1.36, 2.81, −1.1°) 로 옮겨 그 차이를 흡수합니다.
- 바퀴만 미끄러진 마지막 줄은 map → base 가 그대로인데 map → odom 만 0.3 m 움직였습니다. RViz 에서 odom 프레임 축이 조금씩 "뛰는" 것이 바로 이 보정입니다.
AMCL 이 초기 위치 오차를 가장 늦게 바로잡을 환경은?
- 기둥 · 선반 끝 · 문틀이 섞여 있는 창고 통로
- 양쪽 벽이 평평하고 20 m 동안 특징이 없는 긴 복도
- 모서리가 많은 작은 방
- 크기가 다른 상자가 불규칙하게 놓인 적재 구역
정답 보기
② — 평평한 긴 복도에서는 스캔이 앞뒤로 어디서 찍어도 거의 같아 x 방향 가중치 차이가 없습니다. 파티클이 복도 방향으로 길게 퍼진 채 끝 벽을 볼 때까지 수렴하지 못합니다(시뮬레이터 3 의 3번).Nav2 를 켰는데 RViz 에 로봇이 지도 위에 나타나지 않고, 터미널에 "AMCL cannot publish a pose or update the transform. Please set the initial pose..." 가 반복됩니다. 먼저 할 일은?
- map_saver_cli 로 지도를 다시 저장한다
- RViz 에서 2D Pose Estimate 로 로봇 위치와 방향을 지정한다
- max_particles 를 늘린다
- 로봇을 끄고 다시 켠다
정답 보기
② — AMCL 은 초기 위치를 받아야 파티클을 뿌리고 map → odom 을 발행합니다. 그 전에는 TF 트리가 map 에서 끊겨 있어 로봇이 지도 위에 그려지지 않습니다. (myagv_ros2 처럼 `set_initial_pose: True` 로 시작 자세를 파라미터로 주는 방법도 있지만, 로봇이 정말 그 자리에 있어야 합니다.)AMCL 이 map → base_footprint 를 직접 발행하지 않고 map → odom 을 발행하는 이유로 알맞은 것은?
- map → odom 이 계산이 더 빨라서
- base_footprint 는 이미 odom 을 부모로 가지며, TF 트리에서 한 프레임의 부모는 하나뿐이어서
- RViz 가 map → base_footprint 를 그릴 수 없어서
- 오도메트리가 map 좌표를 쓰기 때문에
정답 보기
② — 오도메트리가 odom → base_footprint 를 발행하므로 AMCL 이 map → base_footprint 를 내면 base_footprint 에 부모가 둘이 됩니다. 그래서 (map → base) × (odom → base)⁻¹ = map → odom 을 계산해 냅니다(실습 과제 2-2).3교시 · Nav2 로 목표 · 웨이포인트 주행#
navigation2_active.launch.py 준비: 지도 바꾸고 빌드하기#
공식 문서 6.2.5 의 순서입니다. 기본 launch 파일은 map.yaml 을 읽으므로, 1교시에 저장한 map_test.yaml 을 쓰게 바꿉니다.
# 1) launch 파일에서 지도 파일 이름 바꾸기
cd ~/myagv_plus_ros2/src/myagv_plus_navigation2/launch
gedit navigation2_active.launch.py
# "map_file" 항목의 .../map/map.yaml -> .../map/map_test.yaml 로 고치고 저장
# (공식 문서에는 경로의 ~ 와 파일 이름의 .py 가 빠져 있으니 위처럼 씁니다)
# 2) 그 패키지만 다시 빌드하고 환경 다시 불러오기
cd ~/myagv_plus_ros2
colcon build --packages-select myagv_plus_navigation2
source install/setup.bash
# 3) 실행 (터미널 두 개)
ros2 launch myagv_plus_bringup myagv_plus_bringup.launch.py
ros2 launch myagv_plus_navigation2 navigation2_active.launch.py- 왜 다시 빌드하나요? launch 파일 · 지도 · 파라미터는
src/에 있지만ros2 launch는install/에 복사된 것을 읽습니다. 고친 뒤 빌드를 빼먹으면 예전 지도로 실행됩니다(자주 나는 오류 6). ch09 의 빌드 · source 규칙 그대로입니다. - launch 파일 안의 이름(
map_file인지map인지)은 버전에 따라 다를 수 있습니다. 이전 모델용 myagv_ros2 의 같은 파일은map이라는 인자 이름을 씁니다. 파일을 열어.yaml이 들어간 줄을 찾으면 됩니다. - 실행 인자로 바꿀 수 있게 만들어진 launch 파일이라면
ros2 launch ... map:=/절대/경로/map_test.yaml처럼 빌드 없이 바꿀 수도 있습니다(ros2 launch myagv_plus_navigation2 navigation2_active.launch.py --show-args로 확인).
Nav2 의 구성: ch08 에서 만든 것과 1:1#
| Nav2 구성 요소 | 하는 일 | ch08 에서 만든 것 |
|---|---|---|
| BT Navigator | 목표를 받아 행동 트리(Behavior Tree)를 실행: 계획 → 추종 → 실패하면 복구 → 재시도 | 4교시의 "감지 → 계획 → 제어 루프 + 재계획 조건" |
| Planner Server (전역 계획기) | 전역 코스트맵 위에서 출발 → 목표 경로 (NavFn · Smac 등) | 1교시 A* |
| Controller Server (지역 컨트롤러) | 경로를 따라가며 /cmd_vel 을 냄, 목표 · 진행 검사 | 3교시 Pure Pursuit · DWA |
| Costmap 2D (전역 · 지역) | 지도 + 센서 장애물 + 팽창 → 칸마다 비용 | 2교시 코스트맵 · inflation |
| Recovery (Behavior) Server | spin · backup · wait 같은 복구 행동 | 4교시 복구 행동 |
| Waypoint Follower | 웨이포인트를 차례로 보내고, 도착마다 작업(대기 등) | 4교시 목표 3곳 순회 |
| map_server · AMCL · lifecycle_manager | 지도 제공, 위치 추정(2교시), 서버들을 순서대로 켜기 | ch07 · 2교시 |
이 서버들은 모두 lifecycle 노드라서, lifecycle_manager 가 configure → activate 순서로 켭니다. 하나라도 활성화에 실패하면 Nav2 전체가 "아직 준비 안 됨" 상태로 남습니다(Nav2 Goal 을 눌러도 반응 없음).
RViz 에서 목표 보내기: Nav2 Goal 과 웨이포인트 모드#
공식 문서의 순서입니다.
- 2D Pose Estimate 로 로봇 위치를 맞추고, 스캔이 지도 벽과 겹치는지 확인합니다(2교시). 약간의 오차는 괜찮습니다.
- Nav2 Goal 버튼을 누르고 지도에서 목표를 눌러 끕니다. 화살표 끝이 도착 후 바라볼 방향입니다. 경로가 그려지고 로봇이 따라갑니다. 다음 목표를 줄 때마다 Nav2 Goal 을 다시 눌러야 합니다.
- 웨이포인트: 왼쪽 아래 Nav2 패널에서 Waypoint / Nav Through Poses Mode 를 누르고, Nav2 Goal 로 점을 여러 개 찍은 뒤 Start Waypoint Following 을 누릅니다. 로봇이 찍은 순서대로 갑니다.
Waypoint Following 은 찍은 순서 그대로 갑니다. 가까운 순으로 다시 정렬해 주지 않습니다. 어떤 순서가 짧은지는 아래 시뮬레이터 4 와 실습 과제 3-1 에서 직접 계산해 봅니다.
Python 으로 목표 보내기: nav2_simple_commander#
RViz 를 누르는 대신 프로그램으로 목표를 보내면, 창고 관리 시스템의 주문에 따라 로봇을 움직일 수 있습니다. Nav2 에는 이를 쉽게 해 주는 nav2_simple_commander 가 있습니다. 아래는 실물(또는 Isaac Sim)에서 Nav2 가 실행 중일 때만 동작합니다. 좌표는 여러분이 만든 지도의 map 좌표로 바꿔야 합니다.
# pick_route.py - 입고 -> 선반 3곳 -> 출고 웨이포인트 순회 (Nav2 실행 중일 때)
import math
import time
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
def make_pose(nav, x, y, yaw):
p = PoseStamped()
p.header.frame_id = "map"
p.header.stamp = nav.get_clock().now().to_msg()
p.pose.position.x = x
p.pose.position.y = y
p.pose.orientation.z = math.sin(yaw / 2) # yaw -> 쿼터니언 (z, w)
p.pose.orientation.w = math.cos(yaw / 2)
return p
def main():
rclpy.init()
nav = BasicNavigator()
nav.setInitialPose(make_pose(nav, 0.0, 0.0, 0.0)) # 2D Pose Estimate 와 같은 일
nav.waitUntilNav2Active() # AMCL · Nav2 서버가 모두 켜질 때까지 대기
# 좌표는 예시 - 여러분 지도에서 RViz 의 Publish Point 등으로 읽어 바꿉니다
route = [(3.5, 0.4, math.pi / 2), (3.0, 4.6, -math.pi / 2), (8.0, 2.5, math.pi / 2), (10.0, 5.8, 0.0)]
nav.followWaypoints([make_pose(nav, x, y, th) for x, y, th in route])
while not nav.isTaskComplete():
fb = nav.getFeedback()
if fb:
print("웨이포인트", fb.current_waypoint + 1, "/", len(route))
time.sleep(0.5)
result = nav.getResult()
if result == TaskResult.SUCCEEDED:
print("순회 완료")
elif result == TaskResult.CANCELED:
print("취소됨")
else:
print("실패 - 로그와 RViz 의 코스트맵을 확인하세요")
rclpy.shutdown()
if __name__ == "__main__":
main()# 패키지가 없으면 설치 (출고 이미지에 들어 있는지는 확인 필요)
sudo apt install ros-humble-nav2-simple-commander
python3 pick_route.py- 한 지점만 보낼 때는
nav.goToPose(pose), 지나가기만 할 점들은nav.goThroughPoses(poses)를 씁니다. 멈추려면nav.cancelTask(). - 실행 중 Ctrl + C 로 끄면 진행 중인 목표가 남아 로봇이 계속 갈 수 있습니다. 끝내기 전에
cancelTask()를 부르도록try / finally로 감싸는 습관을 들입니다(ch04 의 "정지 보장" 과 같은 생각).
시뮬레이터 4 · 피킹 스테이션 웨이포인트 순회#
12 m × 8 m 피킹 스테이션 창고입니다. 로봇은 입고에서 출발해 고른 순서대로 선반 앞에 서서(1 s 작업 대기) 마지막에 출고로 갑니다. 각 구간은 Nav2 처럼 **전역 계획(A*, 팽창 0.35 m) → 경로 추종 → 목표 검사** 로 움직입니다. 오른쪽 패널은 지금 일하는 Nav2 구성 요소입니다.
- 지도를 보고 번호 순(선반1 → 2 → 3) 의 총 이동 거리를 어림해 적습니다. 그다음 거리가 가장 짧을 것 같은 순서를 하나 골라 적고, 몇 % 줄어들지 예측합니다.
- 번호 순으로 ▶ Start Waypoint Following 을 돌립니다. 오른쪽 Nav2 패널에서 Planner · Controller · Waypoint Follower 가 어떤 순서로 켜지는지 봅니다.
- ↺ 순서 지우기 → 예측한 순서를 + 버튼으로 만들어 돌립니다. 그다음 📊 6가지 순서 모두 비교 로 확인합니다. 가장 짧은 순서는 번호 순보다 몇 % 짧나요?
- "지도에 없는 팔레트" 를 켜고 번호 순을 다시 돌립니다. 로봇은 언제 팔레트를 알아차리고, 그때 어떤 구성 요소가 일하나요? 📊 비교를 다시 하면 최적 순서가 바뀌나요?
- max_vel_x 를 0.26 → 0.6 으로 바꿔 시간을 비교합니다. 속도가 2.3 배면 시간도 2.3 배 줄어들까요?
- 📊 결과: 번호 순 32.0 m(약 93 s), 가장 짧은 선반2 → 선반3 → 선반1 은 24.0 m(약 71 s)로 약 25 % 짧습니다. 선반1 이 출고 바로 옆에 있으므로 마지막에 들르는 것이 유리합니다. 번호 · 주문 순서가 아니라 지도 위 위치로 순서를 정해야 합니다.
- "계획 거리 합" 은 각 구간 A* 경로의 길이이고, "실제 이동" 은 추종기가 경로 모서리를 조금씩 질러가서 그보다 약간 짧습니다.
- 팔레트는 지도(정적 지도)에 없으므로 처음 계획은 왼쪽 통로를 지나갑니다. 지역 코스트맵 범위(2.5 m) 안에 들어오면 장애물로 반영되고 Planner Server 가 다시 계획해 오른쪽으로 돌아갑니다. 번호 순은 32.0 m → 약 42 m 로 늘어납니다. 막힌 통로에 따라 최적 순서가 달라질 수 있습니다.
- 속도를 올려도 각 지점의 작업 대기 · 제자리 회전 · 가감속 시간은 그대로라 총시간은 속도에 반비례하지 않습니다.
실습 과제 3#
시뮬레이터 4 와 같은 지도 · 같은 비용(로봇 반경 0.22 m, 팽창 0.35 m, cost_scaling_factor 3.0, NavFn 꼴 이동 비용)으로 다섯 지점 사이의 A* 경로 길이를 구하고, 선반 3곳을 도는 6가지 순서의 총 거리를 비교합니다. TODO 두 곳(비용을 반영한 이동 비용, 순서별 총 거리)을 채우세요. 실행에 몇 초 걸립니다. 예측 먼저: 번호 순 대비 가장 짧은 순서는 몇 % 줄어들까요?
import heapq
import itertools
import math
import numpy as np
# 피킹 스테이션 맵 (12 m x 8 m, 교육용). 사각형 (x0, y0, x1, y1)
W, H, RES = 12.0, 8.0, 0.05
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8),
(3.0, 2.0, 10.0, 2.6), (3.0, 4.4, 10.0, 5.0), (5.5, 6.6, 8.5, 7.2),
(0.1, 3.6, 1.6, 3.8), (10.9, 3.6, 11.9, 3.8)]
ST = {"IN": (1.0, 1.0), "S1": (9.0, 3.5), "S2": (4.5, 1.4), "S3": (4.0, 5.6), "OUT": (11.0, 6.8)}
RR, INFL, CSF = 0.22, 0.35, 3.0 # robot_radius, inflation_radius, cost_scaling_factor
NX, NY = round(W / RES), round(H / RES)
occ = np.zeros((NY, NX), dtype=bool)
cx = (np.arange(NX) + 0.5) * RES
cy = (np.arange(NY) + 0.5) * RES
for x0, y0, x1, y1 in BOXES:
occ[np.ix_((cy > y0) & (cy < y1), (cx > x0) & (cx < x1))] = True
# 거리장: 경계 점유 셀까지의 거리 (행 단위로 나눠 계산해 메모리를 아낌)
pad = np.pad(occ, 1, constant_values=True)
edge = occ & ~(pad[:-2, 1:-1] & pad[2:, 1:-1] & pad[1:-1, :-2] & pad[1:-1, 2:])
ej, ei = np.nonzero(edge)
dist = np.zeros((NY, NX), dtype=np.float32)
ii = np.arange(NX)
for j in range(NY):
d2 = (ii[:, None] - ei[None, :]) ** 2 + (j - ej[None, :]) ** 2
dist[j] = np.sqrt(d2.min(axis=1)) * RES
dist[occ] = 0
# Nav2 inflation 식으로 비용 (0~252, 253 이상 = 지나갈 수 없음)
cost = np.where(dist > INFL, 0, np.round(252 * np.exp(-CSF * (dist - RR)))).astype(int)
cost[dist < RR] = 253
cost[occ] = 254
def astar(s, g):
si, sj = int(s[0] // RES), int(s[1] // RES)
gi, gj = int(g[0] // RES), int(g[1] // RES)
def h(i, j):
dx, dy = abs(i - gi), abs(j - gj)
return max(dx, dy) + (math.sqrt(2) - 1) * min(dx, dy)
G = {(si, sj): 0.0}
parent = {}
closed = set()
heap = [(h(si, sj), si, sj)]
while heap:
f, i, j = heapq.heappop(heap)
if (i, j) in closed:
continue
closed.add((i, j))
if (i, j) == (gi, gj):
break
for di, dj in [(1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)]:
a, b = i + di, j + dj
if not (0 <= a < NX and 0 <= b < NY) or (a, b) in closed or cost[b, a] >= 253:
continue
step = 1.0
if di and dj:
if cost[j, a] >= 253 or cost[b, i] >= 253: # 모서리 자르기 금지
continue
step = math.sqrt(2)
ng = G[(i, j)] + step # TODO 1: 비용이 큰 칸일수록 비싸게: step x (1 + 0.016 x cost)
if ng < G.get((a, b), 1e18):
G[(a, b)] = ng
parent[(a, b)] = (i, j)
heapq.heappush(heap, (ng + h(a, b), a, b))
if (gi, gj) not in closed:
return None
path, k = [], (gi, gj)
while k in parent or k == (si, sj):
path.append(((k[0] + 0.5) * RES, (k[1] + 0.5) * RES))
if k == (si, sj):
break
k = parent[k]
return sum(math.dist(path[n], path[n + 1]) for n in range(len(path) - 1))
names = list(ST)
L = {}
for a in names:
for b in names:
if a < b:
L[a, b] = L[b, a] = astar(ST[a], ST[b])
print("두 지점 사이 경로 길이 (m)")
for a, b in [("IN", "S1"), ("IN", "S2"), ("S1", "S2"), ("S1", "S3"), ("S2", "S3"), ("S1", "OUT"), ("S3", "OUT")]:
print(" %-3s - %-3s %6.2f" % (a, b, L[a, b]))
rows = []
for order in itertools.permutations(["S1", "S2", "S3"]):
seq = ["IN"] + list(order) + ["OUT"]
total = 0.0 # TODO 2: 이웃한 두 지점 사이 길이 L[a, b] 를 모두 더하기
rows.append((total, " > ".join(seq)))
base = rows[0][0]
print("\n순서 총 거리 번호 순 대비")
for total, s in sorted(rows):
print("%-28s %6.2f m %+5.1f %%" % (s, total, 100 * (total - base) / base if base else 0.0))풀이 코드 보기
import heapq
import itertools
import math
import numpy as np
# 피킹 스테이션 맵 (12 m x 8 m, 교육용). 사각형 (x0, y0, x1, y1)
W, H, RES = 12.0, 8.0, 0.05
BOXES = [(0, 0, 12, 0.1), (0, 7.9, 12, 8), (0, 0, 0.1, 8), (11.9, 0, 12, 8),
(3.0, 2.0, 10.0, 2.6), (3.0, 4.4, 10.0, 5.0), (5.5, 6.6, 8.5, 7.2),
(0.1, 3.6, 1.6, 3.8), (10.9, 3.6, 11.9, 3.8)]
ST = {"IN": (1.0, 1.0), "S1": (9.0, 3.5), "S2": (4.5, 1.4), "S3": (4.0, 5.6), "OUT": (11.0, 6.8)}
RR, INFL, CSF = 0.22, 0.35, 3.0 # robot_radius, inflation_radius, cost_scaling_factor
NX, NY = round(W / RES), round(H / RES)
occ = np.zeros((NY, NX), dtype=bool)
cx = (np.arange(NX) + 0.5) * RES
cy = (np.arange(NY) + 0.5) * RES
for x0, y0, x1, y1 in BOXES:
occ[np.ix_((cy > y0) & (cy < y1), (cx > x0) & (cx < x1))] = True
# 거리장: 경계 점유 셀까지의 거리 (행 단위로 나눠 계산해 메모리를 아낌)
pad = np.pad(occ, 1, constant_values=True)
edge = occ & ~(pad[:-2, 1:-1] & pad[2:, 1:-1] & pad[1:-1, :-2] & pad[1:-1, 2:])
ej, ei = np.nonzero(edge)
dist = np.zeros((NY, NX), dtype=np.float32)
ii = np.arange(NX)
for j in range(NY):
d2 = (ii[:, None] - ei[None, :]) ** 2 + (j - ej[None, :]) ** 2
dist[j] = np.sqrt(d2.min(axis=1)) * RES
dist[occ] = 0
# Nav2 inflation 식으로 비용 (0~252, 253 이상 = 지나갈 수 없음)
cost = np.where(dist > INFL, 0, np.round(252 * np.exp(-CSF * (dist - RR)))).astype(int)
cost[dist < RR] = 253
cost[occ] = 254
def astar(s, g):
si, sj = int(s[0] // RES), int(s[1] // RES)
gi, gj = int(g[0] // RES), int(g[1] // RES)
def h(i, j):
dx, dy = abs(i - gi), abs(j - gj)
return max(dx, dy) + (math.sqrt(2) - 1) * min(dx, dy)
G = {(si, sj): 0.0}
parent = {}
closed = set()
heap = [(h(si, sj), si, sj)]
while heap:
f, i, j = heapq.heappop(heap)
if (i, j) in closed:
continue
closed.add((i, j))
if (i, j) == (gi, gj):
break
for di, dj in [(1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)]:
a, b = i + di, j + dj
if not (0 <= a < NX and 0 <= b < NY) or (a, b) in closed or cost[b, a] >= 253:
continue
step = 1.0
if di and dj:
if cost[j, a] >= 253 or cost[b, i] >= 253: # 모서리 자르기 금지
continue
step = math.sqrt(2)
ng = G[(i, j)] + step * (1 + 0.016 * cost[b, a]) # NavFn 꼴: (50 + 0.8 cost) / 50
if ng < G.get((a, b), 1e18):
G[(a, b)] = ng
parent[(a, b)] = (i, j)
heapq.heappush(heap, (ng + h(a, b), a, b))
if (gi, gj) not in closed:
return None
path, k = [], (gi, gj)
while k in parent or k == (si, sj):
path.append(((k[0] + 0.5) * RES, (k[1] + 0.5) * RES))
if k == (si, sj):
break
k = parent[k]
return sum(math.dist(path[n], path[n + 1]) for n in range(len(path) - 1))
names = list(ST)
L = {}
for a in names:
for b in names:
if a < b:
L[a, b] = L[b, a] = astar(ST[a], ST[b])
print("두 지점 사이 경로 길이 (m)")
for a, b in [("IN", "S1"), ("IN", "S2"), ("S1", "S2"), ("S1", "S3"), ("S2", "S3"), ("S1", "OUT"), ("S3", "OUT")]:
print(" %-3s - %-3s %6.2f" % (a, b, L[a, b]))
rows = []
for order in itertools.permutations(["S1", "S2", "S3"]):
seq = ["IN"] + list(order) + ["OUT"]
total = sum(L[seq[k], seq[k + 1]] for k in range(len(seq) - 1))
rows.append((total, " > ".join(seq)))
base = rows[0][0]
print("\n순서 총 거리 번호 순 대비")
for total, s in sorted(rows):
print("%-28s %6.2f m %+5.1f %%" % (s, total, 100 * (total - base) / base if base else 0.0))실행 결과:
두 지점 사이 경로 길이 (m)
IN - S1 9.06
IN - S2 3.67
S1 - S2 8.61
S1 - S3 9.01
S2 - S3 6.78
S1 - OUT 4.45
S3 - OUT 7.50
순서 총 거리 번호 순 대비
IN > S2 > S3 > S1 > OUT 23.91 m -25.2 %
IN > S3 > S2 > S1 > OUT 26.19 m -18.1 %
IN > S2 > S1 > S3 > OUT 28.78 m -9.9 %
IN > S1 > S2 > S3 > OUT 31.96 m +0.0 %
IN > S3 > S1 > S2 > OUT 35.19 m +10.1 %
IN > S1 > S3 > S2 > OUT 36.09 m +12.9 %- 시뮬레이터 4 의 📊 와 순위가 같고 길이는 수 cm 차이입니다. 비용이 같은 경로가 여러 개일 때 어느 칸을 먼저 꺼내느냐(동점 처리)가 JavaScript 와 Python 의 우선순위 큐에서 달라 조금 다른 경로를 고르기 때문입니다.
- TODO 1 을 빼먹고 비용 없이 길이만 쓰면 경로가 선반 모서리에 바짝 붙어 조금 짧아지지만, 실물에서는 ch08 에서 본 것처럼 모서리를 긁습니다.
- 선반이 n 곳이면 순서는 n! 가지입니다(5곳 120, 8곳 40320). 지점이 많아지면 모든 순서를 비교하는 대신 "가장 가까운 곳부터" 같은 근사나 경로 최적화 도구를 씁니다. 두 지점 사이 거리 표(L)를 한 번만 계산해 두고 재사용하는 것이 핵심입니다.
BT Navigator 의 기본 행동 트리는 계획이나 추종이 실패하면 복구 행동 → 재시도를 정해진 횟수만큼 되풀이하고, 그래도 안 되면 목표를 중단(ABORTED)합니다. 시도마다의 결과가 주어졌을 때 로그를 만드는 함수를 완성하세요(TODO 한 곳). 실제 트리에서 재시도 횟수는 number_of_retries 같은 값으로 정하고 복구 순서도 다르지만, 흐름은 같습니다.
# BT Navigator 흉내: 계획 -> 추종 -> 실패하면 복구 행동 -> 다시 시도 (number_of_retries 회)
RECOVERIES = ["코스트맵 비우기", "제자리 회전 (spin)", "2 s 대기 (wait)", "뒤로 0.3 m (backup)"]
def navigate(events, retries=2):
"""events: 시도마다 (계획 결과, 추종 결과) 순서대로. True = 성공"""
log, attempt = [], 0
while True:
plan_ok, follow_ok = events[min(attempt, len(events) - 1)]
log.append("시도 %d: 계획 %s" % (attempt + 1, "성공" if plan_ok else "실패"))
if plan_ok:
log.append(" 추종 %s" % ("성공 -> 도착 (SUCCEEDED)" if follow_ok else "실패"))
if follow_ok:
return "SUCCEEDED", log
# TODO: 재시도 횟수(retries)를 다 썼으면 "ABORTED" 로 끝내고,
# 아니면 복구 행동 하나를 log 에 적고 attempt 를 1 늘려 다시 시도합니다
return "ABORTED", log
cases = {
"막힘 없음": [(True, True)],
"사람이 잠깐 막았다 비킴": [(True, False), (True, True)],
"통로가 완전히 막힘": [(False, False)],
"추종이 계속 실패 (yaw 허용 오차 너무 작음)": [(True, False)],
}
for name, ev in cases.items():
result, log = navigate(ev)
print("[%s] -> %s" % (name, result))
print("\n".join(log))
print()풀이 코드 보기
# BT Navigator 흉내: 계획 -> 추종 -> 실패하면 복구 행동 -> 다시 시도 (number_of_retries 회)
RECOVERIES = ["코스트맵 비우기", "제자리 회전 (spin)", "2 s 대기 (wait)", "뒤로 0.3 m (backup)"]
def navigate(events, retries=2):
"""events: 시도마다 (계획 결과, 추종 결과) 순서대로. True = 성공"""
log, attempt = [], 0
while True:
plan_ok, follow_ok = events[min(attempt, len(events) - 1)]
log.append("시도 %d: 계획 %s" % (attempt + 1, "성공" if plan_ok else "실패"))
if plan_ok:
log.append(" 추종 %s" % ("성공 -> 도착 (SUCCEEDED)" if follow_ok else "실패"))
if follow_ok:
return "SUCCEEDED", log
if attempt >= retries:
log.append(" 재시도 %d 회를 다 씀 -> 목표 중단 (ABORTED)" % retries)
return "ABORTED", log
log.append(" 복구 행동: " + RECOVERIES[attempt % len(RECOVERIES)])
attempt += 1
cases = {
"막힘 없음": [(True, True)],
"사람이 잠깐 막았다 비킴": [(True, False), (True, True)],
"통로가 완전히 막힘": [(False, False)],
"추종이 계속 실패 (yaw 허용 오차 너무 작음)": [(True, False)],
}
for name, ev in cases.items():
result, log = navigate(ev)
print("[%s] -> %s" % (name, result))
print("\n".join(log))
print()실행 결과:
[막힘 없음] -> SUCCEEDED
시도 1: 계획 성공
추종 성공 -> 도착 (SUCCEEDED)
[사람이 잠깐 막았다 비킴] -> SUCCEEDED
시도 1: 계획 성공
추종 실패
복구 행동: 코스트맵 비우기
시도 2: 계획 성공
추종 성공 -> 도착 (SUCCEEDED)
[통로가 완전히 막힘] -> ABORTED
시도 1: 계획 실패
복구 행동: 코스트맵 비우기
시도 2: 계획 실패
복구 행동: 제자리 회전 (spin)
시도 3: 계획 실패
재시도 2 회를 다 씀 -> 목표 중단 (ABORTED)
[추종이 계속 실패 (yaw 허용 오차 너무 작음)] -> ABORTED
시도 1: 계획 성공
추종 실패
복구 행동: 코스트맵 비우기
시도 2: 계획 성공
추종 실패
복구 행동: 제자리 회전 (spin)
시도 3: 계획 성공
추종 실패
재시도 2 회를 다 씀 -> 목표 중단 (ABORTED)- "사람이 잠깐 막았다 비킴" 은 첫 추종이 실패해도 복구(코스트맵 비우기) 뒤 다시 시도해 성공합니다. 일시적인 막힘에는 재시도가 효과적입니다.
- "통로가 완전히 막힘" 과 "yaw 허용 오차 너무 작음" 은 몇 번을 다시 해도 같은 이유로 실패합니다. 재시도 횟수를 늘리면 실패를 늦게 알 뿐입니다. 원인을 고치는 것은 4교시의 파라미터 튜닝입니다.
- 시뮬레이터 4 · 5 는 재시도를 한 번(2 s 대기 후)만 하도록 단순화했습니다.
다음 중 **/cmd_vel 을 직접 내보내 로봇 바퀴를 움직이는** Nav2 구성 요소는? (복구 행동 중 제외)
- Planner Server
- Controller Server
- BT Navigator
- map_server
정답 보기
② — Controller Server(지역 컨트롤러)가 전역 경로와 지역 코스트맵을 보고 매 주기 속도 명령을 냅니다. Planner Server 는 경로를, BT Navigator 는 "언제 무엇을 할지" 를, map_server 는 지도를 제공합니다(복구 행동의 spin · backup 도 /cmd_vel 을 냅니다).시뮬레이터 4 에서 번호 순(선반1 → 2 → 3)보다 선반2 → 선반3 → 선반1 이 약 25 % 짧았습니다. 가장 큰 이유는?
- 선반2 로 갈 때 max_vel_x 가 자동으로 올라가서
- 선반1 이 출고 근처에 있어 마지막에 들르면 되돌아가는 길이 줄어서
- 웨이포인트가 적을수록 A* 가 빨라져서
- 선반3 이 장애물 팽창이 작아서
정답 보기
② — 번호 순은 입고(왼쪽 아래) → 선반1(오른쪽 가운데) → 선반2(왼쪽 아래) 로 창고를 한 번 가로질렀다 되돌아옵니다. 출고(오른쪽 위) 가까이 있는 선반1 을 마지막으로 미루면 되돌아가는 길이 사라집니다.1교시에 저장한 map_test.yaml 을 쓰려고 launch 파일의 지도 이름을 고친 뒤 바로 ros2 launch myagv_plus_navigation2 navigation2_active.launch.py 를 실행했더니 예전 지도가 떴습니다. 빠뜨린 단계는?
colcon build --packages-select myagv_plus_navigation2와source install/setup.bashmap_saver_cli를 한 번 더 실행- RViz 에서 2D Pose Estimate
- 로봇 재부팅
정답 보기
① — `ros2 launch` 는 `install/` 에 복사된 launch 파일을 읽습니다. `src/` 의 파일을 고쳤으면 그 패키지를 다시 빌드하고 source 해야 반영됩니다.4교시 · 파라미터 튜닝과 문제 해결, 3D 지도(Rtabmap)#
자주 만지는 파라미터 다섯 개#
Nav2 파라미터는 수백 개지만, 현장에서 처음 만지는 것은 대개 아래 다섯 개(와 로봇 크기)입니다. 값은 이전 모델 myagv_ros2 의 myagv.yaml 기준이며, myAGV Plus 의 실제 값은 로봇의 파라미터 파일에서 확인합니다.
| 파라미터 (어느 서버) | 뜻 | myagv.yaml 값 | 키우면 | 줄이면 |
|---|---|---|---|---|
max_vel_x (컨트롤러) | 최대 전진 속도 m/s | 0.26 | 빨리 도착, 코너에서 바깥으로 밀림 | 느리지만 정확 |
max_vel_y (컨트롤러) | 최대 옆 속도 m/s (메카넘만 의미) | 0.0 | 옆으로 비켜 가는 경로 추종 가능 | 0 이면 차동 구동처럼 돌아서 감 |
inflation_radius (코스트맵) | 장애물 둘레 비용을 칠하는 거리 m | 0.25 | 벽에서 멀리 떨어져 다님, 좁은 곳을 비싸게 봄 | 벽 · 모서리에 바짝 붙음 |
xy_goal_tolerance (목표 검사기) | 목표 위치 허용 오차 m | 0.25 | 일찍 "도착" 선언, 멀리 멈춤 | 정확하지만 오래 걸림 |
yaw_goal_tolerance (목표 검사기) | 목표 방향 허용 오차 rad | 0.25 | 방향이 대충 맞아도 도착 | 목표 앞에서 좌우로 꿈틀거림 |
robot_radius · footprint (코스트맵) | 로봇(+ 적재물) 크기 | 0.22 · 0.34 × 0.24 | 좁은 통로를 못 지나감 | 몸이 장애물에 닿음 |
# 파라미터 파일의 모양 (Nav2 params, 일부만 - 이름 · 값은 로봇에서 확인)
controller_server:
ros__parameters:
controller_frequency: 5.0
general_goal_checker:
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
FollowPath:
max_vel_x: 0.26
max_vel_y: 0.0
max_vel_theta: 0.5
local_costmap:
local_costmap:
ros__parameters:
footprint: "[ [0.17, 0.12], [0.17, -0.12], [-0.17, -0.12], [-0.17, 0.12] ]"
inflation_layer:
inflation_radius: 0.25
cost_scaling_factor: 5.0# 지금 값 확인 (실행 중인 노드의 파라미터)
ros2 param get /controller_server general_goal_checker.yaw_goal_tolerance
ros2 param get /local_costmap/local_costmap inflation_layer.inflation_radius
# 파일 위치 찾기 (Plus 패키지 안의 파라미터 폴더)
ls ~/myagv_plus_ros2/src/myagv_plus_navigation2/일부 파라미터는 ros2 param set 으로 실행 중에 바뀌지만, 바뀌지 않는 것도 많습니다. 확실한 방법은 yaml 수정 → colcon build --packages-select myagv_plus_navigation2 → source → 다시 실행 입니다. 한 번에 하나씩 바꾸고 결과를 적어 두세요.
이전 모델 설정은 robot_model_type: differential · max_vel_y: 0.0 이라 메카넘 바퀴인데도 옆으로 움직이지 않습니다. 옆 이동을 쓰려면 AMCL 의 robot_model_type 을 전방향(nav2_amcl::OmniMotionModel)으로, 컨트롤러의 max_vel_y · min_vel_y · vy_samples 등을 함께 바꿔야 합니다. Plus 의 기본 설정이 어떤지는 확인 필요입니다.
시뮬레이터 5 · Nav2 파라미터 시험장#
10 m × 6 m 시험장입니다. 출발해서 0.7 m 좁은 문을 지나, 위쪽 선반 앞 목표에 선반을 바라보며(위쪽) 서야 합니다. 오른쪽 넓은 문은 우회로입니다. 컨트롤러는 myagv.yaml 처럼 5 Hz 로 돌고, 명령은 한 주기 늦게 바퀴에 도착합니다. 제자리 회전은 0.4 rad/s 보다 느리게 돌 수 없다고 둡니다(모터 · 롤러의 최소 속도, 교육용 가정). 진행 검사는 10 s 동안 0.5 m 를 못 움직이면 실패, 실패하면 2 s 대기 후 한 번 재시도합니다.
- 기본값으로 ▶ 를 눌러 기준을 만듭니다(시간 · 최소 여유 · 방향 오차). 📊 5번 반복으로 표에 남깁니다.
- ① 목표 앞 맴돌기: yaw_goal_tolerance 를 0.01 rad 로 줄이면 도착할 때 어떤 일이 생길지 먼저 적습니다. 재현한 뒤 아래 그래프의 빨간 선(회전 속도)과 "회전 방향이 바뀐 횟수" 를 봅니다. 0.02 · 0.05 에서도 해 보고, 어디서부터 증상이 사라지는지 찾습니다.
- ② 좁은 통로 실패: robot_radius 0.40(넓은 팔레트 적재) + 우회로 막힘. 왜 계획조차 못 할까요? 우회로를 다시 열면? robot_radius 0.30 이면?
- ③ 과속 코너: max_vel_x 1.0 에서 궤적 색과 "최소 여유" 를 봅니다. 해결 방법 두 가지 — max_vel_x 를 몇까지 낮추면 충돌이 사라지나요? 속도는 1.0 그대로 두고 곡률 · 근접 감속을 켜면?
- inflation_radius 를 0.22 → 0.6 → 1.0 으로 바꾸며 경로 모양 · 최소 여유 · 계획 길이를 비교합니다. xy_goal_tolerance 를 0.5 · 0.05 로 바꾸면 실제로 멈춘 자리는 몇 cm 차이인가요?
- 맴돌기: 제자리 회전은 한 주기(0.2 s)에 최소 0.08 rad 씩 돌고, 명령이 한 주기 늦게 닿습니다. 허용 구간 폭(2 × 0.01 = 0.02 rad)이 한 걸음보다 훨씬 좁아 목표 방향을 건너뛰고, 되돌아오다 또 건너뜁니다. 📊: 0.01 rad 는 평균 31 s · 방향 바뀜 6 번(기본 23 s · 0 번), 0.02 는 28 s · 3.8 번, 0.05 부터 증상이 사라집니다. 10 s 넘게 맴돌면 진행 검사가 "Failed to make progress" 를 내고 재시도합니다(로그 확인). 허용 오차는 한 주기 최소 회전량의 절반보다 크게 잡습니다.
- 좁은 통로: 0.7 m 문 가운데에서 양쪽 벽까지 0.35 m 인데 robot_radius 0.40 이면 문 전체가 치명 구역(253)입니다. 우회로까지 막히면 갈 길이 없어 "계획 실패 (Failed to create plan)" 로 바로 끝납니다. 우회로가 열려 있으면 16.8 m 를 돌아 42 s 가 걸립니다. 적재물이 로봇보다 넓을 때는 footprint 를 적재물 기준으로 바꿔야 실제 충돌을 막을 수 있고, 그 대가로 좁은 통로를 포기합니다.
- 과속 코너: 1.0 m/s 에서는 회전 속도 한계(1 rad/s)로 돌 수 있는 반경이 1 m 이상이라, 문으로 꺾을 때 바깥으로 밀려 모서리에 닿습니다(📊 충돌 5, 최소 여유 −6 cm). 0.8 m/s 이하로 낮추거나, 감속 옵션을 켜면 곡선 · 벽 근처에서만 속도를 줄여 충돌 0 으로 15.8 s 에 도착합니다. 0.4 m/s 기본값(22.9 s)보다 빠르면서 안전합니다. Nav2 의 Regulated Pure Pursuit 컨트롤러가 이 아이디어입니다.
- inflation_radius: 0.22(= 로봇 반경)이면 비용이 거의 없어 경로가 모서리에 바짝 붙고 충돌이 납니다(📊 충돌 11). 0.6 이면 계획 길이가 8.96 → 9.37 m 로 조금 늘지만 최소 여유가 15 cm 로 넉넉해집니다. 다만 좁은 문 가운데의 비용은 이미 높아서 inflation_radius 를 키워도 크게 달라지지 않습니다(실습 과제 4-2).
- xy_goal_tolerance: 0.5 m 면 목표에서 평균 34 cm 떨어져 "도착" 합니다. 0.05 로 줄여도 실제로 멈춘 자리는 평균 8 cm 안팎입니다. 위치 추정의 흔들림(±1 cm)과 감속 중 미끄러짐 때문에, 허용 오차를 줄인다고 그만큼 정확해지지는 않습니다.
오류 메시지는 "어느 서버가, 무엇을 못 했나" 로 읽는다#
Nav2 의 로그는 대괄호 안에 노드 이름이 붙습니다. 이름을 먼저 보고 위 구성 요소 표에서 그 서버가 하는 일을 떠올리면 원인 범위가 크게 좁혀집니다.
| 로그 (요지) | 어느 서버 | 먼저 볼 곳 |
|---|---|---|
Timed out waiting for transform ... map | 코스트맵 · BT Navigator | map → odom 이 없음 → 2D Pose Estimate, AMCL |
Goal was rejected · Goal ... is in lethal space | BT Navigator · Planner | 목표가 지도 밖 · 벽 · 팽창 구역 안 |
Starting point in lethal space · Failed to create plan | Planner Server | 로봇 위치 · 지도 어긋남, robot_radius, 막힌 통로 |
Failed to make progress | Controller Server (진행 검사기) | 허용 오차가 너무 작음, 로봇이 막힘 · 미끄러짐 |
Message Filter dropping message | 코스트맵 · AMCL | 스캔 시각과 TF 시각 불일치, 시계 |
자세한 원문과 해결은 아래 "자주 나는 오류와 해결" 에 모았습니다(문구는 Nav2 버전에 따라 조금 다릅니다).
Rtabmap: 카메라로 만드는 3D 지도와 내비게이션#
LiDAR 는 한 높이의 평면만 봅니다. 테이블 상판 · 선반 판 · 매달린 물체는 2D 지도에 없습니다(ch07). myAGV Plus 는 3D 카메라(Orbbec)를 함께 써서 Rtabmap(Real-Time Appearance-Based Mapping)으로 3D 지도를 만들 수 있습니다. 공식 문서 6.2.6 의 순서입니다.
# 3D 지도 작성 (터미널 3개 + 키보드 조종)
ros2 launch myagv_plus_bringup myagv_plus_bringup.launch.py
ros2 launch orbbec_camera astra_pro2.launch.py
ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=false
ros2 run teleop_twist_keyboard teleop_twist_keyboard
# 끝내기: 각 터미널에서 Ctrl + C -> 지도는 ~/.ros/rtabmap.db 에 저장 (숨김 폴더, 파일 창에서 Ctrl + H)
# 지도 작성과 내비게이션을 동시에
ros2 launch myagv_plus_bringup myagv_plus_bringup.launch.py
ros2 launch orbbec_camera astra_pro2.launch.py
ros2 launch rtabmap_demos myagvplus_rgbd_scan.launch.py- 공식 문서는 Rtabmap 지도 작성 때 선속도 0.2, 각속도 0.4 정도로 더 천천히 조종하라고 권합니다. 카메라 영상의 특징점을 이어 붙이므로 빠른 회전에 특히 약합니다.
- 동시 실행 모드에서는 RViz 에 점유 격자와 3D 점 구름이 함께 보이고, 코스트맵에 장애물 · 복셀(voxel) · 팽창 층이 있습니다. 복셀 층은
/camera/obstacles·/camera/ground·/scan을 함께 써서 LiDAR 높이 밖의 장애물도 막습니다. Nav2 패널이 미리 들어 있어 Nav2 Goal · 웨이포인트를 그대로 씁니다. - Rtabmap 은 장소의 모양(외관)으로 루프를 닫습니다. 2교시의 긴 복도처럼 LiDAR 로는 구별이 안 되는 곳도 벽의 포스터 · 표지판으로 구별할 수 있는 대신, 조명이 바뀌거나 특징 없는 흰 벽 앞에서는 약해집니다.
Isaac Sim: 실물 없이 같은 스택을 돌려 보기#
NVIDIA Isaac Sim 은 물리 · 센서까지 흉내 내는 3D 로봇 시뮬레이터입니다. 공식 문서 6.3 은 myAGV Plus 의 Isaac Sim 장면(롤러까지 모델링한 메카넘 바퀴, 창고 · 다중 선반 · 사무실 장면)과 실물과 같은 ros2_control 스택으로 지도 작성 · Nav2 를 돌리는 방법을 안내합니다.
- 요구 사양: RTX 4080 이상 GPU, Isaac Sim 6.0.1, Ubuntu 22.04 + ROS 2 Humble. 이 강좌의 브라우저 시뮬레이터와 달리 고성능 PC 가 필요합니다.
- 순서는 실물과 같습니다: 시뮬레이터 실행 →
sim_bringup→slam_gmapping ... use_sim_time:=true→ teleop →map_saver_cli ... --ros-args -p use_sim_time:=true→navigation2_active.launch.py use_sim_time:=true→ 2D Pose Estimate → Nav2 Goal. 모든 노드에 **use_sim_time:=true** 를 주는 것이 핵심입니다(시뮬레이터 시계/clock을 따르게). - 문서는 시뮬레이터에서도
/odom이 바퀴 속도로 계산되어 메카넘 미끄러짐 때문에 실제로 흐른다고 적고 있습니다. 2교시의 AMCL 이 왜 필요한지 시뮬레이터에서도 볼 수 있습니다. - 자세한 설치 · 실행은 공식 문서와
myagv_plus_isaac_sim저장소(참고자료)를 따릅니다. 이 차시에서는 실행하지 않습니다.
실습 과제 4#
시뮬레이터 5 의 "목표 방향 맞추기" 단계만 떼어 냈습니다(5 Hz, 명령 한 주기 지연, 최소 회전 0.4 rad/s, 각가속도 1 rad/s², 위치 추정 방향 흔들림 0.008 rad). 처음 방향 오차 90° 에서 10 초 안에 허용 오차 안으로 들어오는지, 시드 20 개로 확인합니다. TODO 두 곳(도착 판정, 회전 명령)을 채우세요. 예측 먼저: 0.25 · 0.1 · 0.05 · 0.02 · 0.01 rad 중 어디서부터 실패가 생길까요?
import math
import random
DT, WMAX, WMIN, ACC, GAIN = 0.2, 1.0, 0.4, 1.0, 1.5 # 5 Hz, 최대 / 최소 회전 속도, 각가속도 한계
YAW_NOISE = 0.008 # 위치 추정(AMCL)의 방향 흔들림 (1 s 마다 바뀜)
def rotate_to_goal(yaw_tol, e0=math.pi / 2, seed=1, limit=10.0):
"""제자리 회전으로 목표 방향 맞추기. limit 초 안에 못 맞추면 실패 (progress checker)"""
rnd = random.Random(seed)
th, w, cmd_prev, t, flips, last_sign, bias = 0.0, 0.0, 0.0, 0.0, 0, 0, 0.0
goal = e0
while t < limit:
if round(t * 10) % 10 == 0:
bias = YAW_NOISE * rnd.gauss(0, 1)
e = goal - (th + bias + 0.002 * rnd.gauss(0, 1)) # 로봇이 "안다고 생각하는" 오차
# TODO 1: |e| 가 yaw_tol 보다 작으면 (True, t, flips, abs(goal - th)) 를 돌려줍니다
# TODO 2: 회전 명령 = 부호는 e 와 같고, 크기는 GAIN x |e| 를 WMIN ~ WMAX 로 자른 값
cmd = GAIN * e
sign = 1 if cmd > 0 else -1
if last_sign and sign != last_sign:
flips += 1
last_sign = sign
use, cmd_prev = cmd_prev, cmd # 명령은 한 주기 늦게 바퀴에 도착
w += max(-ACC * DT, min(ACC * DT, use - w)) # 가속도 한계
th += w * DT
t = round(t + DT, 1)
return False, t, flips, abs(goal - th)
print("yaw_goal_tolerance 성공 평균 시간 평균 방향 바뀜 최종 오차 평균")
for tol in (0.25, 0.1, 0.05, 0.02, 0.01):
rs = [rotate_to_goal(tol, seed=s) for s in range(1, 21)]
ok = [r for r in rs if r[0]]
print("%8.2f rad (%4.1f deg) %3d/20 %6.2f s %8.1f 번 %10.3f rad" % (
tol, math.degrees(tol), len(ok), sum(r[1] for r in rs) / 20,
sum(r[2] for r in rs) / 20, sum(r[3] for r in rs) / 20))
print("\n한 주기에 최소로 도는 각도 = WMIN x DT = %.2f rad, 허용 구간 폭 = 2 x tol" % (WMIN * DT))풀이 코드 보기
import math
import random
DT, WMAX, WMIN, ACC, GAIN = 0.2, 1.0, 0.4, 1.0, 1.5 # 5 Hz, 최대 / 최소 회전 속도, 각가속도 한계
YAW_NOISE = 0.008 # 위치 추정(AMCL)의 방향 흔들림 (1 s 마다 바뀜)
def rotate_to_goal(yaw_tol, e0=math.pi / 2, seed=1, limit=10.0):
"""제자리 회전으로 목표 방향 맞추기. limit 초 안에 못 맞추면 실패 (progress checker)"""
rnd = random.Random(seed)
th, w, cmd_prev, t, flips, last_sign, bias = 0.0, 0.0, 0.0, 0.0, 0, 0, 0.0
goal = e0
while t < limit:
if round(t * 10) % 10 == 0:
bias = YAW_NOISE * rnd.gauss(0, 1)
e = goal - (th + bias + 0.002 * rnd.gauss(0, 1)) # 로봇이 "안다고 생각하는" 오차
if abs(e) < yaw_tol:
return True, t, flips, abs(goal - th)
cmd = math.copysign(max(WMIN, min(WMAX, GAIN * abs(e))), e)
sign = 1 if cmd > 0 else -1
if last_sign and sign != last_sign:
flips += 1
last_sign = sign
use, cmd_prev = cmd_prev, cmd # 명령은 한 주기 늦게 바퀴에 도착
w += max(-ACC * DT, min(ACC * DT, use - w)) # 가속도 한계
th += w * DT
t = round(t + DT, 1)
return False, t, flips, abs(goal - th)
print("yaw_goal_tolerance 성공 평균 시간 평균 방향 바뀜 최종 오차 평균")
for tol in (0.25, 0.1, 0.05, 0.02, 0.01):
rs = [rotate_to_goal(tol, seed=s) for s in range(1, 21)]
ok = [r for r in rs if r[0]]
print("%8.2f rad (%4.1f deg) %3d/20 %6.2f s %8.1f 번 %10.3f rad" % (
tol, math.degrees(tol), len(ok), sum(r[1] for r in rs) / 20,
sum(r[2] for r in rs) / 20, sum(r[3] for r in rs) / 20))
print("\n한 주기에 최소로 도는 각도 = WMIN x DT = %.2f rad, 허용 구간 폭 = 2 x tol" % (WMIN * DT))실행 결과:
yaw_goal_tolerance 성공 평균 시간 평균 방향 바뀜 최종 오차 평균
0.25 rad (14.3 deg) 20/20 2.00 s 0.0 번 0.200 rad
0.10 rad ( 5.7 deg) 20/20 2.20 s 0.0 번 0.069 rad
0.05 rad ( 2.9 deg) 20/20 2.40 s 0.0 번 0.022 rad
0.02 rad ( 1.1 deg) 17/20 4.63 s 2.0 번 0.040 rad
0.01 rad ( 0.6 deg) 9/20 7.60 s 4.8 번 0.088 rad
한 주기에 최소로 도는 각도 = WMIN x DT = 0.08 rad, 허용 구간 폭 = 2 x tol- 한 주기에 최소로 도는 각도는 0.4 × 0.2 = 0.08 rad 입니다. 허용 구간 폭(2 × tol)이 이보다 넉넉하면(0.05 rad 이상) 한 번에 들어오고, 좁으면(0.02 · 0.01) 건너뛰고 되돌아오기를 되풀이하다 10 초를 넘깁니다. 0.01 rad 는 20 번 중 9 번만 성공했습니다.
- 실패한 경우의 "최종 오차" 가 오히려 큰 것은 계속 왔다 갔다 하는 중간에 시간이 끝났기 때문입니다. 지나치게 정밀한 허용 오차는 정밀도를 높이지 못하고 시간만 잡아먹습니다.
- 해결: 허용 오차를 키우거나(0.05 ~ 0.1 rad), 컨트롤러 주기를 높이거나(
controller_frequency), 목표 근처에서 더 천천히 돌 수 있게 합니다(최소 회전 속도 · 회전 가속 파라미터). 정말 정밀한 방향이 필요한 작업(도킹 · 충전)은 별도의 정밀 정렬 동작을 씁니다(ch11).
Nav2 inflation 층의 식으로 문 가운데의 비용을 계산해, 로봇 반경과 inflation_radius 가 좁은 문 통과에 주는 영향을 표로 봅니다. 그리고 시뮬레이터 5 에서 본 증상마다 먼저 볼 파라미터를 고릅니다. TODO 두 곳을 채우세요.
import math
LETHAL, INSCRIBED = 254, 253
def inflation_cost(d, robot_radius, inflation_radius, csf=3.0):
"""Nav2 inflation layer 식: 장애물까지 거리 d (m) -> 비용 0~253"""
if d <= 0:
return LETHAL
if d < robot_radius:
return INSCRIBED # 로봇 중심이 여기 오면 몸이 닿음
# TODO 1: inflation_radius 보다 멀면 0, 아니면 252 x exp(-csf x (d - robot_radius)) 를 반올림
return 0
print("문 폭 robot_radius 문 가운데 비용 (inflation 0.35 / 0.6 / 1.0) 지나갈 수 있나")
for width in (0.6, 0.7, 0.9):
for rr in (0.22, 0.30, 0.40):
d = width / 2 # 문 가운데에서 양쪽 벽까지 거리
costs = [inflation_cost(d, rr, max(r, rr)) for r in (0.35, 0.6, 1.0)]
ok = "예" if costs[0] < INSCRIBED else "아니오 (Failed to create plan)"
print("%.1f m %.2f m %3d / %3d / %3d %s" % (width, rr, *costs, ok))
# 증상 -> 먼저 볼 파라미터 (시뮬레이터 5 의 결과를 정리)
# TODO 2: 보기에서 골라 채우기 - yaw_goal_tolerance, xy_goal_tolerance, robot_radius (footprint),
# max_vel_x (또는 감속 옵션), inflation_radius, max_vel_y
DIAG = {
"목표 앞에서 좌우로 꿈틀거리다 한참 뒤 도착하거나 실패": "?",
"목표에서 30 cm 쯤 떨어진 곳에 멈추고 성공이라고 함": "?",
"좁은 문 앞에서 계획 실패, 넓은 우회로로도 못 감": "?",
"코너에서 바깥쪽으로 밀려 선반을 긁음": "?",
"벽에 바짝 붙어 지나가며 모서리를 스침": "?",
"메카넘인데 옆으로 한 번도 안 움직임": "?",
}
answers = {
"목표 앞에서 좌우로 꿈틀거리다 한참 뒤 도착하거나 실패": "yaw_goal_tolerance",
"목표에서 30 cm 쯤 떨어진 곳에 멈추고 성공이라고 함": "xy_goal_tolerance",
"좁은 문 앞에서 계획 실패, 넓은 우회로로도 못 감": "robot_radius (footprint)",
"코너에서 바깥쪽으로 밀려 선반을 긁음": "max_vel_x (또는 감속 옵션)",
"벽에 바짝 붙어 지나가며 모서리를 스침": "inflation_radius",
"메카넘인데 옆으로 한 번도 안 움직임": "max_vel_y",
}
score = sum(answers[k] == DIAG[k] for k in DIAG)
print("\n진단 맞힌 개수: %d / %d" % (score, len(DIAG)))
for k, v in DIAG.items():
print(" %-34s -> %s" % (k, v))풀이 코드 보기
import math
LETHAL, INSCRIBED = 254, 253
def inflation_cost(d, robot_radius, inflation_radius, csf=3.0):
"""Nav2 inflation layer 식: 장애물까지 거리 d (m) -> 비용 0~253"""
if d <= 0:
return LETHAL
if d < robot_radius:
return INSCRIBED # 로봇 중심이 여기 오면 몸이 닿음
if d > inflation_radius:
return 0
return round(252 * math.exp(-csf * (d - robot_radius)))
print("문 폭 robot_radius 문 가운데 비용 (inflation 0.35 / 0.6 / 1.0) 지나갈 수 있나")
for width in (0.6, 0.7, 0.9):
for rr in (0.22, 0.30, 0.40):
d = width / 2 # 문 가운데에서 양쪽 벽까지 거리
costs = [inflation_cost(d, rr, max(r, rr)) for r in (0.35, 0.6, 1.0)]
ok = "예" if costs[0] < INSCRIBED else "아니오 (Failed to create plan)"
print("%.1f m %.2f m %3d / %3d / %3d %s" % (width, rr, *costs, ok))
# 증상 -> 먼저 볼 파라미터 (시뮬레이터 5 의 결과를 정리)
DIAG = {
"목표 앞에서 좌우로 꿈틀거리다 한참 뒤 도착하거나 실패": "yaw_goal_tolerance",
"목표에서 30 cm 쯤 떨어진 곳에 멈추고 성공이라고 함": "xy_goal_tolerance",
"좁은 문 앞에서 계획 실패, 넓은 우회로로도 못 감": "robot_radius (footprint)",
"코너에서 바깥쪽으로 밀려 선반을 긁음": "max_vel_x (또는 감속 옵션)",
"벽에 바짝 붙어 지나가며 모서리를 스침": "inflation_radius",
"메카넘인데 옆으로 한 번도 안 움직임": "max_vel_y",
}
answers = {
"목표 앞에서 좌우로 꿈틀거리다 한참 뒤 도착하거나 실패": "yaw_goal_tolerance",
"목표에서 30 cm 쯤 떨어진 곳에 멈추고 성공이라고 함": "xy_goal_tolerance",
"좁은 문 앞에서 계획 실패, 넓은 우회로로도 못 감": "robot_radius (footprint)",
"코너에서 바깥쪽으로 밀려 선반을 긁음": "max_vel_x (또는 감속 옵션)",
"벽에 바짝 붙어 지나가며 모서리를 스침": "inflation_radius",
"메카넘인데 옆으로 한 번도 안 움직임": "max_vel_y",
}
score = sum(answers[k] == DIAG[k] for k in DIAG)
print("\n진단 맞힌 개수: %d / %d" % (score, len(DIAG)))
for k, v in DIAG.items():
print(" %-34s -> %s" % (k, v))실행 결과:
문 폭 robot_radius 문 가운데 비용 (inflation 0.35 / 0.6 / 1.0) 지나갈 수 있나
0.6 m 0.22 m 198 / 198 / 198 예
0.6 m 0.30 m 252 / 252 / 252 예
0.6 m 0.40 m 253 / 253 / 253 아니오 (Failed to create plan)
0.7 m 0.22 m 171 / 171 / 171 예
0.7 m 0.30 m 217 / 217 / 217 예
0.7 m 0.40 m 253 / 253 / 253 아니오 (Failed to create plan)
0.9 m 0.22 m 0 / 126 / 126 예
0.9 m 0.30 m 0 / 161 / 161 예
0.9 m 0.40 m 0 / 217 / 217 예
진단 맞힌 개수: 6 / 6
목표 앞에서 좌우로 꿈틀거리다 한참 뒤 도착하거나 실패 -> yaw_goal_tolerance
목표에서 30 cm 쯤 떨어진 곳에 멈추고 성공이라고 함 -> xy_goal_tolerance
좁은 문 앞에서 계획 실패, 넓은 우회로로도 못 감 -> robot_radius (footprint)
코너에서 바깥쪽으로 밀려 선반을 긁음 -> max_vel_x (또는 감속 옵션)
벽에 바짝 붙어 지나가며 모서리를 스침 -> inflation_radius
메카넘인데 옆으로 한 번도 안 움직임 -> max_vel_y- 문 폭 0.6 · 0.7 m 에서는 문 가운데까지 거리(0.3 · 0.35 m)가 이미 inflation_radius(0.35 이상)보다 가까워, inflation_radius 를 키워도 가운데 비용은 그대로입니다. 통과 여부를 가르는 것은 robot_radius 입니다(가운데 거리 < 반경이면 253 → 통과 불가).
- 0.9 m 문에서는 inflation_radius 가 0.35 → 0.6 으로 커질 때 가운데가 0 에서 126 이상으로 비싸집니다. 계획기는 지나갈 수는 있지만 다른 길이 있으면 돌아가려 합니다.
- 증상 → 파라미터 진단은 "한 번에 하나만 바꾸기" 의 출발점입니다. 실제로는 두세 가지가 겹치는 경우가 많으니, 시뮬레이터 5 처럼 기준 실행을 먼저 기록해 두고 비교합니다.
로봇이 목표 위치에는 도착했는데 제자리에서 왼쪽 · 오른쪽으로 번갈아 조금씩 돌다가 한참 뒤 "Failed to make progress" 가 납니다. 가장 먼저 조절할 파라미터는?
- max_vel_x 를 키운다
- yaw_goal_tolerance 를 키운다 (예: 0.01 → 0.1 rad)
- inflation_radius 를 키운다
- xy_goal_tolerance 를 0 으로 한다
정답 보기
② — 허용 방향 구간(2 × tol)이 한 제어 주기의 최소 회전량보다 좁아 목표 방향을 계속 건너뛰는 현상입니다. 위치는 이미 허용 오차 안이므로 진행 검사기가 "움직임 없음" 으로 실패를 냅니다(실습 과제 4-1).넓은 팔레트를 싣고 나서부터, 전에는 잘 지나가던 0.7 m 문 앞에서 "Failed to create plan" 이 납니다. 맞는 해석은?
- 적재물 크기에 맞춰 footprint(robot_radius)를 키웠더니 문 가운데가 치명 구역이 되었다 — 이 문은 적재 상태로 지나갈 수 없다
- inflation_radius 가 너무 작다
- yaw_goal_tolerance 가 너무 작다
- AMCL 파티클이 부족하다
정답 보기
① — 문 가운데에서 벽까지 0.35 m 인데 반경이 0.40 m 면 문 전체가 253(inscribed) 이상이 됩니다. 반경을 다시 줄이면 계획은 되지만 실제 팔레트가 문틀에 부딪힙니다. 경로(우회로)나 적재 방법을 바꿔야 합니다.시뮬레이터 5 에서 max_vel_x 1.0 으로 코너 모서리에 닿았습니다. 속도를 크게 희생하지 않고 충돌을 없앤 방법은?
- xy_goal_tolerance 를 키운다
- 곡선 · 장애물 근처에서만 속도를 줄이는 감속(Regulated Pure Pursuit 식)을 켠다
- yaw_goal_tolerance 를 줄인다
- AMCL 의 파티클 수를 늘린다
정답 보기
② — 직선에서는 1.0 m/s 를 내고 곡률이 크거나 벽이 가까울 때만 줄여, 충돌 없이 15.8 s 에 도착했습니다(기본 0.4 m/s 는 22.9 s). max_vel_x 를 0.8 로 낮추는 것도 방법이지만 직선에서도 느려집니다.실습 (실물)#
실물 myAGV Plus 로 지도를 만들고, 그 지도로 웨이포인트 순회를 합니다. 시뮬레이터에서 미리 정한 순서와 파라미터를 그대로 가져가 결과를 비교합니다.
- 지도 만들기: bringup →
slam_gmapping→ teleop(기본 속도). 실습 공간을 한 바퀴 돌고 출발점을 다시 지나 루프를 닫은 뒤map_saver_cli -f ~/myagv_plus_ros2/src/myagv_plus_navigation2/map/map_test로 저장합니다. 저장된 yaml 의 resolution · origin 을 적습니다. - 지도 손질: pgm 을 복사해 두고, 로봇이 가면 안 되는 곳 한 곳을 순수 검정으로 칠합니다. 크기 · 형식을 바꾸지 않았는지 확인합니다(
file map_test.pgm). - Nav2 준비:
navigation2_active.launch.py의 지도 이름을map_test.yaml로 바꾸고colcon build --packages-select myagv_plus_navigation2→source install/setup.bash→ bringup →navigation2_active.launch.py. - 위치 추정: 2D Pose Estimate 를 주고, 로봇을 teleop 으로 1 ~ 2 m 움직이며 파티클(RViz 의 ParticleCloud 표시)이 모이는지 봅니다.
ros2 run tf2_ros tf2_echo map odom의 값을 적습니다. - 웨이포인트 순회: 공간 안에 세 지점(선반 대신 상자)을 정하고 Waypoint / Nav Through Poses Mode 로 두 가지 순서를 각각 돌려 시간을 잽니다. 시뮬레이터 4 의 결론(가까운 순서가 짧다)이 맞는지 확인합니다.
- 파라미터 하나 바꾸기: 파라미터 파일에서
yaw_goal_tolerance를 0.25 → 0.05 로 바꿔 빌드 · 재실행하고, 목표 도착 시간과 도착 방향 오차를 비교합니다. 실험이 끝나면 원래 값으로 되돌립니다.
| 기록 | 1회 | 2회 | 비고 |
|---|---|---|---|
| 지도 resolution · origin | |||
| 2D Pose Estimate 후 map → odom | |||
| 순서 A 시간 · 순서 B 시간 | |||
| yaw 0.25 / 0.05 도착 시간 · 방향 오차 |
모델 확인 (3D 뷰어)#
강좌용 myAGV Plus 모델(assets/robot/robot.json)이 아직 없다면 뷰어에 "모델 없음" 안내가 나옵니다. 모델이 준비되면 Nav2 설정과 관련해 다음을 확인합니다.
base_footprint기준 차체의 가장 바깥 모서리 좌표 → 코스트맵footprint다각형의 네 꼭짓점. 범퍼 · 상판 · 적재물이 차체보다 튀어나오면 그 끝을 씁니다.- LiDAR 링크(
laser계열 프레임)의 위치 · 높이 → 2D 지도에 보이는 높이. 그보다 낮거나 높은 장애물은 Rtabmap(카메라)이나 지도 칠하기로 보완합니다. - 카메라 링크의 위치 · 방향 → Rtabmap 복셀 층이 보는 범위.
모델이 없을 때는 아래 계산으로 footprint 와 robot_radius 의 관계를 확인할 수 있습니다.
import math
# 교육용 가정: 차체 0.34 m x 0.24 m (myagv_ros2 의 footprint). Plus 의 실제 치수는 3D 뷰어 · 사양서에서 확인
def radii(length, width):
"""사각형 footprint -> 외접 반경(모서리까지), 내접 반경(가까운 옆면까지)"""
return math.hypot(length / 2, width / 2), min(length, width) / 2
for name, L, W in [("로봇만", 0.34, 0.24), ("상자 적재 0.45 x 0.40", 0.45, 0.40), ("팔레트 적재 0.70 x 0.50", 0.70, 0.50)]:
outer, inner = radii(L, W)
print("%-22s 외접 %.2f m, 내접 %.2f m -> 지나갈 수 있는 가장 좁은 문 (원으로 볼 때) %.2f m"
% (name, outer, inner, 2 * outer))- robot_radius 하나로 로봇을 원으로 보면 외접 반경을 써야 모서리가 안 닿습니다. 로봇만일 때 0.21 m 이므로 myagv.yaml 의 0.22 m 와 비슷합니다.
- 팔레트(0.70 × 0.50)를 실으면 외접 반경이 0.43 m 가 되어, 원으로 보면 0.86 m 보다 좁은 문은 못 지나갑니다. 사각형
footprint를 쓰면 방향을 맞춰 더 좁은 곳도 지날 수 있지만 계산이 늘어납니다(Smac 계획기 등).
자주 나는 오류와 해결#
1) Nav2 를 켰는데 로봇이 지도에 안 나타나고 transform 시간 초과가 반복됨#
[amcl]: AMCL cannot publish a pose or update the transform. Please set the initial pose...
[global_costmap.global_costmap]: Timed out waiting for transform from base_footprint to map to become available, tf error: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist- 원인: AMCL 이 초기 위치를 못 받아
map → odom을 발행하지 않았습니다. TF 트리가 map 에서 끊겨 코스트맵 · BT Navigator 가 기다리다 시간 초과를 냅니다. bringup 이 안 떠서odom → base_footprint가 없는 경우도 같은 증상입니다. - 해결: RViz 에서 2D Pose Estimate 를 줍니다. 그래도 나면
ros2 run tf2_ros tf2_echo odom base_footprint로 bringup 쪽 TF 가 나오는지 확인합니다(ch09).
2) Nav2 Goal 을 눌렀는데 "Goal was rejected"#
[bt_navigator]: Goal was rejected by server
[planner_server]: GridBased plugin failed to plan calculation to (8.40, 3.10): "Goal Coordinates of(8.400000, 3.100000) was outside bounds"- 원인: 목표가 지도 밖이거나, 벽 · 팽창 구역 안(치명 비용)에 찍혔습니다. Nav2 서버가 아직 활성화되지 않은 상태(lifecycle)에서 보내도 거절됩니다. (문구는 버전 · 계획기에 따라 다름)
- 해결: 지도 안의 흰 칸, 벽에서 robot_radius 이상 떨어진 곳을 찍습니다. 실행 직후라면
ros2 lifecycle get /bt_navigator가active인지 확인합니다.
3) RViz 에서 지도와 스캔이 일정하게 어긋나 보임 · 목표가 모두 같은 방향으로 틀어짐#
(RViz) 빨간 스캔이 모든 벽에서 약 0.5 m 오른쪽에 그려짐, 2D Pose Estimate 를 줘도 곧 다시 어긋남- 원인: yaml 의
origin(또는resolution)을 손으로 고치다 틀렸거나, 다른 지도의 yaml 과 pgm 을 섞었습니다. 어긋남이 어디서나 같으면 origin, 원점에서 멀수록 커지면 resolution 입니다(시뮬레이터 2). - 해결: 저장 도구가 만든 yaml 로 되돌립니다. 지도를 고칠 때는 이미지만 칠하고 yaml 은 건드리지 않습니다.
4) map_saver_cli 가 저장하지 못함#
[map_saver]: Failed to spin map subscription
[map_saver]: Map saver failed- 원인: 정해진 시간 안에
/map토픽을 못 받았습니다. Gmapping 을 이미 껐거나, 다른ROS_DOMAIN_ID의 터미널에서 실행했거나(ch09), 지도가 아직 한 번도 발행되지 않았습니다. - 해결: Gmapping 이 켜진 상태에서, 같은 환경(source · 도메인 ID)의 터미널로 저장합니다.
ros2 topic info /map으로 퍼블리셔가 있는지 먼저 봅니다. 저장 경로의 폴더가 있는지도 확인합니다.
5) 목표 앞에서 좌우로 꿈틀거리다 "Failed to make progress"#
[controller_server]: Failed to make progress
[bt_navigator]: Goal failed- 원인:
yaw_goal_tolerance(또는xy_goal_tolerance)가 한 제어 주기에 움직이는 최소량보다 작아 목표를 건너뛰며 맴돕니다. 로봇이 물리적으로 막혔거나 바퀴가 헛돌 때도 같은 메시지가 납니다(진행 검사기: 정해진 시간 동안 정해진 거리를 못 움직임). - 해결: 허용 오차를 키웁니다(방향 0.05 ~ 0.25 rad). 정말 막힌 것인지 RViz 의 지역 코스트맵과 로봇 주변을 봅니다(시뮬레이터 5 ①, 실습 과제 4-1).
6) 지도 이름을 바꿨는데 예전 지도가 뜸 · 지도 파일을 못 찾음#
[map_server]: Failed processing YAML file /home/er/myagv_plus_ros2/install/myagv_plus_navigation2/share/myagv_plus_navigation2/map/map_test.yaml at position (-1:-1) for reason: bad file- 원인:
src/의 launch 파일 · 지도를 고치고 빌드하지 않았거나, 빌드 전install/에 새 지도가 복사되지 않았습니다. yaml 안의image:이름과 pgm 파일 이름이 다른 경우도 있습니다. - 해결:
colcon build --packages-select myagv_plus_navigation2→source install/setup.bash후 다시 실행합니다. yaml 의image:가 같은 폴더의 pgm 이름과 같은지 확인합니다. (경로 · 사용자 이름은 예시)
7) "Starting point in lethal space" · 경로 계획 실패#
[planner_server]: Starting point in lethal space! Cannot create feasible plan.
[planner_server]: GridBased plugin failed to plan calculation- 원인: 로봇 위치가 코스트맵의 치명 구역 안입니다. 위치 추정이 틀려 지도상 벽 위에 있거나, robot_radius · footprint 가 실제보다 커서 좁은 통로 안의 로봇이 "벽에 닿은" 것으로 보이거나, 사람이 로봇 바로 옆에 서 있습니다.
- 해결: 스캔과 지도가 맞는지(2D Pose Estimate) 먼저 봅니다. footprint 를 적재 상태에 맞게 고치고, 좁은 곳에서 출발해야 하면 teleop 으로 넓은 곳까지 꺼낸 뒤 목표를 줍니다(시뮬레이터 5 ②).
8) 회전할 때 파티클이 흩어지고 위치가 튐 · 메시지를 버린다는 경고#
[amcl]: Message Filter dropping message: frame 'laser' at time 1700000000.123 for reason 'the timestamp on the message is earlier than all the data in the transform cache'- 원인: 스캔 시각과 TF(odom → base → laser) 시각이 맞지 않습니다. 여러 컴퓨터의 시계가 다르거나(노트북에서 RViz 만 띄우는 것은 괜찮지만 노드를 나눠 돌릴 때), 부하가 커서 TF 가 늦습니다. Isaac Sim 에서
use_sim_time:=true를 빠뜨려도 같은 증상입니다. - 해결: 로봇과 노트북의 시계를 맞추고(NTP), 무거운 노드(RViz · 빌드)를 로봇에서 줄입니다. 시뮬레이터에서는 모든 노드에
use_sim_time:=true를 줍니다.
과제#
- 우리 창고 지도 보고서: 시뮬레이터 1 에서 0.3 · 0.6 · 1.0 m/s 로 각각 지도를 만들어 저장하고, 시뮬레이터 2 에서 스캔 일치율과 출고 경로 길이를 비교한 표를 만드세요. "지도 작성 속도 규칙" 을 한 줄로 정리합니다.
- 2D Pose Estimate 허용 오차 실험: 시뮬레이터 3 의 선반 통로에서 초기 위치를 0.5 · 1.0 · 1.5 · 2.0 m 어긋나게(지도를 끌어 직접 지정) 주고, 수렴까지 주행 거리를 표로 만드세요. "몇 m 이상 틀렸으면 기다리지 말고 다시 준다" 는 기준을 정해 봅니다.
- 파라미터 튜닝 노트: 시뮬레이터 5 에서 "시간 18 s 이하 · 충돌 0 · 방향 오차 0.1 rad 이하" 를 동시에 만족하는 설정을 찾고, 바꾼 순서와 📊 결과를 기록하세요. 실습 과제 3-1 을 고쳐 선반이 5 곳일 때(지점 좌표는 직접 정함) 120 가지 순서 중 최단 순서도 구해 봅니다.
참고자료#
- myAGV Plus 공식 문서 (영문): https://github.com/elephantrobotics/myagvPlus_docs/tree/main/MYAGV_PLUS_EN
- 6.2.4 Real-time Mapping with Gmapping (지도 작성 · 저장): https://github.com/elephantrobotics/myagvPlus_docs/blob/main/MYAGV_PLUS_EN/6-Software_Development_Guide/6.2-ApplicationBaseROS2/6.2.4-Real-time_Mapping_with_Gmapping.md
- 6.2.5 Map Navigation (launch 수정 · 빌드 · 2D Pose Estimate · Nav2 Goal · 웨이포인트): https://github.com/elephantrobotics/myagvPlus_docs/blob/main/MYAGV_PLUS_EN/6-Software_Development_Guide/6.2-ApplicationBaseROS2/6.2.5-Navigation-Map_Navigation.md
- 6.2.6 Rtabmap (3D 지도 작성 · 동시 내비게이션): https://github.com/elephantrobotics/myagvPlus_docs/blob/main/MYAGV_PLUS_EN/6-Software_Development_Guide/6.2-ApplicationBaseROS2/6.2.6-Rtabmap.md
- 6.3 Isaac Sim (myAGV Plus 시뮬레이션): https://github.com/elephantrobotics/myagvPlus_docs/blob/main/MYAGV_PLUS_EN/6-Software_Development_Guide/6.3-IsaacSim/myAGV_Plus_Isaac_Sim.md
- myagv_plus_isaac_sim 저장소: https://github.com/elephantrobotics/myagv_plus_isaac_sim
- myagv_ros2 저장소 (이전 모델 myAGV 용,
galactic-JN): https://github.com/elephantrobotics/myagv_ros2 - myagv_ros2 의 Nav2 파라미터
myagv.yaml(AMCL · DWB · 코스트맵 · 목표 검사기): https://github.com/elephantrobotics/myagv_ros2/blob/galactic-JN/myagv_navigation2/param/myagv.yaml - myagv_ros2 의
navigation2_active.launch.py: https://github.com/elephantrobotics/myagv_ros2/blob/galactic-JN/myagv_navigation2/launch/navigation2_active.launch.py - Nav2 문서 (개념 · 구성 요소): https://docs.nav2.org/concepts/index.html
- Nav2 문서 — AMCL 파라미터: https://docs.nav2.org/configuration/packages/configuring-amcl.html
- Nav2 문서 — Map Server / Saver (map.yaml 형식): https://docs.nav2.org/configuration/packages/configuring-map-server.html
- Nav2 문서 — Controller Server (목표 · 진행 검사기): https://docs.nav2.org/configuration/packages/configuring-controller-server.html
- Nav2 문서 — Inflation Layer: https://docs.nav2.org/configuration/packages/costmap-plugins/inflation.html
- Nav2 문서 — Regulated Pure Pursuit: https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html
- Nav2 문서 — Keepout Filter (금지 구역 마스크): https://docs.nav2.org/tutorials/docs/navigation2_with_keepout_filter.html
- Nav2 Simple Commander API: https://docs.nav2.org/commander_api/index.html
- RTAB-Map 프로젝트: http://introlab.github.io/rtabmap/
- D. Fox, Adapting the Sample Size in Particle Filters Through KLD-Sampling (AMCL 의 적응형 파티클 수), International Journal of Robotics Research, 2003
- S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics (몬테카를로 위치 추정 · likelihood field · 납치된 로봇), MIT Press, 2005
- myAGV Plus 제품 사진 · 소개 (hyenc.net): https://www.hyenc.net/products/35
- 이 사이트의 도구: URDF 뷰어 · Python Playground
- 이전 차시: ch09 ROS 2 Humble 과 myAGV Plus 패키지 · 다음 차시: ch11 물류 통합 프로젝트: 자동충전과 다중 AGV