전체 과정 P4 물류 현장으로 — ROS 2 운용과 미션 통합 · 약 200분 · 중급

CH09ROS 2 · Nav2 로 실제 운용하기

ROS 2 노드 · 토픽 · 액션 개념을 시뮬레이터 그래프로 익히고, agv_pro_ros2 패키지로 bringup · 내비게이션 · 웨이포인트 주행 · 자동 충전을 운용합니다.

선수 챕터: CH03 CH04 CH05 CH06 CH07 CH08
Python Playground시뮬레이터

학습 목표#

  • 노드 · 토픽 · 서비스 · 액션의 차이를 말하고, myAGV Pro 의 bringup 이 만드는 그래프(/cmd_vel → agv_pro_node → odom · imu · voltage · TF)를 그릴 수 있습니다. /cmd_vel 의 발행 주기 · 지연 · 손실을 바꾸면 로봇 움직임이 왜 끊기는지 시뮬레이터로 설명할 수 있습니다.
  • agv_pro_ros2 저장소의 패키지(bringup · base · description · gazebo · navigation2 · msgs · autocharge)가 각각 무슨 일을 하는지 말하고, agv_pro_msgs 서비스(SetLedColor · GetDigitalInput · SetDigitalOutput)를 ch03 의 Python API 와 짝지을 수 있습니다.
  • 클론 → colcon build → source → ros2 launch 의 순서를 지키고, 로그 원문을 보고 "source 누락 · 시리얼 포트 · 비상정지 · 배터리 전압 · ROS_DOMAIN_ID 불일치" 중 원인을 고를 수 있습니다.
  • NavigateToPose · NavigateThroughPoses · FollowWaypoints 세 액션의 차이를 피드백(남은 거리 · 복구 횟수 · 현재 웨이포인트)으로 구분하고, 목표 자세의 yaw θ 를 쿼터니언 (z, w) = (sin θ/2, cos θ/2) 로 바꿔 웨이포인트 순회 스크립트를 설계할 수 있습니다.
  • agv_pro_autocharge 의 충전 흐름(충전대 위치 → 1 m 앞까지 내비게이션 → 정밀 도킹)을 설명하고, 배터리 소모율 · 미션 길이 · 충전대 거리로부터 "미션 중 방전되지 않는 충전 시작 임계값" 을 계산할 수 있습니다. 여러 대를 운용할 때 ROS_DOMAIN_ID 와 namespace 가 각각 무엇을 나누는지 말할 수 있습니다.

이론#

ch03 에서는 Python 한 파일(pymycobot 의 MyAGVPro)로 로봇을 직접 움직였고, ch04 ~ ch08 에서는 오도메트리 · 라이다 · SLAM · AMCL · 경로 계획을 원리부터 따라왔습니다. 실제 현장에서는 이것들을 한 프로그램에 다 넣지 않습니다. 드라이버 · 라이다 · 위치 추정 · 경로 계획이 각각 따로 돌고, 서로 정해진 이름의 "통로" 로 데이터를 주고받습니다. 이 틀이 ROS 2 이고, 그 위의 내비게이션 묶음이 Nav2 입니다.

이 차시에서는 myAGV Pro 의 공식 ROS 2 저장소 [elephantrobotics/agv_pro_ros2](https://github.com/elephantrobotics/agv_pro_ros2) (humble 브랜치, Ubuntu 22.04 + ROS 2 Humble) 를 기준으로 실제 운용 순서를 따라갑니다.

교시질문시뮬레이터실습
1노드끼리 무엇을 주고받는가?노드 그래프 · /cmd_vel 발행 주기 · 지연Twist 가짜 발행기, 서비스 요청 만들기
2로봇에서 ROS 2 를 어떻게 켜는가?bringup 진단 콘솔로그 5개 오류 진단 카드
3목표 지점은 어떻게 보내는가?창고 웨이포인트 모드 (액션 3종)yaw ↔ 쿼터니언, 순회 스크립트 설계
4언제 충전하러 가는가? 여러 대는?배터리 임계값 · 다중 로봇 격리충전 임계값 찾기, 충전 접근 목표 계산
agv_pro_ros2 (humble) — 이 차시에서 쓰는 패키지와 관계 agv_pro_description URDF · 메시 (ch01) agv_pro_msgs LED · IO 서비스 정의 4개 agv_pro_base agv_pro_node (C++ 드라이버) agv_pro_bringup base + 라이다 + TF 한 번에 agv_pro_gazebo PC 시뮬레이션 (실물 대신) agv_pro_navigation2 map · agvpro.yaml · 예제 3개 agv_pro_autocharge 충전대 접근 · 도킹 라이다 드라이버 · SLAM Lslidar · livox · gmapping … 먼저 켬 Nav2 위에서 화살표 = "이것을 쓴다 / 먼저 켜 둔다"
agv_pro_ros2 저장소에서 이 차시가 쓰는 패키지. bringup 이 드라이버(base) · 모델(description) · 라이다를 한 번에 켜고, 그 위에 navigation2 를, 다시 그 위에 autocharge 를 올립니다. msgs 는 LED · IO 서비스의 "모양" 만 정의합니다.
이 차시의 값과 시뮬레이터에 대해
  • 패키지 · 파일 · 매개변수 이름은 공개 저장소(humble 브랜치)와 공식 문서 6.2 장(ROS2)을 읽은 것입니다. 로봇에 설치된 버전과 다를 수 있으니 실물에서는 ros2 launch ... --show-args · ros2 param get 으로 확인합니다.
  • 문서와 저장소가 다른 곳이 있습니다. 예: 공식 문서의 Nav2 페이지에 보이는 일부 값(inflation_radius · controller_frequency 등)이 ch08 에서 읽은 저장소의 agvpro.yaml 과 다르고, Gazebo 실행 명령의 패키지 이름도 다릅니다(2교시). 이런 곳은 본문에 표시했습니다.
  • 시뮬레이터 1 ~ 5 는 ROS 2 · Nav2 의 동작 방식을 따라 만든 교육용 모델 입니다. 터미널 로그는 원리를 보여 주려고 재구성한 것이며 실제 문구는 버전마다 다릅니다. 진짜 원문은 "자주 나는 오류와 해결" 에 따로 모았습니다.

1교시 · ROS 2 노드 · 토픽 · 메시지#

네 가지 통신 방식#

ROS 2 프로그램 하나하나를 노드(node) 라고 부릅니다. 노드끼리는 네 가지 방식으로 이야기합니다.

방식모양비유myAGV Pro 예
토픽(topic)한쪽이 계속 발행(publish), 여럿이 구독(subscribe). 답장 없음라디오 방송/cmd_vel (속도 명령), odom, /scan, /tf
서비스(service)요청 1번 → 응답 1번. 짧게 끝나는 일전화 한 통set_led_color, get_digital_input
액션(action)목표(goal) → 진행 중 피드백 여러 번 → 결과(result). 중간 취소 가능택배 배송 조회navigate_to_pose, follow_waypoints
매개변수(parameter)노드의 설정값. 실행 중 읽기 · 쓰기기계의 다이얼port_name, max_vel_x

토픽에 흐르는 데이터 한 덩어리를 메시지(message) 라고 하고, 메시지에는 정해진 형(type) 이 있습니다. 이 차시에서 가장 많이 보는 네 가지입니다.

토픽형주요 필드앞 차시
/cmd_velgeometry_msgs/msg/Twistlinear.x · linear.y (m/s), angular.z (rad/s)ch02 · ch03
odomnav_msgs/msg/Odometrypose.pose (위치 · 쿼터니언), twist.twist (속도)ch04
/scansensor_msgs/msg/LaserScanangle_min · angle_increment · ranges[]ch05
/tftf2_msgs/msg/TFMessage좌표계 사이의 변환(부모 → 자식)ch04 · ch07

Twist 는 3차원 일반형이라 linear.z · angular.x · angular.y 도 있지만, 바닥을 다니는 로봇은 셋만 씁니다. 전방향 로봇인 myAGV Pro 는 linear.y (옆 이동)까지 씁니다.

agv_pro_bringup.launch.py 를 켰을 때의 ROS 2 그래프 (namespace 비움, lidar_type n10p) teleop / Nav2 속도 명령을 내는 노드 agv_pro_node agv_pro_base · 50 Hz 루프 직렬 1 000 000 bps /cmd_vel (Twist) 하위 제어기 · 모터 /dev/agvpro_controller odom (Odometry) imu (Imu) voltage (Float32) /tf odom → base_footprint lslidar_driver N10P · /dev/agvpro_lidar /scan (LaserScan) robot_state_publisher → /tf_static 타원 = 노드, 사각 = 토픽 서비스: set_led_color · set_led_mode set_digital_output · get_digital_input
bringup 을 켜면 생기는 그래프. agv_pro_node 는 /cmd_vel 을 받아 하위 제어기로 보내고, 50 Hz 마다 odom · imu · voltage 를 발행하며 odom → base_footprint TF 를 방송합니다. 라이다 드라이버는 /scan 을 냅니다.

저장소의 agv_pro_base/src/agv_pro_ros.cpp 를 읽어 보면 드라이버 노드의 동작이 다음과 같습니다.

항목값 (코드에서 읽음)뜻
구독/cmd_vel (절대 이름, 큐 10)namespace 를 줘도 이 이름은 바뀌지 않음 → 4교시
속도 자르기linear.x ±1.5, linear.y ±1.0, angular.z ±1.0범위를 넘는 값은 잘려서(클램프) 전달
발행odom · imu · voltage (상대 이름)namespace 를 주면 /agv_pro1/odom 처럼 바뀜
루프20 ms 타이머 = 50 Hz읽기 → odom · voltage · imu 발행
TFodom → base_footprintch04 에서 본 그 변환
직렬/dev/agvpro_controller, 1 000 000 bpsch03 과 같은 하위 제어기
토픽 이름 앞의 / 는 무슨 뜻인가요?

/cmd_vel 처럼 / 로 시작하면 절대 이름 이라 어디서 실행해도 같은 토픽입니다. odom 처럼 / 가 없으면 상대 이름 이라 노드의 namespace 가 앞에 붙습니다(agv_pro1 → /agv_pro1/odom). 여러 대를 운용할 때 이 차이가 결정적입니다(4교시).

토픽은 "마지막으로 받은 값" 만 남습니다#

/cmd_vel 은 서비스가 아니라 토픽이라서, 받는 쪽은 가장 최근에 받은 명령 으로 움직입니다. 그래서 세 가지 질문이 생깁니다.

  1. 명령이 드물게 오면(발행 주기가 낮으면) 그 사이에 로봇은 무엇을 하나요?
  2. 명령이 늦게 오면(네트워크 지연), 또는 중간에 빠지면(손실) 어떻게 되나요?
  3. 명령을 내던 노드가 죽으면(Ctrl+C, Wi-Fi 끊김) 로봇은 멈추나요?

세 번째 질문의 답은 드라이버 · 펌웨어가 "일정 시간 명령이 없으면 멈춤" 이라는 명령 타임아웃(watchdog) 을 갖고 있느냐에 달려 있습니다. myAGV Pro 의 agv_pro_node 코드에서는 이런 타임아웃을 찾지 못했습니다(펌웨어 쪽 동작은 확인 필요). 그래서 이 시뮬레이터는 타임아웃을 "없음" 부터 1.0 s 까지 바꿔 볼 수 있게 했습니다.

예측하기: 2 Hz 로 보내면?#

키보드 원격 조작(teleop)이 0.3 m/s 전진 을 보낸다고 합시다. 시뮬레이터를 열기 전에 공책에 적어 보세요.

  1. 50 Hz 로 보낼 때와 2 Hz 로 보낼 때, 로봇의 평균 속도는 같을까요? (타임아웃 0.2 s 라고 가정)
  2. 타임아웃이 "없음" 이면 2 Hz 라도 부드럽게 갈까요? 그 대가는 무엇일까요?
  3. 6 초 뒤 teleop 을 Ctrl+C 로 끄면, 타임아웃 없음 · 0.5 s 각각 로봇은 언제 멈출까요?

시뮬레이터 1 · 노드 그래프와 /cmd_vel 발행 주기 · 지연#

위쪽은 노드 그래프입니다. 점 하나가 Twist 메시지 하나이고, 빨간 × 는 손실된 메시지입니다. 가운데 그래프는 시간에 따른 속도(점선 = 발행한 명령, 옅은 선 = 드라이버가 받은 명령, 굵은 선 = 실제 바퀴 속도), 아래 띠는 이상적인 위치(옅은 상자)와 실제 위치입니다. 드라이버는 50 Hz 로 돌고, 바퀴 속도는 가속도 1.0 m/s² 이내로 바뀐다고 가정합니다(교육용 값).

시뮬레이터 1 · /cmd_vel 노드 그래프 (teleop → agv_pro_node, 명령 0.3 m/s 를 t = 0.5 ~ 6.0 s 동안 발행)
명령 타임아웃 · 6 s 뒤 ·

해 볼 것

  1. 예측 확인 — 처음 값(10 Hz · 30 ms · 타임아웃 0.5 s)에서 이동 거리가 이상적인 값과 거의 같은지 봅니다. 이어 발행 주기를 2 Hz 로 내리고 타임아웃을 0.2 s 로 바꿉니다. 끊김 횟수와 이동 거리 비율은 얼마인가요? 굵은 선이 톱니 모양이 되는 이유를 "명령 간격 0.5 s vs 타임아웃 0.2 s" 로 설명합니다.
  2. 2 Hz 그대로 타임아웃을 없음 으로 바꿉니다. 끊김은 사라지나요? 이제 "6 s 뒤" 를 teleop 종료 (Ctrl+C) 로 바꾸고 ▶ 재생합니다. 아래 띠의 상자는 어디서 멈추나요?
  3. 20 Hz · 타임아웃 0.5 s 에서 지연 을 0 → 300 → 600 ms 로 올립니다. 끊김은 생기나요? 멈춘 시각은 어떻게 바뀌나요? 지연과 타임아웃은 서로 다른 문제라는 것을 확인합니다.
  4. 손실률 30 % 를 주고 타임아웃을 0.1 s · 0.5 s 로 바꿔 봅니다. 주기가 몇 Hz 이상이면 0.1 s 타임아웃에서도 끊김이 거의 없나요? (힌트: 연속 손실 두 개 = 간격 3 배)
  5. 지연 흔들림 300 ms 를 줍니다. "순서 뒤바뀜" 이 생기는 조건은 무엇인가요? 6 s 직후 0 명령보다 늦게 도착한 0.3 명령은 로봇을 어떻게 만드나요?

관찰 포인트 — 토픽은 "마지막 값" 만 남으므로, 받는 쪽의 동작은 명령 간격과 타임아웃의 크기 비교 로 정해집니다. 간격(1/f)이 타임아웃보다 길면 매 간격마다 멈췄다 다시 출발하는 끊김이 생기고, 타임아웃이 없으면 끊김은 없지만 발행 노드가 죽었을 때 마지막 명령으로 계속 달리는 위험이 생깁니다. 그래서 속도 명령은 보통 타임아웃보다 3 ~ 5 배 빠르게(예: 타임아웃 0.5 s 면 10 ~ 20 Hz) 보냅니다. Nav2 의 controller 는 controller_frequency 로 일정하게 발행합니다. 지연은 끊김을 만들지 않고 모든 반응을 그만큼 늦출 뿐이며, 흔들림이 주기보다 커지면 메시지 순서가 뒤바뀝니다.

확인 문제 1-1

시뮬레이터 1 에서 발행 2 Hz · 지연 30 ms · 타임아웃 0.2 s 로 두었더니 굵은 선이 톱니 모양이 되고 이동 거리가 이상적인 값의 절반 아래로 떨어졌습니다. 가장 알맞은 해석은?

  1. 2 Hz 메시지는 하나에 0.5 s 분량의 이동을 담고 있어서, 하나라도 빠지면 그만큼 덜 간다
  2. 명령 간격 0.5 s 가 타임아웃 0.2 s 보다 길어서, 매 간격마다 0.3 s 가량은 "명령 없음 → 0" 으로 처리되어 섰다 출발하기를 반복한다
  3. 지연 30 ms 때문에 메시지가 순서대로 오지 않는다
  4. 드라이버의 50 Hz 루프가 2 Hz 메시지를 25 번씩 다시 처리해서 느려진다
정답 보기② — 토픽 메시지는 "지금 이 속도로" 라는 상태일 뿐 이동량을 담지 않습니다(①). 손실률이 0 이어도 같은 현상이 나므로 손실 탓이 아닙니다. 마지막 명령을 받은 지 0.2 s 가 지나면 목표가 0 이 되고, 다음 메시지(0.5 s 뒤)가 올 때까지 감속 · 정지합니다. 지연이 흔들리지 않으면 순서는 그대로입니다(③). 드라이버는 같은 메시지를 다시 쓰는 것이지 일을 더 하지 않습니다(④). 해결은 주기를 타임아웃보다 충분히 빠르게(예: 10 Hz 이상) 올리는 것입니다.
확인 문제 1-2

타임아웃 "없음" · 10 Hz 에서 6 s 에 teleop 을 Ctrl+C 로 껐더니 로봇이 10 s 가 지나도 멈추지 않았습니다. 같은 상황에서 타임아웃 0.5 s 라면?

  1. 똑같이 멈추지 않는다. 타임아웃은 발행 중일 때만 동작한다
  2. 6 s 에 바로 멈춘다. 노드가 꺼지면 ROS 2 가 0 을 대신 보낸다
  3. 마지막 메시지를 받은 뒤 0.5 s 가 지나서 감속을 시작해, 약 6.8 s 무렵 멈춘다
  4. 0.5 s 마다 멈췄다 섰다를 반복한다
정답 보기③ — 마지막 메시지는 6 s 직전(5.9 s 발행 + 지연 30 ms)에 도착합니다. 타임아웃은 "마지막으로 받은 때로부터" 세므로 약 6.43 s 에 목표가 0 이 되고, 가속도 1.0 m/s² 로 0.3 m/s 를 줄이는 데 0.3 s 가 걸려 약 6.7 ~ 6.8 s 에 섭니다. ROS 2 는 노드가 꺼졌다고 0 을 대신 보내 주지 않습니다(②). 그래서 실물에서 "명령 없음 → 정지" 는 드라이버나 펌웨어가 해 줘야 하며, 그것이 없으면 비상정지 버튼이 마지막 수단입니다.

agv_pro_ros2 패키지 지도#

저장소의 맨 위 폴더는 20 개가 넘지만, 대부분은 라이다 드라이버 · SLAM 같은 외부 패키지를 함께 담아 둔 것입니다(Lslidar_ROS2_driver · livox_ros_driver2 · slam_gmapping · cartographer · FAST_LIO · navigation2 등). 이 로봇만의 패키지는 이름이 agv_pro_ 로 시작하는 7 개입니다.

패키지들어 있는 것이 강좌에서
agv_pro_descriptionurdf/agv_pro.urdf, 메시, agv_pro_display.launch.py (RViz 표시)ch01 · ch02
agv_pro_baseagv_pro_node — 직렬 드라이버(C++). /cmd_vel → 모터, odom · imu · voltage 발행, LED · IO 서비스ch03 · ch04
agv_pro_bringuplaunch/agv_pro_bringup.launch.py — base + 라이다 + robot_state_publisher 를 한 번에2교시
agv_pro_gazeboGazebo 실행 파일(agv_pro_gazebo.launch.py · teleop_launch.py 등)2교시
agv_pro_navigation2launch/navigation2_active.launch.py, map/map.yaml, param/agvpro.yaml, scripts/example_*.py 3개ch06 · ch08 · 3교시
agv_pro_msgssrv/ 의 서비스 정의 4개 (LED 색 · LED 모드 · 디지털 입력 · 출력)1교시
agv_pro_autochargecombined_auto_recharger 노드, config/charger_position.json · nav_goal_params.yaml4교시

agv_pro_msgs 서비스와 ch03 API#

ch03 에서는 pymycobot 으로 LED 와 IO 를 직접 다뤘습니다. ROS 2 에서는 agv_pro_node 가 같은 기능을 서비스 로 열어 둡니다. 서비스 정의(.srv)는 --- 위가 요청, 아래가 응답입니다.

text
# agv_pro_msgs/srv/SetLedColor.srv
int32 position
int32 brightness
int32 r
int32 g
int32 b
---
bool success
string message
하고 싶은 일ch03 Python APIROS 2 서비스 (이름 · 형)요청 필드
LED 색 바꾸기set_led_color(position, brightness, (r, g, b))set_led_color · SetLedColorposition (0 · 1), brightness, r · g · b (0 ~ 255)
LED 모드(ch03 에서 다루지 않음)set_led_mode · SetLedModebool mode
디지털 입력 읽기get_pin_input(pin)get_digital_input · GetDigitalInputpin (1 ~ 6) → 응답 state
디지털 출력 쓰기set_pin_output(pin, state)set_digital_output · SetDigitalOutputpin (1 ~ 6), state
bash
# 로봇(bringup 실행 중)에서 - 왼쪽 LED 를 초록으로
ros2 service call /set_led_color agv_pro_msgs/srv/SetLedColor "{position: 0, brightness: 255, r: 0, g: 255, b: 0}"
# 입력 1번 읽기
ros2 service call /get_digital_input agv_pro_msgs/srv/GetDigitalInput "{pin: 1}"

두 방식의 큰 차이는 누가 직렬 포트를 쥐고 있느냐 입니다. bringup 이 실행 중이면 agv_pro_node 가 /dev/agvpro_controller 를 쓰고 있으므로, 같은 때에 ch03 의 Python 스크립트로 같은 포트를 열면 두 프로그램의 바이트가 섞입니다. ROS 2 를 켰다면 LED · IO 도 서비스로 다루는 것이 원칙입니다.

서비스는 "빨리 끝나는 일" 에만

서비스는 응답이 올 때까지 부른 쪽이 기다립니다. "선반 A 까지 가기" 처럼 수십 초 걸리고 중간에 취소하고 싶은 일은 서비스가 아니라 액션 으로 만듭니다. Nav2 의 목표 보내기가 액션인 이유입니다(3교시).

2교시 · bringup 과 Gazebo 시뮬레이션#

작업 공간 만들기 — 클론 · 빌드 · source#

ROS 2 패키지는 작업 공간(workspace) 의 src/ 아래에 두고 colcon 으로 빌드합니다. 공식 README 와 문서 6.2.1 의 순서입니다.

bash
# 1) 클론 - 작업 공간 ~/agv_pro_ros2 의 src 로 바로 받습니다
git clone -b humble https://github.com/elephantrobotics/agv_pro_ros2.git ~/agv_pro_ros2/src
# 2) 의존 패키지 설치
cd ~/agv_pro_ros2
rosdep install --from-paths src --ignore-src -r -y
# 3) 빌드 - 로봇(Jetson Orin Nano Super)에서 처음에는 약 30 분 (공식 문서)
colcon build
# 4) 이 터미널에 작업 공간 환경 불러오기 - 새 터미널마다 다시!
source ~/agv_pro_ros2/install/setup.bash

README 는 git clone ... agv_pro_ros2/src 처럼 브랜치를 적지 않습니다. 기본 브랜치가 humble 이 아니면 -b humble 이 필요하므로 위처럼 적어 두는 것이 안전합니다. 공식 문서는 4) 에 local_setup.bash 를 쓰기도 하는데, local_setup 은 이 작업 공간만, setup 은 아래에 깔린 /opt/ros/humble 까지 함께 불러옵니다.

빌드 시간 줄이기
  • 저장소에는 FAST_LIO · cartographer · rtabmap 같은 큰 패키지가 함께 들어 있어 첫 빌드가 깁니다. 고친 패키지만 다시 빌드하려면 colcon build --packages-select agv_pro_base 처럼 고릅니다.
  • Python 파일(예제 스크립트 · autocharge)을 자주 고친다면 colcon build --symlink-install 로 빌드해 두면 다시 빌드하지 않아도 수정이 반영됩니다.
  • 빌드 중 메모리가 모자라 멈추면 colcon build --parallel-workers 2 처럼 동시에 빌드하는 수를 줄입니다.

bringup — 로봇의 "전원 스위치"#

bash
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py

이 한 줄이 agv_pro_node · joint_state_publisher · robot_state_publisher · 라이다 드라이버를 켭니다. launch 파일이 받는 인자는 네 개입니다.

인자기본값뜻
port_name/dev/agvpro_controller하위 제어기 직렬 장치. 로봇 내장 컴퓨터에 udev 로 붙여 둔 이름
namespace''노드 · 상대 토픽 앞에 붙는 이름 (4교시 다중 로봇)
enable_lidartrue라이다 드라이버를 켤지
lidar_typen10pn10p (2D, Lslidar) · mid360 (Livox 3D) · l2 (Unitree 3D) 중 하나
bash
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py --show-args       # 인자 목록 확인
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py lidar_type:=mid360 # 3D 라이다 장착형

agv_pro_node 는 켜질 때 하위 제어기가 다시 시작하기를 3 초 기다린 뒤 전원 상태를 확인하고, 문제가 있으면 오류 코드를 냅니다. 코드에서 읽은 오류 코드는 2 = 비상정지가 풀리지 않음, 3 = "Battery voltage is below 19.5V.", 4 = CAN 초기화 오류, 5 = 모터 초기화 오류 입니다. 이상이 없으면 자동 보고(ch03 의 그 기능)를 켜고 50 Hz 루프를 시작합니다.

로봇과 PC 가 서로를 보려면 — ROS_DOMAIN_ID#

ROS 2 노드들은 같은 네트워크에서 서로를 자동으로 찾습니다(discovery). 이때 같은 ROS_DOMAIN_ID (0 ~ 101 권장, 기본 0) 끼리만 보입니다. 로봇에서 bringup 을 켜고 PC 에서 RViz · teleop 을 쓰려면 세 가지가 맞아야 합니다.

  1. 로봇과 PC 가 같은 네트워크 (같은 공유기 · 멀티캐스트가 통함)
  2. 두 쪽의 ROS_DOMAIN_ID 가 같음 (export ROS_DOMAIN_ID=1 처럼 ~/.bashrc 에)
  3. 어느 쪽도 ROS_LOCALHOST_ONLY=1 이 아님 (이 값이 1 이면 자기 컴퓨터 안에서만 통신)

공식 문서는 같은 교실에서 로봇 두 대를 따로 쓸 때 ROS_DOMAIN_ID=1 · =2 처럼 나누거나, 혼자 쓸 때 ROS_LOCALHOST_ONLY=1 로 막는 방법을 안내합니다.

로봇에 원격으로 들어가기 (공식 문서 4.2.3)#

비주얼 내비게이션형은 스스로 Wi-Fi 핫스팟을 엽니다. 모니터 없이 작업할 때의 기본 경로입니다.

항목기본값 (공식 문서)
핫스팟 이름 · 비밀번호ElephantRobotics_AGVPro_AP_XXXX · Elephant
VNC (원격 화면)10.42.0.1:5900, 비밀번호 Elephant
시스템Jetson Orin Nano, Ubuntu 22.04 aarch64, ROS 2 Humble
기본 비밀번호는 첫날 바꿉니다

같은 실습실의 누구나 같은 기본값으로 다른 팀 로봇의 화면 · 토픽에 들어올 수 있습니다. 로그인 비밀번호는 passwd, VNC 비밀번호는 x11vnc -storepasswd, 관리자 비밀번호는 sudo passwd 로 바꾸고 팀 안전 계획서(ch01)에 보관 방법을 적습니다.

Gazebo — PC 에서 같은 로봇 띄우기#

실물이 없거나, 위험한 설정을 먼저 시험할 때는 PC 의 Gazebo 시뮬레이터에 같은 URDF 로봇을 띄웁니다. 그러면 bringup 대신 Gazebo 가 /cmd_vel 을 받고 odom · /scan 을 냅니다. 같은 Nav2 설정을 use_sim_time:=true 로 그대로 시험할 수 있습니다.

bash
# 공식 문서(6.2.5)에 적힌 명령
ros2 launch agv_pro_description agv_pro_gazebo.launch.py
# 저장소(humble)에서는 이 파일이 agv_pro_gazebo 패키지에 있습니다 → 위 명령이 안 되면
ros2 launch agv_pro_gazebo agv_pro_gazebo.launch.py
# 다른 터미널에서 키보드 조작 (기본 속도 0.25 m/s, 회전 0.5 rad/s)
ros2 run teleop_twist_keyboard teleop_twist_keyboard
문서와 저장소가 다릅니다

저장소의 agv_pro_description/launch/ 에는 agv_pro_display.launch.py 하나만 있고, agv_pro_gazebo.launch.py 는 agv_pro_gazebo/launch/ 에 있습니다. 문서의 명령으로 file 'agv_pro_gazebo.launch.py' was not found in the share directory of package 'agv_pro_description' 류의 오류가 나면 패키지 이름을 agv_pro_gazebo 로 바꿉니다(확인 필요 — 설치된 버전에 따라 다를 수 있음). 이렇게 **오류 문구에 적힌 패키지 · 파일 이름을 ros2 pkg prefix · ls 로 직접 확인** 하는 습관이 가장 빠른 해결책입니다.

이 사이트의 시뮬레이터와 Gazebo

agv_pro_gazebo.launch.py 는 agv_pro_gazebo/urdf/agv_pro.xacro 를 xacro 로 펼쳐 Gazebo 에 넣습니다. 이 사이트의 3D 뷰어와 물류창고 시뮬레이터도 같은 xacro 를 펼친 URDF 를 씁니다. Gazebo 가 없는 PC 에서는 시뮬레이터로 먼저 감을 잡으세요.

teleop_twist_keyboard (Gazebo · 실물)이 사이트 시뮬레이터뜻
i · ,W · S전진 · 후진
J · L (Shift)A · D왼쪽 · 오른쪽 옆 이동
j · lQ · E제자리 왼쪽 · 오른쪽 회전
kSpace정지
q/z · w/x · e/c패널의 속도 슬라이더전체 · 직진 · 회전 속도 10 % 씩

차이도 있습니다. teleop 은 키를 한 번 누르면 그 속도를 계속 보내고 k 로 멈추지만, 사이트 시뮬레이터는 키를 누르고 있는 동안만 움직입니다. 또 Gazebo 쪽은 gazebo_control_plugin.xacro 의 libgazebo_ros_planar_move 플러그인이 /cmd_vel 을 받아 차체를 평면에서 직접 움직이고 /odom 을 냅니다 — 메카넘 롤러 · 바퀴 마찰로 움직이는 것이 아니라서 바퀴 미끄러짐이 오도메트리에 나타나지 않습니다. 미끄러짐 · 엔코더 잡음은 사이트 시뮬레이터의 패널에서 켜서 봅니다 (ch04).

teleop 은 키를 누를 때 Twist 를 발행합니다. 기본 키 배치(i 전진, , 후진, j/l 회전, k 정지, Shift + 키 = 옆 이동 · 전방향 모드)를 쓰면 메카넘 로봇의 옆 이동도 시험할 수 있습니다(키 배치는 teleop_twist_keyboard 버전에 따라 다를 수 있음).

예측하기: 무엇부터 의심할까?#

아래 상황에서 원인을 먼저 짐작해 보세요. 시뮬레이터에서 같은 상황을 만들어 확인합니다.

  1. 로봇에서 bringup 은 잘 켜졌는데, PC 에서 ros2 topic list 를 하면 /parameter_events 와 /rosout 만 보입니다.
  2. 새 터미널을 열고 bringup 을 쳤더니 "Package 'agv_pro_bringup' not found" 가 나옵니다. 어제는 됐습니다.
  3. 충전 케이블을 오래 꽂아 두지 않은 로봇이 켜지자마자 오류 코드를 내고 멈춥니다.

시뮬레이터 2 · bringup 진단 콘솔#

왼쪽의 조건을 바꾸고 ▶ ros2 launch 를 누르면, 로봇 터미널의 로그(왼쪽 아래)와 PC 에서 본 ros2 topic list (오른쪽 아래)가 나옵니다. 위 그림은 노드의 상태(초록 = 동작, 빨강 = 죽음, 회색 = 시작 안 함)와 로봇 · PC 사이의 discovery 연결입니다. 로그는 원리를 보여 주려고 재구성한 예시 이며, 굵게 표시한 문구(Package ... not found, Battery voltage is below 19.5V.)만 원문 그대로입니다.

시뮬레이터 2 · bringup 진단 콘솔 (로봇 = myAGV Pro 내장 컴퓨터, PC = 내 노트북)
· 상황 불러오기:
로봇 터미널
PC 터미널 $ ros2 topic list

해 볼 것

  1. 정상 을 불러와 ▶ 실행합니다. 노드 네 개가 초록이 되고, PC 의 ros2 topic list 에 /cmd_vel · /odom · /scan · /tf 가 보이는지 확인합니다.
  2. 상황 1 ~ 5 를 하나씩 불러와 실행하기 전에 원인을 먼저 짐작하고, 실행 결과의 "원인 후보" 와 비교합니다. 어떤 상황은 로봇 쪽 로그가 완전히 정상인데도 문제가 있습니다. 어느 것인가요?
  3. 배터리 전압을 19.4 → 19.5 → 19.6 V 로 바꿔 가며 실행합니다. 경계값은 몇 V 이고, 경계 바로 위에서도 왜 안심하면 안 되나요? (ch03 의 "부하에 따른 전압 처짐" 을 떠올리세요.)
  4. 상황 5 에서 lidar_type 만 장착된 라이다에 맞게 바꾸면 해결되나요? /scan 이 목록에 있다 는 것과 데이터가 온다 는 것은 왜 다른가요? (ros2 topic hz /scan 으로 구분합니다.)
  5. 정상 상태에서 "같은 공유기" 를 끄거나 로봇에 ROS_LOCALHOST_ONLY=1 을 켭니다. 로봇 로그는 그대로인데 PC 쪽만 바뀌는 것을 확인합니다.

관찰 포인트 — bringup 문제는 어느 단계에서 멈췄는지 로 나눕니다. ① ros2 가 패키지를 못 찾으면 노드는 하나도 시작하지 않습니다(작업 공간 · source). ② 노드는 시작했는데 바로 죽으면 그 노드의 하드웨어 조건(직렬 포트 · 권한 · 비상정지 · 전압)입니다. ③ 노드는 살아 있는데 데이터가 없으면 설정 불일치(lidar_type)입니다. ④ 로봇에서는 모두 정상인데 PC 에서만 안 보이면 네트워크 · ROS_DOMAIN_ID · ROS_LOCALHOST_ONLY 입니다. 로그의 첫 번째 오류 줄 이 가장 중요한 단서이고, 뒤따르는 "process has died" 는 결과일 뿐입니다.

확인 문제 2-1

로봇에서 bringup 로그는 오류 없이 끝났고, 로봇 터미널에서 ros2 topic echo /odom 도 잘 나옵니다. 그런데 PC 에서 ros2 topic list 를 하면 /parameter_events 와 /rosout 만 보입니다. 가장 먼저 확인할 것은?

  1. 로봇에서 colcon build 를 다시 한다
  2. 로봇과 PC 의 ROS_DOMAIN_ID · ROS_LOCALHOST_ONLY 와 같은 네트워크인지
  3. lidar_type 인자
  4. 배터리 전압
정답 보기② — 로봇 안에서는 토픽이 잘 흐르므로 빌드 · 드라이버 · 라이다 · 전원 문제가 아닙니다(①, ③, ④). PC 에서만 안 보이는 것은 discovery 문제이며, 시뮬레이터 상황 1 처럼 DOMAIN_ID 가 다르거나, 한쪽이 LOCALHOST_ONLY 이거나, 다른 네트워크(또는 멀티캐스트가 막힌 공유기)일 때 생깁니다. `/parameter_events` · `/rosout` 은 PC 자신의 ros2 데몬이 만드는 것이라 항상 보입니다.
확인 문제 2-2

충전기를 뺀 채 오래 보관한 로봇에서 bringup 을 실행하니 Battery voltage is below 19.5V. 가 나오고 agv_pro_node 가 죽었습니다. 충전기를 꽂고 5 분 뒤 다시 실행했더니 이번에는 켜졌지만, 로봇을 출발시키자 몇 초 만에 같은 오류로 멈췄습니다. 가장 알맞은 설명은?

  1. 드라이버가 전압을 잘못 읽는 버그다
  2. 충전기를 꽂으면 단자 전압이 잠깐 올라가 시작 검사를 통과하지만, 실제 잔량은 거의 없어 출발 순간 부하로 전압이 다시 19.5 V 아래로 처진다
  3. 비상정지 버튼이 눌려 있다
  4. ROS_DOMAIN_ID 가 바뀌었다
정답 보기② — ch03 에서 본 V = V휴지 − I · R내부 입니다. 거의 빈 배터리는 휴지 전압 자체가 낮고, 출발할 때 전류가 커지면 더 처집니다. 시작 검사는 한 번 보는 것이라 경계 근처에서는 통과와 실패를 오갑니다. 충분히 충전한 뒤 쓰는 것이 해결이며, 버그(①)로 단정하기 전에 `ros2 topic echo /voltage` 로 정지 · 주행 중 전압을 비교합니다. 비상정지는 오류 코드 2 로 따로 나옵니다(③).

3교시 · Nav2 로 목표 지점 보내기#

ch06 에서 만든 지도, ch07 의 AMCL, ch08 의 코스트맵 · 계획기 · 컨트롤러를 한 번에 켜는 파일이 agv_pro_navigation2/launch/navigation2_active.launch.py 입니다. 공식 문서 6.2.3 의 순서는 이렇습니다.

bash
# 터미널 1 - 로봇 기동
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
# 터미널 2 - Nav2 + RViz
ros2 launch agv_pro_navigation2 navigation2_active.launch.py
# 다른 지도를 쓸 때
ros2 launch agv_pro_navigation2 navigation2_active.launch.py map:=$HOME/agv_pro_ros2/src/agv_pro_navigation2/map/map_test.yaml
navigation2_active.launch.py use_sim_time:=false · use_rviz:=true map/map.yaml (+ map.pgm) map:= 로 바꿀 수 있음 param/agvpro.yaml params_file:= 로 바꿀 수 있음 nav2_bringup / bringup_launch.py + rviz2 (agvpro_navigation2.rviz) map_server amcl (ch07) bt_navigator planner · controller waypoint_follower bt_navigator 의 액션: navigate_to_pose · navigate_through_poses / waypoint_follower 의 액션: follow_waypoints 내 스크립트 (nav2_simple_commander 의 BasicNavigator) goToPose() · goThroughPoses() · followWaypoints() → getFeedback() → getResult()
navigation2_active.launch.py 는 지도(map.yaml)와 설정(agvpro.yaml)을 nav2_bringup 의 bringup_launch.py 에 넘기고 RViz 를 켭니다. 내 스크립트는 bt_navigator · waypoint_follower 가 여는 액션에 목표를 보냅니다.
launch 인자기본값연결되는 것
mapagv_pro_navigation2/map/map.yamlmap_server 가 읽는 지도 (ch06 의 .pgm + .yaml)
params_fileagv_pro_navigation2/param/agvpro.yamlAMCL · 코스트맵 · 계획기 · 컨트롤러 · 웨이포인트 설정 전부 (ch07 · ch08)
use_sim_timefalseGazebo 에서 쓸 때만 true
use_rviztrueRViz 를 rviz/agvpro_navigation2.rviz 로 켬

RViz 가 열리면 **먼저 2D Pose Estimate 로 초기 위치를 줍니다.** AMCL 이 위치를 모르면 map → odom TF 가 없어서 어떤 목표도 시작할 수 없습니다(ch07). 그다음 Nav2 Goal 로 목표 하나를 주거나, Waypoint 모드로 여러 점을 찍습니다.

매개변수는 "지금 돌고 있는 값" 으로 확인

agvpro.yaml 은 저장소 · 문서 · 로봇마다 다를 수 있습니다(이론 앞의 안내 참고). 실제로 쓰이는 값은 실행 중에 물어봅니다.

bash
ros2 param get /controller_server FollowPath.max_vel_x
ros2 param get /global_costmap/global_costmap inflation_layer.inflation_radius
ros2 param get /waypoint_follower wait_at_waypoint.waypoint_pause_duration

목표를 보내는 세 가지 액션#

Nav2 는 목표 보내기를 액션 으로 엽니다. 서비스와 달리 진행 중에 피드백이 오고, 중간에 취소할 수 있습니다.

액션 (형)무엇을 하나피드백 (주요 필드)BasicNavigator 메서드
navigate_to_pose (NavigateToPose)목표 하나 로 감current_pose · navigation_time · estimated_time_remaining · number_of_recoveries · distance_remaininggoToPose(pose)
navigate_through_poses (NavigateThroughPoses)여러 점을 멈추지 않고 지나 마지막에 섬. 방향은 마지막 점만 맞춤위 필드 + number_of_poses_remaininggoThroughPoses(poses)
follow_waypoints (FollowWaypoints)점마다 NavigateToPose 를 차례로 보내고, 도착할 때마다 작업(기본: 잠깐 대기)을 함current_waypoint (몇 번째로 가는 중인지)followWaypoints(poses)

FollowWaypoints 의 결과에는 missed_waypoints (실패한 웨이포인트 번호 목록)가 들어 있습니다. 점마다 서는 대기 시간은 waypoint_follower 의 wait_at_waypoint 플러그인 매개변수 waypoint_pause_duration (ms) 로 정합니다. 공식 문서 6.2.3 에 보이는 설정 예는 200 입니다.

저장소 예제 읽기 — example_nav_to_pose.py#

agv_pro_navigation2/scripts/ 에는 세 액션에 하나씩 예제가 있습니다(example_nav_to_pose.py · example_nav_through_poses.py · example_waypoint_follower.py). 셋 다 같은 뼈대입니다. 아래는 example_nav_to_pose.py 의 구조를 줄여 옮긴 것입니다(목표값은 저장소 원문 그대로).

python실물 전용
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

def make_pose(nav, x, y, oz, ow):
    p = PoseStamped()
    p.header.frame_id = 'map'                    # 지도 좌표계 기준
    p.header.stamp = nav.get_clock().now().to_msg()
    p.pose.position.x, p.pose.position.y = x, y
    p.pose.orientation.z, p.pose.orientation.w = oz, ow   # yaw 만 있는 쿼터니언
    return p

rclpy.init()
nav = BasicNavigator()
# 예제에서는 아래 두 줄이 주석 처리되어 있음 - RViz 의 2D Pose Estimate 로 초기 위치를 준다는 뜻
# nav.setInitialPose(make_pose(nav, -1.92, -0.54, 0.0, 1.0))
# nav.waitUntilNav2Active()

goal_A = [1.6766083240509033, 0.37930558800697327, -0.03491306994337919, 0.9993903529387947]
goal_B = [-0.5062443017959595, 1.559376835823059, 0.6869307039904945, 0.7267229237578264]
for g in (goal_A, goal_B):
    nav.goToPose(make_pose(nav, *g))
    while not nav.isTaskComplete():
        fb = nav.getFeedback()                    # NavigateToPose.Feedback
        if fb:
            print(f"remain {fb.distance_remaining:.2f} m, recoveries {fb.number_of_recoveries}")
    result = nav.getResult()
    if result == TaskResult.SUCCEEDED:
        print('Goal succeeded!')
    elif result == TaskResult.CANCELED:
        print('Goal was canceled!')
    elif result == TaskResult.FAILED:
        print('Goal failed!')

읽을 점:

  • 목표는 PoseStamped 하나이고 **frame_id = 'map'** 입니다. 지도의 원점(map.yaml 의 origin)이 기준이라, 지도를 새로 만들면 좌표가 모두 바뀝니다.
  • 방향은 각도가 아니라 **쿼터니언 (z, w)** 로 줍니다. goal_A 의 (−0.0349, 0.9994) 는 몇 도일까요? 바로 아래에서 계산합니다.
  • isTaskComplete() 를 돌며 getFeedback() 을 읽고, 끝나면 getResult() 로 성공 · 취소 · 실패를 구분합니다. 액션이라서 가능한 구조입니다.
  • example_waypoint_follower.py 는 600 초가 지나면 cancelTask() 로 취소하고, 첫 묶음(way1_goals)이 성공해야 둘째 묶음을 보냅니다. 같은 파일의 피드백 출력 조건 if (feedback and i % 5) and verbose == 0: 은 이름과 반대로 verbose=False 일 때 출력하는 점도 눈여겨보세요 — 남의 예제는 "읽고 고쳐 쓰는" 것입니다.

yaw θ 와 쿼터니언#

바닥을 다니는 로봇의 방향은 z 축 회전(yaw) θ 하나뿐입니다. 이때 쿼터니언은 x = y = 0 이고

  • z = sin(θ / 2), w = cos(θ / 2)
  • 거꾸로 θ = 2 · atan2(z, w)

입니다. 각도의 절반 이 들어가는 것이 핵심입니다. 그래서

θzw뜻
0°01지도 +x 방향
90°0.70710.7071+y 방향
180°10−x 방향 ((−1, 0) 으로 써도 같음)
−90°−0.70710.7071−y 방향

(z, w) 와 (−z, −w) 는 같은 방향 입니다(쿼터니언의 이중 표현). 180° 를 (1, 0) 으로 쓰든 (−1, 0) 으로 쓰든 로봇은 같은 쪽을 봅니다. 또 z² + w² = 1 이어야 하며, 크기가 1 이 아니면 Nav2 가 목표를 거절하거나 TF 에서 경고가 납니다.

예측하기: 세 액션의 시간 차이#

입고장 → 선반 A → 선반 C → 출고장 네 점을 같은 속도로 돕니다.

  1. NavigateThroughPoses 와 FollowWaypoints 중 어느 쪽이 더 빨리 끝날까요? 차이는 어디서 생길까요?
  2. 네 점을 NavigateToPose 하나로 보내면(Nav2 Goal 을 네 번 연달아 클릭하면) 어떻게 될까요?
  3. 지도에 없는 팔레트가 통로를 막고 있으면 피드백의 어떤 숫자가 바뀔까요?

시뮬레이터 3 · 창고 웨이포인트 모드#

10 × 6 m 창고입니다. 선반 A · B · C 가 가로로 놓여 있고, 왼쪽에 입고장, 오른쪽에 출고장이 있습니다. 지도를 클릭하면 웨이포인트가 추가 되고, 클릭한 채 끌면 그 방향이 목표 yaw 가 됩니다(끌지 않으면 앞 점에서 오는 방향). 로봇(0.52 × 0.36 m)은 왼쪽 아래 대기 위치에서 출발하며, agvpro.yaml 처럼 옆으로는 가지 않고(max_vel_y: 0) 회전은 0.5 rad/s 이하입니다. 계획은 팽창 반경 0.25 m 의 격자 A* (셀 0.1 m), 빨간 점선 상자는 지도에 없는 팔레트로, 라이다(점선 원 1.5 m)에 보여야 알게 됩니다. 막히면 2 s 기다리는 복구 동작 후 다시 계획합니다(교육용 단순화).

시뮬레이터 3 · Nav2 웨이포인트 모드 (클릭 = 점 추가, 끌기 = 방향 / 파랑 = 계획 경로, 초록 = 지나온 길, 보라 = 팽창)
액션 · 지도에 없는 팔레트 · 배속
·

해 볼 것

  1. 프리셋 순회 를 누르고 표의 θ · z · w 를 읽습니다. 입고장(180°)의 (z, w) 는 왜 (1, 0) 인가요? 선반 A(−90°)는? 그다음 FollowWaypoints 로 ▶ 를 누르고 끝까지 계산 해 navigation_time 을 적습니다.
  2. 같은 점으로 액션만 NavigateThroughPoses 로 바꿔 다시 실행합니다. 시간이 얼마나 줄었나요? 선반 A · C 에서 로봇이 서서 돌았나요? 그 차이가 이 두 액션의 차이입니다. 이번에는 NavigateToPose 로 바꿔 보세요. 로봇은 어디로 가나요?
  3. FollowWaypoints 로 두고 waypoint_pause_duration 을 200 → 3000 ms 로 바꿉니다. 시간은 정확히 얼마 늘어나나요? (점 4 개 × 2.8 s 와 비교)
  4. 팔레트를 왼쪽 통로 에 두고 FollowWaypoints 를 실행합니다. 어느 구간에서 복구가 일어나나요? 이어 오른쪽 통로 로 바꿔 실행합니다. 복구 횟수 · 돌아간 거리를 비교하고, 같은 팔레트가 경로에 따라 "문제" 가 되기도 하고 아니기도 한 이유를 설명합니다.
  5. yaw_goal_tolerance 를 0.6 rad 으로 키우면 도착 로그의 "yaw 오차" 는 어떻게 되나요? 선반 앞에서 정확한 방향이 필요할 때 이 값은 어떻게 정해야 할까요? 마지막으로 선반 위(검은 칸)를 클릭해 거절되는 것도 확인합니다.

관찰 포인트 — 세 액션은 "점마다 서는가 · 방향을 맞추는가" 로 갈립니다. FollowWaypoints 는 점마다 NavigateToPose 를 보내므로 매번 감속 · 정지 · 방향 맞춤 · 대기를 하고, 피드백은 "몇 번째 점인가(current_waypoint)" 를 줍니다. NavigateThroughPoses 는 한 경로로 이어 달리고 마지막 방향만 맞추므로 빠르지만, 선반 앞에서 "멈춰서 일하기" 가 필요하면 맞지 않습니다. NavigateToPose 는 목표가 하나뿐이라 여러 점을 보내면 마지막 것만 남습니다(RViz 에서 Nav2 Goal 을 연달아 누르면 앞 목표가 취소되는 것과 같음). 지도에 없는 장애물은 경로 위에 있을 때만 복구 · 재계획을 부르고, 그 흔적은 number_of_recoveries 와 늘어난 navigation_time 으로 남습니다. 방향은 yaw_goal_tolerance 안에 들어오는 순간 "도착" 이므로, 허용 오차가 크면 그만큼 비뚤게 섭니다.

확인 문제 3-1

프리셋 네 점을 같은 속도(0.26 m/s)로 돌렸더니 FollowWaypoints 가 NavigateThroughPoses 보다 눈에 띄게 오래 걸렸습니다. 차이의 원인으로 가장 알맞은 것은?

  1. FollowWaypoints 가 더 긴 경로를 계획하기 때문
  2. FollowWaypoints 는 점마다 멈추고, 그 점의 목표 방향으로 제자리 회전한 뒤 waypoint_pause_duration 만큼 기다리기 때문
  3. NavigateThroughPoses 가 max_vel_x 를 무시하고 더 빨리 달리기 때문
  4. FollowWaypoints 는 점마다 AMCL 초기 위치를 다시 잡기 때문
정답 보기② — 두 액션은 같은 계획기로 같은 점을 지나므로 경로 길이는 거의 같습니다(①). FollowWaypoints 는 점마다 NavigateToPose 를 새로 보내므로 감속 → 정지 → 목표 yaw 로 회전(최대 0.5 rad/s, 180° 면 6 s 이상) → 대기를 반복합니다. NavigateThroughPoses 는 중간 점의 방향을 맞추지 않습니다. 속도 한계는 둘 다 같고(③), 초기 위치는 처음 한 번만 줍니다(④). 시뮬레이터에서는 pause 를 0 으로 해도 회전 시간 때문에 차이가 남습니다.
확인 문제 3-2

목표 방향을 θ = 120° 로 주려고 orientation.z = sin(120°) = 0.866, orientation.w = cos(120°) = −0.5 로 적었습니다. 로봇은 어느 쪽을 보게 될까요?

  1. 120° — z² + w² = 1 이므로 올바른 쿼터니언이다
  2. 240° (= −120°) — 각도의 절반을 써야 하는데 전체 각도를 넣었다
  3. 60° — w 의 부호만 틀렸다
  4. Nav2 가 거절한다 — w 는 음수가 될 수 없다
정답 보기② — yaw 쿼터니언은 (z, w) = (sin θ/2, cos θ/2) 입니다. (0.866, −0.5) 를 거꾸로 풀면 θ = 2 · atan2(0.866, −0.5) = 2 × 120° = 240°, 즉 −120° 방향입니다. 크기가 1 이라서 오류 없이 받아들여지므로(①, ④) 더 위험한 실수입니다. 120° 의 올바른 값은 (sin 60°, cos 60°) = (0.866, 0.5) 입니다. w 는 음수여도 되며, (z, w) 와 (−z, −w) 는 같은 방향입니다.

4교시 · 자동 충전과 다중 로봇#

agv_pro_autocharge 의 흐름#

자동 충전 모듈이 달린 myAGV Pro 는 agv_pro_autocharge 패키지의 combined_auto_recharger 노드로 충전대에 스스로 붙습니다. 공식 문서 6.2.8 은 터미널 다섯 개를 차례로 엽니다(3D 라이다 L2 기준).

bash
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py                         # 1) 로봇 기동
ros2 launch unitree_lidar_ros2 launch.py                                      # 2) 3D 라이다 (문서의 구성)
ros2 launch pointcloud_to_laserscan pointcloud_to_laserscan.launch.py         # 3) 3D 점구름 → 2D /scan
ros2 launch agv_pro_navigation2 navigation2_active.launch.py                  # 4) Nav2
ros2 run agv_pro_autocharge combined_auto_recharger                           # 5) 자동 충전 노드
combined_auto_recharger 의 흐름 (agv_pro_autocharge) ① 충전대 위치 charger_position.json p_x · p_y · orien_z · orien_w ② 접근 목표 계산 앞쪽 forward_distance (1 m) yaw − yaw_offset_deg (10°) ③ Nav2 로 접근 goThroughPoses([목표]) AMCL 위치 기준 (오차 수 cm) ④ 정밀 도킹 /dev/agvpro_ec130 · 9600 적외선 정렬 → /cmd_vel ④ 에서 도킹 모듈이 보내는 mode 바이트에 따라 0x01 정상모듈이 준 속도를 /cmd_vel 로 0xBB 압력 구역정지 (접촉 중) 0xAA 충전 구역정지 후 종료 = 충전 시작 0xCF 비상정지 · 장애물정지 후 종료 "언제 충전하러 갈지" 는 이 노드가 정하지 않습니다 (배터리 임계값 없음). → 미션 관리자가 voltage 토픽을 보고 판단해야 함 (4교시 시뮬레이터)
충전은 두 단계입니다. 지도 위의 충전대 위치를 기준으로 1 m 앞까지는 Nav2 로 가고(위치 오차 수 cm), 마지막 1 m 는 도킹 모듈이 적외선으로 정렬하며 보내는 속도로 천천히 붙습니다.

코드에서 읽은 단계별 동작입니다.

  1. 충전대 위치 저장 — config/charger_position.json 에 p_x · p_y · orien_z · orien_w 가 들어 있습니다. RViz 의 2D Goal Pose 도구의 토픽을 /charger_position_update 로 바꾸고 충전대 위치를 클릭하면 이 파일에 저장되며, /goal_marker 로 화살표와 "Charger" 글자가 표시됩니다.
  2. 접근 목표 계산 — config/nav_goal_params.yaml 의 forward_distance: 1 (충전대 1 m 앞) 과 yaw_offset_deg: 10.0 (시계 방향 10° 보정)으로 목표를 만듭니다: yaw = 2 · atan2(orien_z, orien_w), 목표 = (p_x + 1 · cos yaw, p_y + 1 · sin yaw), 목표 yaw = yaw − 10°.
  3. Nav2 접근 — 터미널에서 q 를 누르면 goThroughPoses([목표]) 로 접근을 시작합니다.
  4. 정밀 도킹 — 도착하면 도킹 모듈(/dev/agvpro_ec130, 9600 bps)에 AT+CG · AT+AT 를 보내고, 모듈이 보내는 프레임(CAN ID 0x182)에서 속도 · mode 바이트 · 적외선 비트를 읽습니다. mode 가 0x01 이면 받은 속도를 /cmd_vel 로 내보내고, 0xBB (압력 구역) 이면 정지, 0xAA (충전 구역) 이면 정지 후 종료, 0xCF (비상정지) 또는 적외선 장애물 비트가 켜지면 정지 후 종료합니다.
자동 충전 README 는 오래되었습니다

패키지 안 README 는 패키지 이름을 agv_autocharge_ros2, 포트를 /dev/ttyCH341USB0 으로 적고 있지만, 실제 코드는 agv_pro_autocharge · /dev/agvpro_ec130 입니다. 또 설정 파일 경로가 /home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config 로 코드에 박혀 있어, 사용자 이름이나 작업 공간 위치가 다르면 파일을 찾지 못하고 기본값(0, 0, 0, 1)을 씁니다(자주 나는 오류 7). 명령은 이 차시처럼 코드 기준 으로 씁니다.

"언제" 충전하러 갈지는 누가 정하나요?#

combined_auto_recharger 에는 배터리 임계값이 없습니다. 사람이 q 를 눌러야 출발합니다. 실제 운용에서는 미션 관리자(ch10 에서 만들 프로그램)가 voltage 토픽을 보고 "지금 충전하러 가야 하는가" 를 판단해야 합니다. 판단의 기준은 간단한 부등식입니다.

  • 충전 시작 임계값 thr ≥ 보호 하한 + (미션 한 번에 드는 양 × 최악의 변동) + (충전대까지 가는 데 드는 양)

미션을 "끝낸 뒤" 에 확인한다면, 임계값 바로 위에서 시작한 미션이 끝날 때까지 버텨야 하기 때문입니다. myAGV Pro 의 배터리는 24 V 20 Ah(480 Wh, ch01)이고, 컴퓨터 · 센서 같은 기본 부하가 커서 소모는 거리보다 시간에 비례 하는 편입니다(ch01 의 P = 45 W + 1.05 · m · v 에서 45 W 가 대부분 → 약 10 %/h). 리튬인산철 배터리는 전압 곡선이 평평해서 전압만으로 잔량(%)을 정확히 읽기 어렵다는 점도 기억합니다(ch03).

예측하기: 15 % 면 충분할까?#

시뮬레이터 4 의 처음 값은 소모 10 %/h, 미션 1 200 m (0.4 m/s 로 50 분 + 상하차 2 분), 변동 ±30 %, 충전대까지 40 m, 보호 하한 10 % 입니다.

  1. 임계값 15 % 로 "미션 끝에서 확인" 하면 하루(24 h) 동안 방전 정지가 생길까요?
  2. 방전되지 않으려면 임계값이 최소 몇 % 여야 할까요? 위 부등식으로 먼저 계산해 보세요.
  3. 임계값을 아주 높게(60 %) 두면 무엇이 달라질까요?

시뮬레이터 4 · 충전 시작 임계값 찾기#

위 그래프는 24 시간 동안의 배터리 잔량(%)입니다. 띠 색은 그때 로봇이 하던 일(파랑 = 미션, 주황 = 충전대로 이동 · 도킹, 초록 = 충전, 빨강 = 방전 정지 후 사람이 옮기는 30 분)입니다. 충전은 90 % 까지 하고 다시 미션으로 돌아갑니다. 미션마다 소모는 변동 범위 안에서 무작위로 달라집니다.

시뮬레이터 4 · 배터리 · 미션 · 충전 임계값 (24 h, 주행 0.4 m/s, 보호 하한 10 %, 90 % 까지 충전)
판단 방식 ·

해 볼 것

  1. 예측 확인 — 처음 값(임계값 15 %)에서 방전 정지가 몇 번, 언제 생기나요? 출력 맨 아래의 "부등식으로 계산한 최소 임계값" 과 공책의 계산을 비교합니다.
  2. 임계값을 1 % 씩 올리며 방전 정지가 사라지는 첫 값을 찾습니다. "변동 다시 뽑기" 를 여러 번 눌러도 안전한가요? 계산값과 비교해, 그보다 낮은 값에서 "운 좋게" 안전한 경우가 있는지 봅니다.
  3. 미션 길이를 600 m · 2 400 m 로 바꿔 최소 임계값이 어떻게 변하는지 표로 적습니다. 미션 길이와 임계값은 어떤 관계인가요?
  4. 충전대까지 거리를 40 → 300 m 로 늘립니다. 임계값은 얼마나 더 필요해지나요? 미션 길이를 바꿀 때와 비교하면 영향이 큰가요, 작은가요?
  5. 임계값을 60 % 로 올려 충전하러 간 횟수 · 미션 횟수를 22 % 일 때와 비교합니다. 이어 판단 방식을 미션 시작 전 예측 으로 바꿉니다. 임계값 슬라이더와 상관없이 방전이 없나요?

관찰 포인트 — "미션이 끝날 때 확인" 하는 방식에서는 임계값 바로 위(예: 15.1 %)에서 시작한 미션도 끝까지 버텨야 하므로, 임계값은 하한 + 최악의 미션 1 회 + 충전대까지 이상이어야 합니다. 미션이 길수록, 변동이 클수록 임계값이 크게 올라가고, 충전대까지 거리는 0.4 m/s 로 몇 분이면 가므로 영향이 작습니다. 임계값을 너무 높이면 방전은 없지만 아직 쓸 수 있는 배터리로 더 자주 충전하러 갑니다. 이 모델에서는 하루 중 충전 시간이 대부분이라 미션 수 차이는 크지 않지만, 충전대 왕복 · 도킹 횟수가 늘어 충전대가 하나뿐인 현장에서는 여러 대가 충전대를 두고 기다리는 문제가 생깁니다. 예측 방식은 매 미션 전에 "이번 미션 + 충전대 이동" 을 계산하므로 고정 임계값보다 늦게, 그러나 안전하게 충전하러 갑니다. 이 판단을 내리는 쪽이 combined_auto_recharger 가 아니라 미션 관리자 라는 것이 핵심입니다.

확인 문제 4-1

시뮬레이터 4 의 처음 값(소모 10 %/h, 미션 1 200 m, 변동 ±30 %, 충전대 40 m)에서 "미션 끝에서 확인" 방식으로 방전 정지가 없으려면 임계값이 대략 얼마 이상이어야 할까요?

  1. 약 11 % — 하한 10 % 에 충전대까지 가는 양만 더하면 된다
  2. 약 21 % — 하한 10 % + 최악의 미션 1 회(약 9 %) + 충전대까지(약 0.4 %)
  3. 약 50 % — 배터리는 절반 아래로 쓰면 안 된다
  4. 임계값과 무관하다 — 변동이 있으면 언제든 방전된다
정답 보기② — 미션 1 회는 1 200 m ÷ 0.4 m/s = 3 000 s 에 상하차 120 s 를 더해, 최악(+30 %)이면 3 900 + 120 = 4 020 s ≈ 1.12 h → 약 11.2 % 입니다. 여기에 충전대까지 (40 ÷ 0.4 + 60) s 의 약 0.44 % 를 더하면 10 + 11.2 + 0.4 ≈ 21.6 % 입니다(시뮬레이터 출력과 비교). ① 은 미션이 끝날 때까지 버티는 몫을 빠뜨렸고, ③ 은 근거가 없으며(가동률만 떨어짐), ④ 변동에도 상한이 있으므로 최악을 넣어 계산하면 막을 수 있습니다.

여러 대 운용 — ROS_DOMAIN_ID 와 namespace#

같은 공간에서 로봇 여러 대를 운용할 때 나누는 방법은 두 가지입니다.

방법무엇이 나뉘나장점단점
ROS_DOMAIN_ID 를 로봇마다 다르게통신 자체 (서로 안 보임)설정이 간단, 이름 충돌 없음PC 하나로 여러 대를 함께 보려면 PC 도 도메인을 바꿔 가며 써야 함
같은 도메인 + namespace 로 이름 나누기토픽 · 노드 이름 (/agv_pro1/odom)PC 한 대에서 모든 로봇을 동시에 봄 (관제)절대 이름 토픽은 나뉘지 않음, 라이다 · Nav2 설정도 namespace 에 맞춰야 함

공식 문서 6.2.4 의 다중 제어 절차는 두 번째 방법입니다. 모든 기기의 ~/.bashrc 에 export RMW_IMPLEMENTATION=rmw_fastrtps_cpp 와 같은 export ROS_DOMAIN_ID=1 을 넣고, 라이다 설정 lsn10p.yaml 에 namespace 항목을 더해 colcon build --packages-select lslidar_driver 로 다시 빌드한 뒤, 로봇마다 다른 namespace 로 bringup 합니다.

bash
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py namespace:=agv_pro1   # 1번 로봇
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py namespace:=agv_pro2   # 2번 로봇

라이다 설정은 파일 맨 위에 namespace 한 줄을 더하고 그 아래를 한 칸 들여쓰기 하는 것이 전부입니다 (공식 문서 6.2.4 — 들여쓰기가 틀리면 설정이 조용히 무시됩니다).

yaml
# ~/agv_pro_ros2/src/Lslidar_ROS2_driver/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml
agv_pro1:                        # 2번 로봇은 agv_pro2:
  lslidar_driver_node:
    ros__parameters:
      frame_id: laser_link
      lidar_name: N10_P
      scan_topic: /scan
      interface_selection: serial
      serial_port_: /dev/agvpro_lidar
      # ... 나머지 값은 원래 파일 그대로

PC 에는 sudo apt-get install ros-humble-teleop-twist-keyboard 로 키보드 노드를 설치하고, 움직이는 동안 멈출 때는 k 를 누릅니다.

그런데 1교시에서 본 것처럼 agv_pro_node 는 /cmd_vel 을 절대 이름으로 구독합니다. 그래서 문서도 "PC 에서 teleop 을 실행하면 두 로봇이 같은 /cmd_vel 을 받는다" 고 적고 있습니다. 시뮬레이터 5 에서 확인해 봅니다.

시뮬레이터 5 · 다중 로봇 토픽 격리#

PC 에서 teleop 으로 i (전진) 키를 2 초 누릅니다. 어느 로봇이 움직이는지, PC 의 ros2 topic list 에 무엇이 보이는지 확인합니다.

시뮬레이터 5 · 다중 로봇 격리 (PC 1 대 + myAGV Pro 2 대)
· · teleop 이 발행할 토픽 ·

해 볼 것

  1. 처음 값(모두 도메인 1, namespace 없음)에서 ▶ 를 누릅니다. 몇 대가 움직이나요? topic list 의 "발행자 2" 는 무엇을 뜻하나요?
  2. namespace 를 켜고 다시 ▶. odom 은 나뉘었는데 움직이는 로봇은 그대로입니다. 왜 그런가요? teleop 토픽을 /agv_pro1/cmd_vel 로 리매핑하면?
  3. 드라이버 수정 까지 켜고 teleop 토픽을 /agv_pro1/cmd_vel → /agv_pro2/cmd_vel 로 바꿔 봅니다. 이제 한 대씩 따로 움직이나요?
  4. 수정을 모두 끄고, 대신 로봇 2 의 도메인만 2 로 바꿉니다. 로봇 1 만 움직이나요? 이때 PC 에서 로봇 2 를 보려면 무엇을 해야 하나요?

관찰 포인트 — ROS_DOMAIN_ID 는 통신 자체를 나누므로 가장 확실하지만 PC 한 대로 여러 대를 함께 보기 어렵습니다. namespace 는 상대 이름으로 만든 토픽만 나눕니다. myAGV Pro 의 드라이버는 /cmd_vel 을 절대 이름으로 구독하므로 namespace 만으로는 속도 명령이 나뉘지 않고, 드라이버 코드(구독 이름 · 리매핑)까지 고쳐야 진짜로 따로 움직입니다. 여러 대를 관제하는 Nav2 구성(로봇마다 namespace 가 붙은 Nav2)은 이 문제를 먼저 풀어야 합니다.

확인 문제 4-2

두 로봇을 같은 ROS_DOMAIN_ID=1 로 두고 namespace:=agv_pro1 · namespace:=agv_pro2 로 bringup 했습니다. PC 에서 ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/agv_pro1/cmd_vel 로 1번만 움직이려 했는데 아무 로봇도 움직이지 않습니다. 이유는?

  1. 도메인이 같아서 메시지가 충돌했다
  2. 두 로봇의 드라이버는 namespace 와 상관없이 /cmd_vel 을 구독하므로, /agv_pro1/cmd_vel 에는 구독자가 없다
  3. teleop 은 리매핑을 지원하지 않는다
  4. namespace 를 주면 /cmd_vel 이 자동으로 /agv_pro1/cmd_vel 이 되므로 리매핑이 두 번 적용되었다
정답 보기② — 시뮬레이터 5 의 "namespace 켬 + 리매핑 /agv_pro1/cmd_vel + 드라이버 수정 끔" 과 같은 상황입니다. 드라이버 코드가 `/cmd_vel` (절대 이름)을 구독하고 bringup 도 `('cmd_vel', '/cmd_vel')` 로 리매핑하므로 namespace 가 붙지 않습니다(④ 오답). `ros2 topic info /agv_pro1/cmd_vel` 을 하면 Subscription count: 0 이 나옵니다. 리매핑 없이 `/cmd_vel` 로 보내면 두 대가 함께 움직입니다.

실습 (시뮬레이터)#

교시마다 Playground 에서 코드를 직접 돌립니다. starter 의 TODO 를 채워 실행한 뒤 풀이 와 비교하세요. 모든 코드는 표준 라이브러리 + numpy 만 쓰며, ROS 2 없이 메시지 · 서비스 · 액션의 "모양" 과 계산만 흉내 냅니다. starter 는 TODO 를 채우지 않아도 오류 없이 실행되지만 결과가 틀리게 나옵니다.

Playground 열기

실습 1-1 · Twist 가짜 발행기와 50 Hz 드라이버 (1교시)

geometry_msgs/msg/Twist 와 같은 모양의 dict 를 만들고, 정해진 주기 · 지연 · 손실로 "발행" 한 뒤, agv_pro_node 처럼 50 Hz 로 받아 움직이는 가짜 드라이버에 넣습니다. 시뮬레이터 1 과 같은 모델입니다.

  • TODO 1 make_twist() — linear.x · linear.y · angular.z 채우기
  • TODO 2 드라이버의 속도 자르기 (LIMIT, agv_pro_ros.cpp 의 값)
  • TODO 3 명령 타임아웃 — 마지막으로 받은 뒤 timeout 초가 지나면 목표 0
  • 합격 조건: 2 Hz · 타임아웃 0.2 s 에서 이동 거리가 다른 조합보다 뚜렷이 짧고(끊김 10 번 안팎), 타임아웃이 없을 때 발행이 끊겨도 계속 달리는 것이 출력에 보임

예측 먼저 — 표의 8 줄 중 이동 거리가 가장 짧은 줄은 어디일까요? 그 값은 1.65 m 의 몇 % 쯤일까요?

python시뮬 실행 가능
import json
import random

# agv_pro_node 가 자르는 범위 (agv_pro_ros.cpp)
LIMIT = {"x": 1.5, "y": 1.0, "z": 1.0}

def clamp(v, lim):
    return max(-lim, min(lim, v))

def make_twist(vx=0.0, vy=0.0, wz=0.0):
    """geometry_msgs/msg/Twist 와 같은 모양의 dict"""
    # TODO 1: linear.x = vx, linear.y = vy, angular.z = wz 를 넣으세요 (나머지는 0.0)
    return {
        "linear": {"x": 0.0, "y": 0.0, "z": 0.0},
        "angular": {"x": 0.0, "y": 0.0, "z": 0.0},
    }

def to_cli(msg):
    """ros2 topic pub 에 넣을 YAML 문자열"""
    l, a = msg["linear"], msg["angular"]
    return ('ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist '
            f'"{{linear: {{x: {l["x"]}, y: {l["y"]}, z: 0.0}}, angular: {{x: 0.0, y: 0.0, z: {a["z"]}}}}}"')

def fake_publish(rate_hz, latency, loss, t_end=6.0, t0=0.5, seed=1):
    """(도착 시각, msg) 목록. 손실된 메시지는 빠짐"""
    rnd = random.Random(seed)
    out, k = [], 0
    while True:
        t = t0 + k / rate_hz
        if t >= t_end:
            break
        msg = make_twist(0.3, 0.0, 0.0)
        if rnd.random() >= loss:
            out.append((t + latency, msg))
        k += 1
    return out

def fake_base(arrivals, timeout, t_total=10.0, dt=0.02, acc=1.0):
    """50 Hz 드라이버 흉내: 마지막으로 받은 명령, 타임아웃, 가속 제한, 속도 자르기"""
    target, last, v, x, i, stops, moving = 0.0, -1e9, 0.0, 0.0, 0, 0, False
    for n in range(int(round(t_total / dt)) + 1):
        t = n * dt
        while i < len(arrivals) and arrivals[i][0] <= t + 1e-9:
            target = arrivals[i][1]["linear"]["x"]   # TODO 2: LIMIT["x"] 로 자르기 (clamp)
            last = arrivals[i][0]
            i += 1
        tgt = target   # TODO 3: timeout 이 있고 t - last > timeout 이면 0.0
        v += max(-acc * dt, min(acc * dt, tgt - v))
        x += v * dt
        if 1.0 < t < 6.0:
            if v < 0.05 and moving:
                stops += 1
                moving = False
            if v > 0.1:
                moving = True
    return x, stops

print(json.dumps(make_twist(0.3, 0.1, -0.2)))
print(to_cli(make_twist(0.2)))
print()
print(" Hz  timeout   distance(m)  stops")
for hz in (2, 5, 10, 20):
    for to in (0.2, 0.5):
        d, s = fake_base(fake_publish(hz, 0.03, 0.0), to)
        print(f"{hz:3d}  {to:5.1f}     {d:8.2f}     {s:3d}")
d, s = fake_base(fake_publish(10, 0.03, 0.0), None)
print("no timeout, publisher killed at 6 s ->", round(d, 2), "m (still moving)")

해 볼 것

  1. 첫 줄의 JSON 과 ros2 topic pub 문자열을 확인합니다. 이 문자열을 실물 로봇 터미널에 그대로 붙이면 0.2 m/s 명령이 한 번 나갑니다(실물 실습 1 에서 사용).
  2. 표에서 끊김이 생기는 조합을 찾고, "명령 간격 1/f > 타임아웃" 조건과 맞는지 확인합니다.
  3. fake_publish(10, 0.03, 0.3) 처럼 손실 30 % 를 주고 타임아웃 0.2 s 와 0.1 s 를 비교합니다.
  4. make_twist(2.0) 을 보내면 드라이버는 몇 m/s 로 달리나요? 자르기가 없으면 무엇이 위험할까요?
풀이
python시뮬 실행 가능
import json
import random

# agv_pro_node 가 자르는 범위 (agv_pro_ros.cpp)
LIMIT = {"x": 1.5, "y": 1.0, "z": 1.0}

def clamp(v, lim):
    return max(-lim, min(lim, v))

def make_twist(vx=0.0, vy=0.0, wz=0.0):
    """geometry_msgs/msg/Twist 와 같은 모양의 dict"""
    return {
        "linear": {"x": float(vx), "y": float(vy), "z": 0.0},
        "angular": {"x": 0.0, "y": 0.0, "z": float(wz)},
    }

def to_cli(msg):
    """ros2 topic pub 에 넣을 YAML 문자열"""
    l, a = msg["linear"], msg["angular"]
    return ('ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist '
            f'"{{linear: {{x: {l["x"]}, y: {l["y"]}, z: 0.0}}, angular: {{x: 0.0, y: 0.0, z: {a["z"]}}}}}"')

def fake_publish(rate_hz, latency, loss, t_end=6.0, t0=0.5, seed=1):
    """(도착 시각, msg) 목록. 손실된 메시지는 빠짐"""
    rnd = random.Random(seed)
    out, k = [], 0
    while True:
        t = t0 + k / rate_hz
        if t >= t_end:
            break
        msg = make_twist(0.3, 0.0, 0.0)
        if rnd.random() >= loss:
            out.append((t + latency, msg))
        k += 1
    return out

def fake_base(arrivals, timeout, t_total=10.0, dt=0.02, acc=1.0):
    """50 Hz 드라이버 흉내: 마지막으로 받은 명령, 타임아웃, 가속 제한, 속도 자르기"""
    target, last, v, x, i, stops, moving = 0.0, -1e9, 0.0, 0.0, 0, 0, False
    for n in range(int(round(t_total / dt)) + 1):
        t = n * dt
        while i < len(arrivals) and arrivals[i][0] <= t + 1e-9:
            target = clamp(arrivals[i][1]["linear"]["x"], LIMIT["x"])
            last = arrivals[i][0]
            i += 1
        tgt = 0.0 if (timeout and t - last > timeout) else target
        v += max(-acc * dt, min(acc * dt, tgt - v))
        x += v * dt
        if 1.0 < t < 6.0:
            if v < 0.05 and moving:
                stops += 1
                moving = False
            if v > 0.1:
                moving = True
    return x, stops

print(json.dumps(make_twist(0.3, 0.1, -0.2)))
print(to_cli(make_twist(0.2)))
print()
print(" Hz  timeout   distance(m)  stops")
for hz in (2, 5, 10, 20):
    for to in (0.2, 0.5):
        d, s = fake_base(fake_publish(hz, 0.03, 0.0), to)
        print(f"{hz:3d}  {to:5.1f}     {d:8.2f}     {s:3d}")
d, s = fake_base(fake_publish(10, 0.03, 0.0), None)
print("no timeout, publisher killed at 6 s ->", round(d, 2), "m (still moving)")
  • 예시 결과: 2 Hz · 0.2 s 만 0.44 m(약 27 %), 끊김 10 번입니다. 나머지는 1.65 ~ 1.79 m 로 거의 같습니다(타임아웃 0.5 s 쪽이 조금 긴 것은 6 s 이후 0 명령 전까지 조금 더 달리기 때문). 타임아웃이 없으면 발행이 6 s 에 끊겨도 10 s 까지 2.8 m 를 갑니다.
  • make_twist(2.0) 은 LIMIT["x"] = 1.5 로 잘립니다. 다만 myAGV Pro 의 사양 최고 속도와 실제 펌웨어 한계는 별개이므로, 실물에서는 Nav2 · teleop 쪽에서 먼저 낮게 제한합니다.
실습 1-2 · ch03 API 호출을 ROS 2 서비스 호출로 옮기기 (1교시)

ch03 의 set_led_color(0, 255, (0, 255, 0)) · get_pin_input(1) · set_pin_output(2, 1) 을 agv_pro_msgs 서비스 호출 문자열로 바꾸고, 범위를 벗어난 값은 보내기 전에 막습니다.

  • TODO 1 SRV 표에 디지털 입력 · 출력 서비스의 요청 필드 이름을 채웁니다 (.srv 정의는 1교시 표).
  • TODO 2 범위 검사 — position 0 ~ 1, 밝기 · RGB 0 ~ 255, 핀 1 ~ 6, state 0 ~ 1
  • 합격 조건: 정상 호출 4 개는 ros2 service call ... 문자열, 잘못된 2 개(핀 7, LED 위치 2)는 ERROR 줄
python시뮬 실행 가능
# ch03 의 pymycobot 호출을 ROS 2 서비스 호출로 옮기기 (agv_pro_msgs)
SRV = {
    # ch03 함수 이름 : (서비스 이름, 서비스 형, 요청 필드 이름들)
    "set_led_color": ("/set_led_color", "agv_pro_msgs/srv/SetLedColor", ["position", "brightness", "r", "g", "b"]),
    # TODO 1: get_pin_input -> /get_digital_input (GetDigitalInput, ["pin"])
    #         set_pin_output -> /set_digital_output (SetDigitalOutput, ["pin", "state"])
    "get_pin_input": ("/get_digital_input", "agv_pro_msgs/srv/GetDigitalInput", []),
    "set_pin_output": ("/set_digital_output", "agv_pro_msgs/srv/SetDigitalOutput", []),
}
RANGE = {"position": (0, 1), "brightness": (0, 255), "r": (0, 255), "g": (0, 255), "b": (0, 255),
         "pin": (1, 6), "state": (0, 1)}

def flatten(func, args):
    """set_led_color(0, 255, (0, 255, 0)) 처럼 튜플이 섞인 인자를 펼침"""
    flat = []
    for a in args:
        if isinstance(a, (tuple, list)):
            flat.extend(a)
        else:
            flat.append(a)
    return flat

def to_service_call(func, *args):
    name, typ, fields = SRV[func]
    values = flatten(func, args)
    if len(values) != len(fields):
        raise ValueError(f"{func}: 필드 {len(fields)}개가 필요한데 {len(values)}개")
    req = {}
    for f, v in zip(fields, values):
        lo, hi = RANGE[f]
        # TODO 2: v 가 lo ~ hi 밖이면 ValueError 를 일으키세요
        req[f] = int(v)
    body = ", ".join(f"{k}: {v}" for k, v in req.items())
    return f'ros2 service call {name} {typ} "{{{body}}}"'

calls = [
    ("set_led_color", 0, 255, (0, 255, 0)),
    ("set_led_color", 1, 128, (255, 0, 0)),
    ("get_pin_input", 1),
    ("set_pin_output", 2, 1),
    ("set_pin_output", 7, 1),          # 잘못된 핀
    ("set_led_color", 2, 255, (0, 0, 255)),  # 잘못된 위치
]
for c in calls:
    try:
        print(to_service_call(c[0], *c[1:]))
    except ValueError as e:
        print("ERROR", e)
풀이
python시뮬 실행 가능
# ch03 의 pymycobot 호출을 ROS 2 서비스 호출로 옮기기 (agv_pro_msgs)
SRV = {
    # ch03 함수 이름 : (서비스 이름, 서비스 형, 요청 필드 이름들)
    "set_led_color": ("/set_led_color", "agv_pro_msgs/srv/SetLedColor", ["position", "brightness", "r", "g", "b"]),
    "get_pin_input": ("/get_digital_input", "agv_pro_msgs/srv/GetDigitalInput", ["pin"]),
    "set_pin_output": ("/set_digital_output", "agv_pro_msgs/srv/SetDigitalOutput", ["pin", "state"]),
}
RANGE = {"position": (0, 1), "brightness": (0, 255), "r": (0, 255), "g": (0, 255), "b": (0, 255),
         "pin": (1, 6), "state": (0, 1)}

def flatten(func, args):
    """set_led_color(0, 255, (0, 255, 0)) 처럼 튜플이 섞인 인자를 펼침"""
    flat = []
    for a in args:
        if isinstance(a, (tuple, list)):
            flat.extend(a)
        else:
            flat.append(a)
    return flat

def to_service_call(func, *args):
    name, typ, fields = SRV[func]
    values = flatten(func, args)
    if len(values) != len(fields):
        raise ValueError(f"{func}: 필드 {len(fields)}개가 필요한데 {len(values)}개")
    req = {}
    for f, v in zip(fields, values):
        lo, hi = RANGE[f]
        if not (lo <= v <= hi):
            raise ValueError(f"{func}: {f}={v} 가 범위 {lo}~{hi} 밖")
        req[f] = int(v)
    body = ", ".join(f"{k}: {v}" for k, v in req.items())
    return f'ros2 service call {name} {typ} "{{{body}}}"'

calls = [
    ("set_led_color", 0, 255, (0, 255, 0)),
    ("set_led_color", 1, 128, (255, 0, 0)),
    ("get_pin_input", 1),
    ("set_pin_output", 2, 1),
    ("set_pin_output", 7, 1),          # 잘못된 핀
    ("set_led_color", 2, 255, (0, 0, 255)),  # 잘못된 위치
]
for c in calls:
    try:
        print(to_service_call(c[0], *c[1:]))
    except ValueError as e:
        print("ERROR", e)
  • 출력 첫 줄이 1교시 본문의 명령과 같은지 확인합니다. 서비스는 요청을 보내기 전에 형(type)이 맞는지만 보므로, 범위 검사는 부르는 쪽의 책임입니다. 드라이버 코드에도 핀 1 ~ 6 검사가 있지만, 보내기 전에 막으면 원인을 더 빨리 찾습니다.
실습 2-1 · 오류 진단 카드 5장 (2교시)

로그 원문(또는 그와 같은 형태) 5 장을 보고 원인을 객관식으로 고릅니다. 먼저 코드를 보지 말고 카드마다 답을 공책에 적은 뒤, 같은 판단을 RULES (단서 → 원인) 로 옮겨 자동 진단기를 만듭니다.

  • TODO RULES 에 카드 2 ~ 5 의 단서와 원인 문구를 채웁니다. 단서는 로그에서 그 원인에만 나오는 문구 여야 합니다.
  • 합격 조건: score 5/5
  • 카드 2 의 문구는 드라이버마다 다르며, Permission denied 라는 낱말이 핵심입니다. 카드 1 · 4 · 5 는 원문 형식 그대로입니다.
python시뮬 실행 가능
# 오류 진단 카드 5장: 로그를 보고 원인 보기(1~4)를 고르는 규칙 만들기
CARDS = [
    {"log": "Package 'agv_pro_bringup' not found: \"package 'agv_pro_bringup' not found, searching: ['/opt/ros/humble']\"",
     "choices": ["작업 공간을 source 하지 않음", "라이다 종류가 다름", "배터리 전압 부족", "ROS_DOMAIN_ID 불일치"],
     "answer": 1},
    {"log": "[agv_pro_node-1] open /dev/ttyACM0 failed: Permission denied\n[ERROR] [agv_pro_node-1]: process has died",
     "choices": ["작업 공간을 source 하지 않음", "직렬 포트 권한 (dialout 그룹)", "비상정지 눌림", "Nav2 초기 위치 없음"],
     "answer": 2},
    {"log": "(로봇) ros2 topic echo /odom --once  -> 정상 출력\n(PC)   ros2 topic list -> /parameter_events  /rosout",
     "choices": ["agv_pro_node 가 죽음", "PC 에서 colcon build 안 함", "로봇과 PC 의 ROS_DOMAIN_ID 가 다름", "배터리 전압 부족"],
     "answer": 3},
    {"log": "[agv_pro_node-1] Battery voltage is below 19.5V.",
     "choices": ["직렬 포트 권한", "ROS_DOMAIN_ID 불일치", "라이다 종류가 다름", "충전 필요 (전압 19.5 V 미만)"],
     "answer": 4},
    {"log": "file 'agv_pro_gazebo.launch.py' was not found in the share directory of package 'agv_pro_description' at '/home/elephant/agv_pro_ros2/install/agv_pro_description/share/agv_pro_description'",
     "choices": ["Gazebo 가 설치되지 않음", "launch 파일이 다른 패키지(agv_pro_gazebo)에 있음", "source 를 하지 않음", "URDF 문법 오류"],
     "answer": 2},
]

# 규칙: (로그에 들어 있는 단서, 그 단서가 뜻하는 원인 문구의 일부)
RULES = [
    ("Package '", "source"),
    # TODO: 나머지 카드 4장의 단서와 원인 문구를 채우세요
    #   예) ("로그에 있는 특징적인 문구", "정답 보기에 들어 있는 낱말")
]

def diagnose(card):
    """규칙에 맞는 단서를 찾고, 그 원인 문구가 들어 있는 보기 번호(1~4)를 돌려줌. 모르면 0"""
    for clue, cause in RULES:
        if clue in card["log"]:
            for i, c in enumerate(card["choices"], start=1):
                if cause in c:
                    return i
    return 0

score = 0
for n, card in enumerate(CARDS, start=1):
    pick = diagnose(card)
    ok = pick == card["answer"]
    score += ok
    first = card["log"].splitlines()[0][:70]
    print(f"card {n}: pick {pick} / answer {card['answer']} {'OK' if ok else 'X'}  | {first}")
print(f"score {score}/{len(CARDS)}")

해 볼 것

  1. 카드 3 의 단서를 "/rosout" 하나로만 잡으면 어떤 문제가 생길까요? (다른 로그에도 나올 수 있는 낱말인지 생각합니다.)
  2. 시뮬레이터 2 의 상황 5(라이다 종류 불일치)를 카드 6 으로 추가하고 규칙을 하나 더 만듭니다.
풀이
python시뮬 실행 가능
# 오류 진단 카드 5장: 로그를 보고 원인 보기(1~4)를 고르는 규칙 만들기
CARDS = [
    {"log": "Package 'agv_pro_bringup' not found: \"package 'agv_pro_bringup' not found, searching: ['/opt/ros/humble']\"",
     "choices": ["작업 공간을 source 하지 않음", "라이다 종류가 다름", "배터리 전압 부족", "ROS_DOMAIN_ID 불일치"],
     "answer": 1},
    {"log": "[agv_pro_node-1] open /dev/ttyACM0 failed: Permission denied\n[ERROR] [agv_pro_node-1]: process has died",
     "choices": ["작업 공간을 source 하지 않음", "직렬 포트 권한 (dialout 그룹)", "비상정지 눌림", "Nav2 초기 위치 없음"],
     "answer": 2},
    {"log": "(로봇) ros2 topic echo /odom --once  -> 정상 출력\n(PC)   ros2 topic list -> /parameter_events  /rosout",
     "choices": ["agv_pro_node 가 죽음", "PC 에서 colcon build 안 함", "로봇과 PC 의 ROS_DOMAIN_ID 가 다름", "배터리 전압 부족"],
     "answer": 3},
    {"log": "[agv_pro_node-1] Battery voltage is below 19.5V.",
     "choices": ["직렬 포트 권한", "ROS_DOMAIN_ID 불일치", "라이다 종류가 다름", "충전 필요 (전압 19.5 V 미만)"],
     "answer": 4},
    {"log": "file 'agv_pro_gazebo.launch.py' was not found in the share directory of package 'agv_pro_description' at '/home/elephant/agv_pro_ros2/install/agv_pro_description/share/agv_pro_description'",
     "choices": ["Gazebo 가 설치되지 않음", "launch 파일이 다른 패키지(agv_pro_gazebo)에 있음", "source 를 하지 않음", "URDF 문법 오류"],
     "answer": 2},
]

# 규칙: (로그에 들어 있는 단서, 그 단서가 뜻하는 원인 문구의 일부)
RULES = [
    ("Package '", "source"),
    ("Permission denied", "권한"),
    ("/parameter_events  /rosout", "ROS_DOMAIN_ID"),
    ("below 19.5V", "19.5"),
    ("was not found in the share directory", "다른 패키지"),
]

def diagnose(card):
    """규칙에 맞는 단서를 찾고, 그 원인 문구가 들어 있는 보기 번호(1~4)를 돌려줌. 모르면 0"""
    for clue, cause in RULES:
        if clue in card["log"]:
            for i, c in enumerate(card["choices"], start=1):
                if cause in c:
                    return i
    return 0

score = 0
for n, card in enumerate(CARDS, start=1):
    pick = diagnose(card)
    ok = pick == card["answer"]
    score += ok
    first = card["log"].splitlines()[0][:70]
    print(f"card {n}: pick {pick} / answer {card['answer']} {'OK' if ok else 'X'}  | {first}")
print(f"score {score}/{len(CARDS)}")
  • 정답: 카드 1 ① source 누락, 카드 2 ② 직렬 포트 권한, 카드 3 ③ DOMAIN_ID 불일치, 카드 4 ④ 배터리 전압, 카드 5 ② launch 파일이 다른 패키지에 있음.
  • 진단의 원칙은 시뮬레이터 2 와 같습니다: "어느 단계에서 멈췄는가" 를 먼저 보고, 로그의 첫 오류 줄에서 원인에만 나오는 낱말을 찾습니다.
실습 3-1 · yaw ↔ 쿼터니언 변환과 저장소 예제 목표 읽기 (3교시)

yaw_to_quat() · quat_to_yaw() 를 완성하고, agv_pro_navigation2/scripts 예제의 목표 6 개가 각각 몇 도를 향하는지 읽습니다.

  • TODO 1 · 2 두 변환 함수 (각도의 절반)
  • 합격 조건: 0 · 45 · 90 · 120 · 180 · −90 ° 가 되돌린 값과 같고, 270° 는 −90° 로 나옴. 예제 목표의 |q| 가 모두 1.0000

예측 먼저 — goal_B 의 (0.687, 0.727) 은 대략 몇 도일까요? (z ≈ w 이면 θ/2 ≈ 45°)

python시뮬 실행 가능
import math

def yaw_to_quat(theta):
    """yaw(rad) -> (z, w). 바닥 로봇은 x = y = 0"""
    return 0.0, 1.0   # TODO 1: (sin(theta / 2), cos(theta / 2))

def quat_to_yaw(z, w):
    """(z, w) -> yaw(rad), -pi ~ pi"""
    return 0.0   # TODO 2: 2 * atan2(z, w)

def wrap(a):
    return math.atan2(math.sin(a), math.cos(a))

# agv_pro_navigation2/scripts 의 목표값 [x, y, oz, ow]
GOALS = {
    "nav_to_pose goal_A": [1.6766083240509033, 0.37930558800697327, -0.03491306994337919, 0.9993903529387947],
    "nav_to_pose goal_B": [-0.5062443017959595, 1.559376835823059, 0.6869307039904945, 0.7267229237578264],
    "waypoint 1": [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
    "waypoint 2": [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
    "waypoint 3": [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
    "waypoint 4": [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877],
}

print("name                    x       y      yaw(deg)  |q|")
for name, (x, y, oz, ow) in GOALS.items():
    th = quat_to_yaw(oz, ow)
    print(f"{name:20s} {x:7.2f} {y:7.2f}   {math.degrees(th):8.1f}  {math.hypot(oz, ow):.4f}")

print()
print("deg    z        w       back(deg)")
for deg in (0, 45, 90, 120, 180, -90, 270):
    z, w = yaw_to_quat(math.radians(deg))
    back = math.degrees(wrap(quat_to_yaw(z, w)))
    print(f"{deg:4d}  {z:7.4f}  {w:7.4f}   {back:7.1f}")

# 흔한 실수: 절반이 아니라 전체 각도를 넣음
z_bad, w_bad = math.sin(math.radians(120)), math.cos(math.radians(120))
print()
print("mistake (sin 120, cos 120) ->", round(math.degrees(wrap(quat_to_yaw(z_bad, w_bad))), 1), "deg")
# (z, w) 와 (-z, -w) 는 같은 방향
z, w = yaw_to_quat(math.radians(150))
print("(z, w) vs (-z, -w):", round(math.degrees(wrap(quat_to_yaw(z, w))), 1), round(math.degrees(wrap(quat_to_yaw(-z, -w))), 1))
풀이
python시뮬 실행 가능
import math

def yaw_to_quat(theta):
    """yaw(rad) -> (z, w). 바닥 로봇은 x = y = 0"""
    return math.sin(theta / 2), math.cos(theta / 2)

def quat_to_yaw(z, w):
    """(z, w) -> yaw(rad), -pi ~ pi"""
    return 2 * math.atan2(z, w)

def wrap(a):
    return math.atan2(math.sin(a), math.cos(a))

# agv_pro_navigation2/scripts 의 목표값 [x, y, oz, ow]
GOALS = {
    "nav_to_pose goal_A": [1.6766083240509033, 0.37930558800697327, -0.03491306994337919, 0.9993903529387947],
    "nav_to_pose goal_B": [-0.5062443017959595, 1.559376835823059, 0.6869307039904945, 0.7267229237578264],
    "waypoint 1": [0.5062479972839355, -0.5562516450881958, -0.011363976322727884, 0.9999354279362925],
    "waypoint 2": [1.7874977588653564, -0.6250066757202148, 0.7002746726771927, 0.7138735061667792],
    "waypoint 3": [1.3625057935714722, 1.5999948978424072, 0.9999809382375775, 0.006174395637980891],
    "waypoint 4": [-1.4687445163726807, 1.487505555152893, -0.9025521753719631, 0.43058050435584877],
}

print("name                    x       y      yaw(deg)  |q|")
for name, (x, y, oz, ow) in GOALS.items():
    th = quat_to_yaw(oz, ow)
    print(f"{name:20s} {x:7.2f} {y:7.2f}   {math.degrees(th):8.1f}  {math.hypot(oz, ow):.4f}")

print()
print("deg    z        w       back(deg)")
for deg in (0, 45, 90, 120, 180, -90, 270):
    z, w = yaw_to_quat(math.radians(deg))
    back = math.degrees(wrap(quat_to_yaw(z, w)))
    print(f"{deg:4d}  {z:7.4f}  {w:7.4f}   {back:7.1f}")

# 흔한 실수: 절반이 아니라 전체 각도를 넣음
z_bad, w_bad = math.sin(math.radians(120)), math.cos(math.radians(120))
print()
print("mistake (sin 120, cos 120) ->", round(math.degrees(wrap(quat_to_yaw(z_bad, w_bad))), 1), "deg")
# (z, w) 와 (-z, -w) 는 같은 방향
z, w = yaw_to_quat(math.radians(150))
print("(z, w) vs (-z, -w):", round(math.degrees(wrap(quat_to_yaw(z, w))), 1), round(math.degrees(wrap(quat_to_yaw(-z, -w))), 1))
  • 예시 결과: goal_A −4.0°, goal_B 86.8°, waypoint 1 ~ 4 는 −1.3°, 88.9°, 179.3°, −129.0° 입니다. RViz 에서 화살표로 찍은 값이라 깔끔한 각도가 아닙니다.
  • 흔한 실수 (sin 120°, cos 120°) 는 −120° 가 됩니다(확인 문제 3-2). 270° 처럼 범위를 넘는 각도는 −90° 와 같은 쿼터니언이 됩니다.
실습 3-2 · 입고장 → 선반 A → 선반 C → 출고장 순회 설계 (3교시)

시뮬레이터 3 과 같은 창고에서 웨이포인트 목록(PoseStamped 에 넣을 x · y · oz · ow)을 만들고, A* 경로 길이로 FollowWaypoints 와 NavigateThroughPoses 의 소요 시간을 미리 계산합니다. 실제 로봇에서는 이 목록을 그대로 followWaypoints() 에 넘기면 됩니다(실물 실습 3).

  • TODO 1 to_pose() 의 쿼터니언
  • TODO 2 FollowWaypoints 한 구간의 시간 = 출발 회전 + 주행(사다리꼴) + 도착 회전 + 대기
  • TODO 3 ROUTE 를 네 점으로
  • 합격 조건: 네 점의 oz · ow 가 시뮬레이터 3 의 표와 같고, FollowWaypoints 시간이 NavigateThroughPoses 보다 길게 나옴
python시뮬 실행 가능
import math
import heapq
import numpy as np

# 시뮬레이터 3 과 같은 창고: 10 x 6 m, 셀 0.1 m, 팽창 0.25 m
RES, NX, NY, R_INF = 0.1, 100, 60, 0.25
V_MAX, ACC, W_MAX, PAUSE = 0.26, 0.5, 0.5, 0.2   # agvpro.yaml 의 max_vel_x, 회전 0.5 rad/s, 대기 200 ms
HOME = (0.9, 0.9, math.pi / 2)

def make_map():
    xs = (np.arange(NX) + 0.5) * RES
    ys = (np.arange(NY) + 0.5) * RES
    X, Y = np.meshgrid(xs, ys)
    occ = np.zeros((NY, NX), bool)
    occ[0, :] = occ[-1, :] = occ[:, 0] = occ[:, -1] = True
    rows = (X >= 2) & (X < 8)
    for y0, y1 in ((4.4, 4.8), (3.0, 3.4), (1.6, 2.0)):      # 선반 A, B, C
        occ |= rows & (Y >= y0) & (Y < y1)
    return occ

def distance_field(occ, maxd=0.8):
    d = np.full(occ.shape, 9.0)
    k = int(maxd / RES)
    oy, ox = np.nonzero(occ)
    for dy in range(-k, k + 1):
        for dx in range(-k, k + 1):
            r = math.hypot(dx, dy) * RES
            if r > maxd:
                continue
            yy, xx = oy + dy, ox + dx
            m = (yy >= 0) & (yy < NY) & (xx >= 0) & (xx < NX)
            np.minimum.at(d, (yy[m], xx[m]), r)
    return d

def astar(d, start, goal):
    s = (int(start[0] / RES), int(start[1] / RES))
    g = (int(goal[0] / RES), int(goal[1] / RES))
    free = d > R_INF + 1e-6
    if not free[g[1], g[0]]:
        return None
    def h(c):
        dx, dy = abs(c[0] - g[0]), abs(c[1] - g[1])
        return RES * (max(dx, dy) + 0.4142 * min(dx, dy))
    cost = {s: 0.0}
    par = {s: None}
    pq = [(h(s), 0.0, s)]
    while pq:
        f, c, cur = heapq.heappop(pq)
        if cur == g:
            break
        if c > cost[cur] + 1e-9:
            continue
        for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)):
            n = (cur[0] + dx, cur[1] + dy)
            if not (0 <= n[0] < NX and 0 <= n[1] < NY) or not free[n[1], n[0]]:
                continue
            dist = d[n[1], n[0]]
            extra = 3 * math.exp(-5 * (dist - 0.3)) if dist < 0.8 else 0.0
            nc = c + math.hypot(dx, dy) * RES * (1 + extra)
            if nc < cost.get(n, 1e18):
                cost[n] = nc
                par[n] = cur
                heapq.heappush(pq, (nc + h(n), nc, n))
    if g not in par:
        return None
    cells = [g]
    while par[cells[-1]] is not None:
        cells.append(par[cells[-1]])
    pts = [((i + 0.5) * RES, (j + 0.5) * RES) for i, j in reversed(cells)]
    pts[0], pts[-1] = start[:2], goal[:2]
    return smooth(d, pts)

def visible(d, a, b):
    n = max(1, int(math.dist(a, b) / 0.05))
    for k in range(1, n):
        x = a[0] + (b[0] - a[0]) * k / n
        y = a[1] + (b[1] - a[1]) * k / n
        if d[int(y / RES), int(x / RES)] < 0.4:
            return False
    return True

def smooth(d, p):
    out, i = [p[0]], 0
    while i < len(p) - 1:
        j = len(p) - 1
        while j > i + 1 and not visible(d, p[i], p[j]):
            j -= 1
        out.append(p[j])
        i = j
    return out

def length(p):
    return sum(math.dist(p[k - 1], p[k]) for k in range(1, len(p)))

def heading(a, b):
    return math.atan2(b[1] - a[1], b[0] - a[0])

def turn(a, b):
    return abs(math.atan2(math.sin(b - a), math.cos(b - a)))

def drive_time(L):
    """사다리꼴 속도: 0 -> V_MAX -> 0 (가속도 ACC)"""
    if L <= V_MAX ** 2 / ACC:
        return 2 * math.sqrt(L / ACC)
    return L / V_MAX + V_MAX / ACC

# 웨이포인트: (이름, x, y, yaw_deg)
# TODO 3: 입고장 (0.9, 3.0, 180) -> 선반 A (5.0, 5.4, -90) -> 선반 C (5.0, 1.0, 90) -> 출고장 (9.2, 3.2, 0)
ROUTE = [("입고장", 0.9, 3.0, 180), ("출고장", 9.2, 3.2, 0)]

def to_pose(name, x, y, deg):
    th = math.radians(deg)
    # TODO 1: 쿼터니언 oz, ow 를 계산하세요
    return {"name": name, "x": x, "y": y, "th": th, "oz": 0.0, "ow": 1.0}

def follow_waypoints(d, poses):
    """FollowWaypoints 흉내: 점마다 출발 회전 + 주행 + 도착 회전 + 대기"""
    x, y, th = HOME
    total = 0.0
    for i, p in enumerate(poses):
        path = astar(d, (x, y), (p["x"], p["y"]))
        if path is None:
            print(f"current_waypoint={i} FAILED (missed_waypoints=[{i}])")
            return None
        L = length(path)
        rot0 = turn(th, heading(path[0], path[1]))
        rot1 = turn(heading(path[-2], path[-1]), p["th"])
        t = drive_time(L)   # TODO 2: 출발 회전 rot0 / W_MAX + 도착 회전 rot1 / W_MAX + 대기 PAUSE 더하기
        total += t
        print(f"current_waypoint={i} {p['name']:4s} L={L:5.2f} m  rot {math.degrees(rot0):5.0f}/{math.degrees(rot1):4.0f} deg  t={t:5.1f} s  sum={total:6.1f} s")
        x, y, th = p["x"], p["y"], p["th"]
    return total

def through_poses(d, poses):
    """NavigateThroughPoses 흉내: 이어서 달리고 마지막 방향만 맞춤 (중간 회전은 무시)"""
    x, y, th = HOME
    full = [(x, y)]
    for p in poses:
        seg = astar(d, full[-1], (p["x"], p["y"]))
        full += seg[1:]
    L = length(full)
    rot0 = turn(th, heading(full[0], full[1]))
    rot1 = turn(heading(full[-2], full[-1]), poses[-1]["th"])
    return L, rot0 / W_MAX + drive_time(L) + rot1 / W_MAX

d = distance_field(make_map())
poses = [to_pose(*r) for r in ROUTE]
for p in poses:
    print(f"{p['name']:4s} x={p['x']:.2f} y={p['y']:.2f} yaw={math.degrees(p['th']):6.1f}  oz={p['oz']:+.4f} ow={p['ow']:+.4f}")
print()
t_follow = follow_waypoints(d, poses)
L, t_through = through_poses(d, poses)
print()
print(f"FollowWaypoints      ~ {t_follow:6.1f} s")
print(f"NavigateThroughPoses ~ {t_through:6.1f} s  (path {L:.2f} m)")

해 볼 것

  1. 계산한 시간을 시뮬레이터 3 의 실행 결과(FollowWaypoints · NavigateThroughPoses)와 비교합니다. 어느 쪽이 얼마나 다르고, 이 계산이 빠뜨린 것은 무엇일까요?
  2. 순서를 입고장 → 선반 C → 선반 A → 출고장 으로 바꾸면 시간이 줄어드나요? 선반 A 와 C 의 방문 순서가 왜 중요한지 경로 길이로 설명합니다.
  3. V_MAX 를 0.5 로 올리면 시간이 반이 되나요? 줄지 않는 부분은 무엇인가요?
풀이
python시뮬 실행 가능
import math
import heapq
import numpy as np

# 시뮬레이터 3 과 같은 창고: 10 x 6 m, 셀 0.1 m, 팽창 0.25 m
RES, NX, NY, R_INF = 0.1, 100, 60, 0.25
V_MAX, ACC, W_MAX, PAUSE = 0.26, 0.5, 0.5, 0.2   # agvpro.yaml 의 max_vel_x, 회전 0.5 rad/s, 대기 200 ms
HOME = (0.9, 0.9, math.pi / 2)

def make_map():
    xs = (np.arange(NX) + 0.5) * RES
    ys = (np.arange(NY) + 0.5) * RES
    X, Y = np.meshgrid(xs, ys)
    occ = np.zeros((NY, NX), bool)
    occ[0, :] = occ[-1, :] = occ[:, 0] = occ[:, -1] = True
    rows = (X >= 2) & (X < 8)
    for y0, y1 in ((4.4, 4.8), (3.0, 3.4), (1.6, 2.0)):      # 선반 A, B, C
        occ |= rows & (Y >= y0) & (Y < y1)
    return occ

def distance_field(occ, maxd=0.8):
    d = np.full(occ.shape, 9.0)
    k = int(maxd / RES)
    oy, ox = np.nonzero(occ)
    for dy in range(-k, k + 1):
        for dx in range(-k, k + 1):
            r = math.hypot(dx, dy) * RES
            if r > maxd:
                continue
            yy, xx = oy + dy, ox + dx
            m = (yy >= 0) & (yy < NY) & (xx >= 0) & (xx < NX)
            np.minimum.at(d, (yy[m], xx[m]), r)
    return d

def astar(d, start, goal):
    s = (int(start[0] / RES), int(start[1] / RES))
    g = (int(goal[0] / RES), int(goal[1] / RES))
    free = d > R_INF + 1e-6
    if not free[g[1], g[0]]:
        return None
    def h(c):
        dx, dy = abs(c[0] - g[0]), abs(c[1] - g[1])
        return RES * (max(dx, dy) + 0.4142 * min(dx, dy))
    cost = {s: 0.0}
    par = {s: None}
    pq = [(h(s), 0.0, s)]
    while pq:
        f, c, cur = heapq.heappop(pq)
        if cur == g:
            break
        if c > cost[cur] + 1e-9:
            continue
        for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1), (1, 1), (1, -1), (-1, 1), (-1, -1)):
            n = (cur[0] + dx, cur[1] + dy)
            if not (0 <= n[0] < NX and 0 <= n[1] < NY) or not free[n[1], n[0]]:
                continue
            dist = d[n[1], n[0]]
            extra = 3 * math.exp(-5 * (dist - 0.3)) if dist < 0.8 else 0.0
            nc = c + math.hypot(dx, dy) * RES * (1 + extra)
            if nc < cost.get(n, 1e18):
                cost[n] = nc
                par[n] = cur
                heapq.heappush(pq, (nc + h(n), nc, n))
    if g not in par:
        return None
    cells = [g]
    while par[cells[-1]] is not None:
        cells.append(par[cells[-1]])
    pts = [((i + 0.5) * RES, (j + 0.5) * RES) for i, j in reversed(cells)]
    pts[0], pts[-1] = start[:2], goal[:2]
    return smooth(d, pts)

def visible(d, a, b):
    n = max(1, int(math.dist(a, b) / 0.05))
    for k in range(1, n):
        x = a[0] + (b[0] - a[0]) * k / n
        y = a[1] + (b[1] - a[1]) * k / n
        if d[int(y / RES), int(x / RES)] < 0.4:
            return False
    return True

def smooth(d, p):
    out, i = [p[0]], 0
    while i < len(p) - 1:
        j = len(p) - 1
        while j > i + 1 and not visible(d, p[i], p[j]):
            j -= 1
        out.append(p[j])
        i = j
    return out

def length(p):
    return sum(math.dist(p[k - 1], p[k]) for k in range(1, len(p)))

def heading(a, b):
    return math.atan2(b[1] - a[1], b[0] - a[0])

def turn(a, b):
    return abs(math.atan2(math.sin(b - a), math.cos(b - a)))

def drive_time(L):
    """사다리꼴 속도: 0 -> V_MAX -> 0 (가속도 ACC)"""
    if L <= V_MAX ** 2 / ACC:
        return 2 * math.sqrt(L / ACC)
    return L / V_MAX + V_MAX / ACC

# 웨이포인트: (이름, x, y, yaw_deg)
ROUTE = [("입고장", 0.9, 3.0, 180), ("선반 A", 5.0, 5.4, -90), ("선반 C", 5.0, 1.0, 90), ("출고장", 9.2, 3.2, 0)]

def to_pose(name, x, y, deg):
    th = math.radians(deg)
    return {"name": name, "x": x, "y": y, "th": th, "oz": math.sin(th / 2), "ow": math.cos(th / 2)}

def follow_waypoints(d, poses):
    """FollowWaypoints 흉내: 점마다 출발 회전 + 주행 + 도착 회전 + 대기"""
    x, y, th = HOME
    total = 0.0
    for i, p in enumerate(poses):
        path = astar(d, (x, y), (p["x"], p["y"]))
        if path is None:
            print(f"current_waypoint={i} FAILED (missed_waypoints=[{i}])")
            return None
        L = length(path)
        rot0 = turn(th, heading(path[0], path[1]))
        rot1 = turn(heading(path[-2], path[-1]), p["th"])
        t = rot0 / W_MAX + drive_time(L) + rot1 / W_MAX + PAUSE
        total += t
        print(f"current_waypoint={i} {p['name']:4s} L={L:5.2f} m  rot {math.degrees(rot0):5.0f}/{math.degrees(rot1):4.0f} deg  t={t:5.1f} s  sum={total:6.1f} s")
        x, y, th = p["x"], p["y"], p["th"]
    return total

def through_poses(d, poses):
    """NavigateThroughPoses 흉내: 이어서 달리고 마지막 방향만 맞춤 (중간 회전은 무시)"""
    x, y, th = HOME
    full = [(x, y)]
    for p in poses:
        seg = astar(d, full[-1], (p["x"], p["y"]))
        full += seg[1:]
    L = length(full)
    rot0 = turn(th, heading(full[0], full[1]))
    rot1 = turn(heading(full[-2], full[-1]), poses[-1]["th"])
    return L, rot0 / W_MAX + drive_time(L) + rot1 / W_MAX

d = distance_field(make_map())
poses = [to_pose(*r) for r in ROUTE]
for p in poses:
    print(f"{p['name']:4s} x={p['x']:.2f} y={p['y']:.2f} yaw={math.degrees(p['th']):6.1f}  oz={p['oz']:+.4f} ow={p['ow']:+.4f}")
print()
t_follow = follow_waypoints(d, poses)
L, t_through = through_poses(d, poses)
print()
print(f"FollowWaypoints      ~ {t_follow:6.1f} s")
print(f"NavigateThroughPoses ~ {t_through:6.1f} s  (path {L:.2f} m)")
  • 예시 결과: FollowWaypoints 약 119 s, NavigateThroughPoses 약 98 s (경로 24.6 m). 시뮬레이터 3 은 약 117 s · 105 s 입니다. 이 계산은 NavigateThroughPoses 의 중간 점에서 경로가 꺾일 때 제자리 회전하는 시간과, yaw_goal_tolerance(0.25 rad) 안에 들면 회전을 멈추는 효과를 넣지 않았습니다.
  • 선반 A → C 구간(11.1 m)은 선반 열 끝을 돌아가야 해서 가장 깁니다. 속도를 올려도 회전 · 대기 시간은 그대로라 시간이 반이 되지 않습니다.
실습 4-1 · 방전되지 않는 충전 시작 임계값 찾기 (4교시)

시뮬레이터 4 와 같은 하루 운용 모델로, 배터리 소모율과 미션 길이를 바꿔 가며 30 개의 무작위 날 모두에서 방전 정지가 없는 가장 낮은 임계값을 찾고 부등식과 비교합니다.

  • TODO 1 미션이 끝날 때 잔량이 임계값보다 크면 충전하지 않고 다음 미션
  • TODO 2 formula() — 하한 + 최악의 미션 1 회 + 충전대까지
  • 합격 조건: 표의 sim_min_thr 와 formula 가 몇 % 안에서 비슷하고, 임계값 15 % 는 30 일 중 여러 날 방전, 22 % 는 0 일
python시뮬 실행 가능
import random

# 시뮬레이터 4 와 같은 모델 (단위: %, s)
V, LOAD, DOCK, RESERVE, FULL, DAY = 0.4, 120, 60, 10.0, 90.0, 24 * 3600

def run_day(thr, rate_h=10, L=1200, var=0.30, dist=40, charge_h=16, seed=0):
    """하루 운용. 반환: (끝낸 미션 수, 방전 정지 횟수)"""
    rnd = random.Random(seed)
    rs, cs = rate_h / 3600, charge_h / 3600
    trip = dist / V + DOCK
    t, b, done, dead = 0.0, 100.0, 0, 0
    while t < DAY:
        dur = (L / V) * (1 + var * (2 * rnd.random() - 1)) + LOAD
        if b - rs * dur < RESERVE:              # 미션 도중 하한
            dead += 1
            t += (b - RESERVE) / rs + 1800       # 멈춘 뒤 사람이 옮기는 30 분
            b = RESERVE
        else:
            t += dur
            b -= rs * dur
            done += 1
            if True:                             # TODO 1: 잔량 b 가 임계값 thr 보다 크면 충전하지 않고 다음 미션
                continue
            if b - rs * trip < RESERVE:          # 충전대로 가는 길에 하한
                dead += 1
                t += 1800
                b = RESERVE
            else:
                t += trip
                b -= rs * trip
        t += (FULL - b) / cs                     # 90 % 까지 충전
        b = FULL
        t += dist / V                            # 미션 위치로 복귀
        b -= rs * dist / V
    return done, dead

def min_safe_threshold(seeds=30, **kw):
    """모든 seed 에서 방전 정지가 0 인 가장 낮은 임계값 (정수 %)"""
    for thr in range(10, 81):
        if all(run_day(thr, seed=s, **kw)[1] == 0 for s in range(seeds)):
            return thr
    return None

def formula(rate_h=10, L=1200, var=0.30, dist=40, charge_h=16):
    rs = rate_h / 3600
    return RESERVE   # TODO 2: + 최악의 미션 1 회 소모 + 충전대까지 소모

print(" L(m)  rate(%/h)  sim_min_thr  formula")
for L in (600, 1200, 2400):
    for rate in (10, 20):
        sim = min_safe_threshold(L=L, rate_h=rate)
        print(f"{L:5d}   {rate:5d}       {sim}        {formula(rate_h=rate, L=L):5.1f}")

print()
for thr in (15, 22, 40):
    res = [run_day(thr, seed=s) for s in range(30)]
    bad = sum(1 for _, dd in res if dd > 0)
    avg = sum(m for m, _ in res) / len(res)
    print(f"thr {thr:2d} %: days with dead stop {bad:2d}/30, missions/day {avg:4.1f}")
풀이
python시뮬 실행 가능
import random

# 시뮬레이터 4 와 같은 모델 (단위: %, s)
V, LOAD, DOCK, RESERVE, FULL, DAY = 0.4, 120, 60, 10.0, 90.0, 24 * 3600

def run_day(thr, rate_h=10, L=1200, var=0.30, dist=40, charge_h=16, seed=0):
    """하루 운용. 반환: (끝낸 미션 수, 방전 정지 횟수)"""
    rnd = random.Random(seed)
    rs, cs = rate_h / 3600, charge_h / 3600
    trip = dist / V + DOCK
    t, b, done, dead = 0.0, 100.0, 0, 0
    while t < DAY:
        dur = (L / V) * (1 + var * (2 * rnd.random() - 1)) + LOAD
        if b - rs * dur < RESERVE:              # 미션 도중 하한
            dead += 1
            t += (b - RESERVE) / rs + 1800       # 멈춘 뒤 사람이 옮기는 30 분
            b = RESERVE
        else:
            t += dur
            b -= rs * dur
            done += 1
            if b > thr:                          # 미션이 끝날 때 확인
                continue
            if b - rs * trip < RESERVE:          # 충전대로 가는 길에 하한
                dead += 1
                t += 1800
                b = RESERVE
            else:
                t += trip
                b -= rs * trip
        t += (FULL - b) / cs                     # 90 % 까지 충전
        b = FULL
        t += dist / V                            # 미션 위치로 복귀
        b -= rs * dist / V
    return done, dead

def min_safe_threshold(seeds=30, **kw):
    """모든 seed 에서 방전 정지가 0 인 가장 낮은 임계값 (정수 %)"""
    for thr in range(10, 81):
        if all(run_day(thr, seed=s, **kw)[1] == 0 for s in range(seeds)):
            return thr
    return None

def formula(rate_h=10, L=1200, var=0.30, dist=40, charge_h=16):
    rs = rate_h / 3600
    return RESERVE + rs * ((L / V) * (1 + var) + LOAD) + rs * (dist / V + DOCK)

print(" L(m)  rate(%/h)  sim_min_thr  formula")
for L in (600, 1200, 2400):
    for rate in (10, 20):
        sim = min_safe_threshold(L=L, rate_h=rate)
        print(f"{L:5d}   {rate:5d}       {sim}        {formula(rate_h=rate, L=L):5.1f}")

print()
for thr in (15, 22, 40):
    res = [run_day(thr, seed=s) for s in range(30)]
    bad = sum(1 for _, dd in res if dd > 0)
    avg = sum(m for m, _ in res) / len(res)
    print(f"thr {thr:2d} %: days with dead stop {bad:2d}/30, missions/day {avg:4.1f}")
  • 예시 결과: 1 200 m · 10 %/h 에서 시뮬레이션 22 %, 부등식 21.6 %. 미션 길이를 두 배로 하면 필요한 여유(임계값 − 10 %)도 거의 두 배, 소모율을 두 배로 해도 거의 두 배가 됩니다. 2 400 m 에서 시뮬레이션 값(29 %)이 부등식(32.4 %)보다 낮은 것은 30 일 안에 "최악의 +30 %" 가 나오지 않았을 뿐입니다 — 운영 기준은 부등식 쪽을 씁니다.
  • 임계값 15 % 는 30 일 중 23 일 방전 정지가 생깁니다. 22 % 와 40 % 의 하루 미션 수는 큰 차이가 없습니다.
실습 4-2 · charger_position.json 으로 접근 목표 계산 (4교시)

combined_auto_recharger 가 하는 계산을 그대로 따라 합니다. 저장소의 충전대 위치와 nav_goal_params.yaml 값을 넣어 Nav2 로 보낼 접근 목표를 구합니다.

  • TODO 1 ~ 3 쿼터니언 → yaw, 충전대 앞 forward_distance, yaw_offset_deg 보정
  • 합격 조건: 충전대와 목표 사이 거리 1.000 m, 목표 yaw 가 충전대 yaw 보다 10° 작음
python시뮬 실행 가능
import json
import math

# agv_pro_autocharge/config 의 두 파일 (저장소 값 그대로)
CHARGER_JSON = '{"p_x": -0.04373347759246826, "p_y": -4.024151802062988, "orien_z": 0.09378381732290733, "orien_w": 0.9955925851513477}'
NAV_GOAL_PARAMS = {"forward_distance": 1.0, "yaw_offset_deg": 10.0}

def approach_goal(charger, forward_distance, yaw_offset_deg):
    """충전대 자세 -> Nav2 로 보낼 접근 목표 (x, y, yaw, oz, ow)"""
    yaw = 0.0                      # TODO 1: 쿼터니언 -> yaw
    gx = charger["p_x"]            # TODO 2: 충전대 방향으로 forward_distance 만큼 앞
    gy = charger["p_y"]
    gyaw = yaw                     # TODO 3: yaw_offset_deg 만큼 시계 방향(빼기)
    return gx, gy, gyaw, math.sin(gyaw / 2), math.cos(gyaw / 2)

charger = json.loads(CHARGER_JSON)
yaw_c = 2 * math.atan2(charger["orien_z"], charger["orien_w"])
print(f"charger : x={charger['p_x']:.3f} y={charger['p_y']:.3f} yaw={math.degrees(yaw_c):.2f} deg")
gx, gy, gyaw, oz, ow = approach_goal(charger, **NAV_GOAL_PARAMS)
print(f"goal    : x={gx:.3f} y={gy:.3f} yaw={math.degrees(gyaw):.2f} deg  oz={oz:.4f} ow={ow:.4f}")
print(f"distance charger-goal = {math.hypot(gx - charger['p_x'], gy - charger['p_y']):.3f} m")

# 설정 파일을 못 찾으면 노드는 (0, 0, 0, 1) 을 씀 -> 어디로 가게 될까?
default = {"p_x": 0.0, "p_y": 0.0, "orien_z": 0.0, "orien_w": 1.0}
dx, dy, dyaw, _, _ = approach_goal(default, 1.0, 0.0)
print(f"file missing -> goal x={dx:.2f} y={dy:.2f} yaw={math.degrees(dyaw):.1f} deg (map origin + 1 m)")

해 볼 것

  1. 충전대 yaw 가 10.76° 인데 목표 yaw 를 10° 빼는 이유를 생각해 봅니다(도킹 모듈의 적외선 정렬 범위 · 충전대 설치 각도 보정, 확인 필요). yaw_offset_deg 를 0 으로 두면 목표가 어떻게 바뀌나요?
  2. 설정 파일 경로가 맞지 않아 기본값 (0, 0, 0, 1) 이 쓰이면 로봇은 어디로 가나요? 그곳이 지도 위에서 벽이나 선반이라면 어떤 오류가 날까요?
풀이
python시뮬 실행 가능
import json
import math

# agv_pro_autocharge/config 의 두 파일 (저장소 값 그대로)
CHARGER_JSON = '{"p_x": -0.04373347759246826, "p_y": -4.024151802062988, "orien_z": 0.09378381732290733, "orien_w": 0.9955925851513477}'
NAV_GOAL_PARAMS = {"forward_distance": 1.0, "yaw_offset_deg": 10.0}

def approach_goal(charger, forward_distance, yaw_offset_deg):
    """충전대 자세 -> Nav2 로 보낼 접근 목표 (x, y, yaw, oz, ow)"""
    yaw = 2 * math.atan2(charger["orien_z"], charger["orien_w"])
    gx = charger["p_x"] + forward_distance * math.cos(yaw)
    gy = charger["p_y"] + forward_distance * math.sin(yaw)
    gyaw = yaw - math.radians(yaw_offset_deg)
    return gx, gy, gyaw, math.sin(gyaw / 2), math.cos(gyaw / 2)

charger = json.loads(CHARGER_JSON)
yaw_c = 2 * math.atan2(charger["orien_z"], charger["orien_w"])
print(f"charger : x={charger['p_x']:.3f} y={charger['p_y']:.3f} yaw={math.degrees(yaw_c):.2f} deg")
gx, gy, gyaw, oz, ow = approach_goal(charger, **NAV_GOAL_PARAMS)
print(f"goal    : x={gx:.3f} y={gy:.3f} yaw={math.degrees(gyaw):.2f} deg  oz={oz:.4f} ow={ow:.4f}")
print(f"distance charger-goal = {math.hypot(gx - charger['p_x'], gy - charger['p_y']):.3f} m")

# 설정 파일을 못 찾으면 노드는 (0, 0, 0, 1) 을 씀 -> 어디로 가게 될까?
default = {"p_x": 0.0, "p_y": 0.0, "orien_z": 0.0, "orien_w": 1.0}
dx, dy, dyaw, _, _ = approach_goal(default, 1.0, 0.0)
print(f"file missing -> goal x={dx:.2f} y={dy:.2f} yaw={math.degrees(dyaw):.1f} deg (map origin + 1 m)")
  • 예시 결과: 충전대 (−0.044, −4.024, 10.76°) → 목표 (0.939, −3.837, 0.76°), oz 0.0067 · ow 1.0000. 충전대 쪽에서 1 m 떨어진 곳에서 충전대를 거의 정면으로 보는 자세입니다.
  • 파일을 못 찾으면 지도 원점에서 1 m 앞 (1.00, 0.00) 으로 갑니다. 원점이 벽 근처면 Nav2 가 계획에 실패하고, 빈 곳이면 엉뚱한 곳에서 도킹을 시도합니다 — 노드 시작 로그와 RViz 의 /goal_marker 화살표 위치를 꼭 확인합니다.

실습 (실물)#

myAGV Pro (내장 컴퓨터에 ~/agv_pro_ros2 가 빌드된 상태)와 같은 공유기에 연결한 PC 로 합니다. 모든 명령은 새 터미널마다 source ~/agv_pro_ros2/install/setup.bash 를 먼저 실행한 것으로 봅니다.

실물 운용 전 확인

실물 실습 1 — 그래프 확인과 한 번 움직이기 (1 · 2교시)

bash
# 로봇
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
# PC (같은 ROS_DOMAIN_ID)
ros2 node list                       # /agv_pro_node, /robot_state_publisher, 라이다 노드가 보이는가
ros2 topic hz /odom                  # 약 50 Hz 인가 (발행 루프 20 ms)
ros2 topic info /cmd_vel --verbose   # 구독자: agv_pro_node 1 개
ros2 topic echo /voltage --once      # 배터리 전압
ros2 service call /set_led_color agv_pro_msgs/srv/SetLedColor "{position: 0, brightness: 255, r: 0, g: 0, b: 255}"
# 0.2 m/s 명령 한 번 (로봇 앞 2 m 를 비우고, 보조자는 비상정지 옆에)
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

--once 로 한 번 보냈을 때 로봇이 잠깐 갔다가 서는지, 계속 가는지 를 기록합니다. 계속 간다면 이 펌웨어 · 드라이버 조합에는 명령 타임아웃이 없다는 뜻이므로 바로 ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{}" (모두 0)을 보내거나 비상정지를 누릅니다. 이 결과는 실습 1-1 의 timeout 값을 정하는 근거가 됩니다.

실물 실습 2 — Gazebo 로 먼저, 그다음 실물 (2교시)

bash
# PC
ros2 launch agv_pro_gazebo agv_pro_gazebo.launch.py        # 안 되면 문서의 agv_pro_description 쪽 명령
ros2 run teleop_twist_keyboard teleop_twist_keyboard

Gazebo 에서 전진 · 회전 · (Shift 키) 옆 이동을 해 보고, 같은 teleop 으로 실물을 조작해 차이(가속감 · 미끄러짐)를 적습니다. Gazebo 를 켤 때는 PC 의 ROS_DOMAIN_ID 를 로봇과 다르게 두어야 Gazebo 와 실물이 같은 /cmd_vel 을 받지 않습니다. 실물을 조작할 때는 Gazebo 를 끄고, 새 터미널에서 로봇과 같은 도메인으로 바꿉니다.

실물 실습 3 — 지도 위 웨이포인트 순회 (3교시)

  1. ch06 에서 만든 실습실 지도로 Nav2 를 켭니다: ros2 launch agv_pro_navigation2 navigation2_active.launch.py map:=<지도.yaml>.
  2. RViz 에서 2D Pose Estimate 로 초기 위치를 주고, 레이저 점이 벽과 겹치는지 확인합니다(ch07).
  3. RViz 의 Publish Point 또는 ros2 topic echo /clicked_point 로 실습실의 "입고장 · 선반 A · 선반 C · 출고장" 네 지점 좌표를 읽고, 실습 3-2 의 ROUTE 에 넣어 oz · ow 를 계산합니다.
  4. 아래 스크립트로 순회합니다. 처음에는 PAUSE 를 길게(3 s) 두어 점마다 위치를 확인합니다.
python실물 전용
import math
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

ROUTE = [("in", 0.9, 3.0, 180), ("A", 5.0, 5.4, -90), ("C", 5.0, 1.0, 90), ("out", 9.2, 3.2, 0)]  # 실습실 값으로 바꿈

def pose(nav, x, y, deg):
    p = PoseStamped()
    p.header.frame_id = 'map'
    p.header.stamp = nav.get_clock().now().to_msg()
    p.pose.position.x, p.pose.position.y = x, y
    th = math.radians(deg)
    p.pose.orientation.z, p.pose.orientation.w = math.sin(th / 2), math.cos(th / 2)
    return p

rclpy.init()
nav = BasicNavigator()
nav.waitUntilNav2Active()
nav.followWaypoints([pose(nav, x, y, d) for _, x, y, d in ROUTE])
while not nav.isTaskComplete():
    fb = nav.getFeedback()
    if fb:
        print("current_waypoint:", fb.current_waypoint)
result = nav.getResult()
print("result:", result)
  1. 같은 점을 goThroughPoses() 로 바꿔 시간을 비교하고, 시뮬레이터 3 · 실습 3-2 의 예측과 표로 정리합니다. 사람이 경로를 막아 서면 number_of_recoveries 가 바뀌는지도 봅니다(보조자가 비상정지 옆에 있을 때만).

실물 실습 4 — 자동 충전 (4교시, 자동 충전 모듈 장착형)

  1. 충전대를 벽에 붙여 두고, Nav2 를 켠 상태에서 ros2 run agv_pro_autocharge combined_auto_recharger 를 실행합니다. 시작 로그에서 설정 파일을 읽었는지(경로) 확인합니다.
  2. RViz 의 2D Goal Pose 도구 토픽을 /charger_position_update 로 바꾸고, 충전대 위치 · 방향을 클릭합니다. /goal_marker 화살표가 충전대에 맞는지 봅니다.
  3. 실습 4-2 로 접근 목표를 계산해 두고, 터미널에서 q 를 눌러 출발합니다. 로봇이 계산한 목표 근처에 서는지, 이어서 저속 도킹으로 넘어가는지 관찰합니다.
  4. 도킹이 끝나면 충전 표시등을 확인합니다. 실패하면 "자주 나는 오류" 7 · 8 을 봅니다.

자주 나는 오류와 해결#

1) 패키지를 찾지 못함 (source 누락)#

text
Package 'agv_pro_bringup' not found: "package 'agv_pro_bringup' not found, searching: ['/opt/ros/humble']"
  • 원인: 이 터미널이 작업 공간을 모릅니다. 검색 경로에 /opt/ros/humble 만 있고 ~/agv_pro_ros2/install 이 없다는 것이 단서입니다. colcon build 를 하지 않았거나 실패해서 install/ 이 없는 경우도 같은 문구가 나옵니다.
  • 해결: source ~/agv_pro_ros2/install/setup.bash 를 실행하고, 매번 하기 싫으면 ~/.bashrc 끝에 넣습니다. 그래도 안 되면 ls ~/agv_pro_ros2/install/agv_pro_bringup 으로 빌드 결과가 있는지 보고, 없으면 colcon build 의 마지막 요약(Summary: N packages finished, M packages failed)을 확인합니다.

2) launch 파일을 찾지 못함 (패키지 이름이 다름)#

text
file 'agv_pro_gazebo.launch.py' was not found in the share directory of package 'agv_pro_description' at '/home/elephant/agv_pro_ros2/install/agv_pro_description/share/agv_pro_description'
  • 원인: 패키지는 찾았지만 그 안에 launch 파일이 없습니다. 공식 문서의 Gazebo 명령은 agv_pro_description 을 쓰지만, 저장소(humble)에서는 이 파일이 agv_pro_gazebo 패키지에 있습니다(2교시).
  • 해결: ls $(ros2 pkg prefix agv_pro_gazebo)/share/agv_pro_gazebo/launch 로 실제 파일을 찾고 ros2 launch agv_pro_gazebo agv_pro_gazebo.launch.py 로 실행합니다. 파일을 고친 뒤에는 다시 빌드해야 share/ 에 반영됩니다(--symlink-install 이 아니라면).

3) 직렬 포트를 열지 못함#

text
[Errno 13] could not open port /dev/ttyACM0: [Errno 13] Permission denied: '/dev/ttyACM0'
  • 원인: 위 문구는 ch03 의 Python(pyserial) 원문이며, C++ 드라이버(agv_pro_node)의 문구는 다르지만 Permission denied · No such file or directory 같은 핵심 낱말은 같습니다. 권한이 없거나(dialout 그룹), 장치 이름이 틀렸거나(로봇 내장 컴퓨터는 /dev/agvpro_controller, PC 직결은 /dev/ttyACM0), 다른 프로그램(ch03 의 Python 스크립트 등)이 포트를 쓰고 있습니다.
  • 해결: sudo usermod -aG dialout $USER 후 다시 로그인, ls -l /dev/agvpro_controller /dev/ttyACM* 로 이름 확인, PC 에서 직접 연결했다면 port_name:=/dev/ttyACM0 를 줍니다. 포트를 쥔 프로세스는 fuser /dev/agvpro_controller 로 찾습니다. bringup 중에는 Python SDK 를 함께 쓰지 않습니다(1교시).

4) 시작하자마자 전압 오류#

text
Battery voltage is below 19.5V.
  • 원인: agv_pro_node 가 시작 검사에서 배터리 전압이 19.5 V 미만이라고 판단했습니다(전원 상태 오류 코드 3). 거의 빈 배터리는 충전기를 꽂은 직후 잠깐 통과했다가 출발 순간 다시 처질 수 있습니다(확인 문제 2-2). 오류 코드 2(비상정지 미해제), 4(CAN 초기화), 5(모터 초기화)도 같은 시작 검사에서 나옵니다.
  • 해결: 충분히 충전한 뒤 다시 시작합니다. 운용 중에는 voltage 토픽을 구독해 미션 관리자가 미리 충전하러 가게 합니다(4교시).

5) PC 에서 로봇 토픽이 보이지 않음#

text
$ ros2 topic list
/parameter_events
/rosout
  • 원인: 로봇 쪽은 정상인데 discovery 가 되지 않습니다. ROS_DOMAIN_ID 가 다르거나, 어느 한쪽이 ROS_LOCALHOST_ONLY=1 이거나, 다른 네트워크 · 멀티캐스트를 막는 공유기(손님용 Wi-Fi 등)입니다. RMW_IMPLEMENTATION 이 서로 다르면 통신이 안 되는 경우도 있습니다(공식 문서는 rmw_fastrtps_cpp 로 맞춤).
  • 해결: 두 쪽에서 printenv | grep ROS 로 ROS_DOMAIN_ID · ROS_LOCALHOST_ONLY 를 비교하고 ~/.bashrc 를 같게 고친 뒤 새 터미널을 엽니다. 설정을 바꾼 뒤에는 ros2 daemon stop 으로 낡은 목록을 지웁니다. 그래도 안 되면 ros2 multicast receive / ros2 multicast send 로 멀티캐스트가 통하는지 확인합니다.

6) Nav2 목표가 시작되지 않음 (초기 위치 없음)#

text
[amcl]: AMCL cannot publish a pose or update the transform. Please set the initial pose...
[bt_navigator]: Goal was rejected  /  Timed out waiting for transform from base_link to map
  • 원인: AMCL 이 초기 위치를 받지 못해 map → odom TF 가 없습니다(ch07). 저장소 예제는 setInitialPose() · waitUntilNav2Active() 를 주석 처리해 두었으므로, RViz 에서 위치를 주지 않고 예제를 돌리면 이렇게 됩니다. 문구는 Nav2 버전에 따라 다릅니다.
  • 해결: RViz 의 2D Pose Estimate 로 초기 위치를 주거나, 스크립트에서 nav.setInitialPose(...) 후 nav.waitUntilNav2Active() 를 부릅니다. 목표 자세의 쿼터니언이 정규화되었는지(z² + w² = 1)도 확인합니다(실습 3-1).

7) 자동 충전 노드가 엉뚱한 곳으로 감#

text
(로그) 설정 파일을 찾지 못해 기본값 사용  /  RViz 의 Charger 화살표가 지도 원점 근처에 있음
  • 원인: combined_auto_recharger 는 설정 파일을 /home/elephant/agv_pro_ros2/src/agv_pro_autocharge/config 같은 고정 경로에서 찾습니다. 사용자 이름 · 작업 공간 위치가 다르면 charger_position.json 을 못 읽고 (0, 0, 0, 1) 을 써서, 지도 원점 1 m 앞으로 갑니다(실습 4-2). 로그 문구는 코드 버전에 따라 다릅니다.
  • 해결: 시작 로그의 경로를 확인하고, 같은 경로에 설정 파일을 두거나 코드의 경로 목록을 고친 뒤 다시 빌드합니다. RViz 에서 /goal_marker 를 켜고 충전대 위치를 /charger_position_update 로 다시 찍습니다.

8) 자동 충전 README 대로 했는데 패키지 · 포트가 없음#

text
Package 'agv_autocharge_ros2' not found
  • 원인: 패키지 안 README 가 옛 이름(agv_autocharge_ros2, /dev/ttyCH341USB0)을 쓰고 있습니다. 실제는 agv_pro_autocharge 패키지, 도킹 모듈 포트 /dev/agvpro_ec130 (9600 bps)입니다.
  • 해결: ros2 run agv_pro_autocharge combined_auto_recharger 로 실행하고, ls -l /dev/agvpro_ec130 으로 도킹 모듈 장치가 있는지 확인합니다. 없으면 자동 충전 모듈 장착형인지, udev 규칙이 설치되었는지 확인합니다(확인 필요).

9) 두 로봇이 같은 명령으로 함께 움직임#

text
$ ros2 topic info /cmd_vel
Type: geometry_msgs/msg/Twist
Publisher count: 1
Subscription count: 2
  • 원인: 같은 ROS_DOMAIN_ID 에서 두 로봇의 agv_pro_node 가 모두 절대 이름 /cmd_vel 을 구독합니다. namespace 를 줘도 이 이름은 바뀌지 않습니다(시뮬레이터 5, 확인 문제 4-2).
  • 해결: 실습 단계에서는 로봇마다 ROS_DOMAIN_ID 를 다르게 둡니다. 한 PC 로 함께 관제해야 하면 드라이버의 구독 이름을 상대 이름으로 바꾸고 bringup 의 리매핑 ('cmd_vel', '/cmd_vel') 도 고친 뒤 다시 빌드합니다. 고친 뒤 ros2 topic info /agv_pro1/cmd_vel 의 구독자가 1 인지 확인합니다.

10) Playground 에서 각도가 이상하게 나옴#

text
mistake (sin 120, cos 120) -> -120.0 deg
  • 원인: 쿼터니언에 각도의 절반이 아니라 전체 각도를 넣었거나, math.sin(120) 처럼 도(°) 를 라디안 함수에 그대로 넣었습니다(math.sin(120) 은 120 rad).
  • 해결: math.radians(deg) 로 바꾼 뒤 sin(θ/2) · cos(θ/2) 를 씁니다. 결과는 quat_to_yaw() 로 되돌려 원래 각도가 나오는지 확인하는 습관을 들입니다(실습 3-1).

11) 시스템이 망가져 부팅이 안 됨 — 이미지 다시 굽기#

text
$ cat /etc/version
cat: /etc/version: No such file or directory
  • 원인: 내장 컴퓨터의 SSD 시스템이 손상되었거나, 다른 이미지로 덮어써 버전 파일이 없습니다. 공식 이미지에서는 cat /etc/version 이 v1.0.0_250716 같은 버전을 보여 줍니다 (공식 문서 5.1 이미지 업데이트).
  • 해결: 공식 문서는 특별한 경우가 아니면 직접 분해하지 말라 고 경고합니다. 꼭 필요하면 문서 절차를 그대로 따릅니다: 메인보드 뒤의 SSD(기본 238 GB)를 꺼내 M.2 NVMe 어댑터로 Windows PC 에 연결 → 포맷 창은 모두 취소 → DiskGenius 로 모든 파티션 삭제 → 새 파티션(기본값) → 변경 저장 → balenaEtcher 로 이미지(myAGVPro_v1.0.0_250716) 굽기 → SSD 를 다시 꽂고 HDMI 모니터 · 19 V 어댑터로 부팅 → cat /etc/version 확인. 굽기 전에 ~/agv_pro_ros2/src/agv_pro_navigation2/map/ 의 지도와 바꾼 설정 파일을 반드시 백업합니다.

과제#

  1. 노드 그래프 지도: 실물 실습 1 에서 ros2 node list · ros2 topic list -t · ros2 service list -t 결과를 모아, 1교시 그림처럼 노드 · 토픽 · 서비스 그래프를 직접 그립니다(rqt_graph 화면과 비교). namespace 를 줬을 때 바뀌는 이름에 표시합니다.
  2. 진단 카드 늘리기: 실습 2-1 에 이 차시 "자주 나는 오류" 3 · 6 · 7 · 9 를 카드로 추가하고, 내가 실제로 만난 오류 하나를 원문 그대로 카드로 만듭니다. 규칙이 서로 충돌하지 않는지 확인합니다.
  3. 순회 시간표: 실습 3-2 로 네 점의 모든 방문 순서(입고장 처음 · 출고장 마지막 고정 → 2 가지)와 세 액션(시뮬레이터 3)의 시간을 표로 만들고, 실습실 실물 결과 한 줄을 더합니다. 예측과 실물의 차이를 원인별(회전 · 가감속 · 복구)로 나눕니다.
  4. 충전 정책 한 장: 실습 4-1 로 "미션 길이 600 · 1 200 · 2 400 m × 소모율 10 · 20 %/h" 의 최소 임계값 표를 만들고, 우리 현장(가정)에 쓸 임계값 또는 예측 규칙을 근거와 함께 한 문단으로 씁니다. 이 규칙은 ch10 의 미션 관리자에 그대로 들어갑니다.

참고자료#

  • myAGV Pro 공식 문서: https://docs.elephantrobotics.com/docs/myAGV_Pro_en/
    • 6.2.1 ROS2 소개 (빌드 · bringup · ROS_DOMAIN_ID): https://docs.elephantrobotics.com/docs/myAGV_Pro_en/6-SDKDevelopment/6.2-ApplicationBaseROS2/6.2.1-ROS2_Introduction.html
    • 6.2.3 Navigation2: https://docs.elephantrobotics.com/docs/myAGV_Pro_en/6-SDKDevelopment/6.2-ApplicationBaseROS2/6.2.3-Navigation2.html
    • 6.2.4 다중 AGV Pro 제어: https://docs.elephantrobotics.com/docs/myAGV_Pro_en/6-SDKDevelopment/6.2-ApplicationBaseROS2/6.2.4-multi_control.html
    • 6.2.5 Gazebo: https://docs.elephantrobotics.com/docs/myAGV_Pro_en/6-SDKDevelopment/6.2-ApplicationBaseROS2/6.2.5-Gazebo.html
    • 6.2.8 Autocharge: https://docs.elephantrobotics.com/docs/myAGV_Pro_en/6-SDKDevelopment/6.2-ApplicationBaseROS2/6.2.8-Autocharge.html
  • myAGV Pro ROS 2 저장소 (humble): https://github.com/elephantrobotics/agv_pro_ros2
    • agv_pro_base/src/agv_pro_ros.cpp (토픽 · 서비스 · 50 Hz 루프 · 전원 오류 코드), agv_pro_bringup/launch/agv_pro_bringup.launch.py, agv_pro_msgs/srv/*.srv, agv_pro_navigation2/launch/navigation2_active.launch.py, agv_pro_navigation2/scripts/example_*.py, agv_pro_autocharge/agv_pro_autocharge/combined_auto_recharger.py
  • 제품 페이지: https://www.elephantrobotics.com/en/myagv-pro/
  • 제품 소개 · 이미지 (현영이앤씨): https://www.hyenc.net/products/36
  • ROS 2 Humble 개념 (노드 · 토픽 · 서비스 · 액션): https://docs.ros.org/en/humble/Concepts/Basic.html
  • ROS 2 도메인 ID: https://docs.ros.org/en/humble/Concepts/Intermediate/About-Domain-ID.html
  • ROS 2 토픽 이름 규칙 (절대 · 상대 이름, namespace): https://design.ros2.org/articles/topic_and_service_names.html
  • colcon 빌드 튜토리얼: https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Colcon-Tutorial.html
  • Nav2 Simple Commander (BasicNavigator API): https://docs.nav2.org/commander_api/index.html
  • Nav2 Waypoint Follower 설정: https://docs.nav2.org/configuration/packages/configuring-waypoint-follower.html
  • nav2_msgs 액션 정의 (NavigateToPose · NavigateThroughPoses · FollowWaypoints): https://github.com/ros-navigation/navigation2/tree/humble/nav2_msgs/action
  • teleop_twist_keyboard: https://github.com/ros2/teleop_twist_keyboard
  • 이 사이트의 도구: Playground · 물류창고 시뮬레이터 · 3D 뷰어