fori_zltech_motor_test/doc/07-lidar-slam-plan.md

07. 2.5D 맵 기반 자율주행 — 현재 상태 정리와 다음 단계 (Phase 6)

문서 성격 변경: 이전 버전은 "계획안, 코드 미착수"를 전제로 썼다. 그러나 실제로는 별도 워크스페이스 ~/fori_ws에 이 문서가 계획하려던 내용의 상당 부분이 이미 구현되어 동작 중이다(§7.0). 그래서 이 문서는 더 이상 "무엇을 어떻게 새로 만들 것인가"가 아니라 **"무엇이 이미 있고, 무엇이 검증되지 않았고, 무엇을 다음에 해야 하는가"**를 정리하는 문서로 성격을 바꾼다. 06번 문서의 RC 통합 작업과는 독립적으로 진행된 별도 트랙이었다는 점에 유의한다.

7.0 두 저장소의 관계 — 왜 이 문서가 실제와 어긋나 있었는가

저장소	역할	상태
fori_zltech_motor_test(이 레포)	RC 수동조종(Xbox/CRSF) + ZLAC8015D Modbus RTU 프로토콜을 CRC 검증까지 포함해 실기체에서 검증하는 테스트대. ROS2 없이 단일 C++ 프로그램(xbox_motor_control.cpp)으로 동작.	계속 유지·개발 중(06번 문서 Phase 1~5a)
~/fori_ws	ROS2 Humble 기반 완전한 자율주행 통합 스택. livox_ros_driver2, FAST-LIO(LIO), Nav2, 커스텀 시리얼 브릿지, 웹 대시보드까지 포함.	이미 상당 부분 구현·통합 완료, 실기체 검증은 일부만 진행

~/fori_ws/fori_ws/src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp 상단 주석:

"only the low-level serial transport is replaced with the CRC-validated, short-poll implementation verified on the real robot in zlac8015d_motor_test/fori_zltech_motor_test/xbox_motor_control.cpp."

즉 두 저장소는 이미 실제로 연결된 형제 프로젝트다 — 이 레포에서 검증한 Modbus 프로토콜을 fori_ws가 가져다 ROS2 노드로 감쌌다. 이 문서(07번)는 그 사실을 모른 채 "URDF부터 새로 작성해야 한다"는 전제로 쓰여 있었으므로, 이번에 전면 수정한다.

7.1 사용자가 요청한 컨셉과 실제 구현 대조

사용자가 이번에 요청한 자율주행 방향 — 이미 가진 2.5D 맵을 활용, 고도 정보로 오르막/내리막 속도 조절, Mid-360S 그대로 사용, pcd_gunsan_output 맵 활용 — 은 다음과 같이 이미 구현되어 있다.

요청 사항	구현 상태	근거 파일
2.5D 맵(포인트별 고도 포함)	구현됨. pcd_gunsan.pcd(9,791,311포인트, RGB 컬러 포함, ~1.05GB)를 pcd_gridmap_converter가 표준 Nav2 점유격자(map.pgm/yaml)와 별도 고도 그레이스케일 맵(elevation_map.pgm/yaml)으로 변환	~/pcd_gridmap_converter/src/pcd_to_gridmap_node.cpp, 출력 ~/pcd_gunsan_output/
오르막/내리막 속도 조절	구현됨. TerrainSpeedNode가 현재 위치+진행방향 0.5m 앞의 고도차(grade)를 계산해 오르막은 부스트(최대 1.3배), 내리막은 감속(최소 0.5배), EMA로 스무딩	~/fori_ws/fori_ws/src/fori_serial_bridge/fori_serial_bridge/terrain_speed_node.py
Mid-360S 유지	그대로 사용. livox_ros_driver2가 /livox/lidar, /livox/imu 발행	~/fori_ws/fori_ws/src/livox_ros_driver2/
pcd_gunsan_output 맵 활용	이미 이 경로를 Nav2 map_server와 terrain_speed_node가 직접 참조 중. 사용자가 언급한 /home/pcd_gunsan_output은 실제로는 /home/yoo/pcd_gunsan_output이다(확인 완료)	fori_nav2_params.yaml:3, fori_full.launch.py:18-21

elevation_map.pgm 디코딩 방식(참고용): 픽셀 0=미상, 그 외에는 z = min_elevation + (pixel-1)/254*(max_elevation-min_elevation)로 선형 복원. 현재 저장된 범위는 -1.99m ~ 3.00m(elevation_map.yaml). 이 범위가 실제 지형 고도차를 다 담고 있는지는 Z 관심영역(ROI) 파라미터(min_height/max_height)에 달려 있는데, 과거 한 번 이 값이 필터 경계값과 정확히 일치해 잘렸을(clipping) 가능성이 제기되어 더 넓은 범위로 재변환한 이력이 있다 — §7.5 Phase 2에서 재확인이 필요하다.

7.2 판단 1 — "URDF 먼저, 그 다음 ros2_control" 순서가 맞는가

결론: URDF 부분은 맞고 이미 있다. ros2_control은 불필요하다 — 이미 더 가벼운 대안으로 대체되어 잘 동작 중이다.

URDF: 있음, 다만 표준 패턴과 다른 구조

~/fori_ws/fori_ws/src/FAST-LIVO2/urdf/fori_robot.urdf에 Mid-360 라이다, Hikrobot 카메라, 4바퀴(continuous joint)가 실측 치수로 정의되어 있다. 다만 **루트 링크가 base_link가 아니라 aft_mapped**다 — FAST-LIO(LIO SLAM)가 런타임에 camera_init → aft_mapped TF를 동적으로 발행하고, base_link는 그 아래에 고정 조인트로 매달린 구조(aft_mapped_to_base, xyz="-0.26953 0 -0.41296" rpy="0 -0.277309 0", 라이다 실측 피치 보정 포함)다.

이건 표준 로봇공학 URDF 패턴(고정된 base_link를 루트로 두고 오도메트리가 odom→base_link를 발행)과 다르다 — 대신 "라이다 SLAM이 로봇의 기준 좌표계 자체를 소유하고, 로봇 형상은 그 자세를 따라간다"는 구조다. 이 선택 자체는 실용적이다(LIO가 어차피 가장 정확한 자세 추정원이므로). 다만 §7.4에서 다루는 TF 트리 충돌 문제의 근본 원인이기도 하다.

ros2_control: 불필요 — 더 가벼운 직접 시리얼 브릿지가 이미 그 자리를 대신함

이전 버전 문서는 hardware_interface::SystemInterface를 구현하는 ros2_control 플러그인을 써야 한다고 했다. 실제로는 그렇게 하지 않았다. 대신 fori_serial_bridge_cpp::ForiSerialBridge (일반 rclcpp::Node)가:

• /cmd_vel(Twist)을 직접 구독
• 20Hz 제어 루프에서 가감속 램프 + IMU 요레이트 PI 보정 + 차동 킨매틱스 → 좌우 RPM 산출
• ZLAC8015D에 RS485 Modbus RTU로 직접 명령(레지스터 0x2088 속도 쓰기, 0x20AD 피드백 읽기)
• /odom_wheels, /joint_states, /battery_state 발행

이 구조로 이미 Nav2와 통합되어 동작 중이다(serial_bridge_node.cpp:112-134). ros2_control의 핵심 이점은 (1) diff_drive_controller 같은 표준 컨트롤러 재사용, (2) controller_manager의 표준 생명주기 관리인데, 이 로봇은 애초에 4륜 스킷스티어라 표준 컨트롤러가 정확히 맞지 않고(이전 버전 문서도 §7.4.4에서 이 점을 지적했다), 커스텀 로직(가감속 램프, IMU PI 보정, 배터리 필터링, 모의주행 폴백)이 필요해 결국 커스텀 코드를 짜야 하는 건 ros2_control을 쓰든 안 쓰든 마찬가지다. 지금 상태에서 ros2_control로 다시 감싸는 건 실질적 이득 없이 이미 검증된 경로를 재작성하는 것이므로 우선순위가 낮다. 유지를 권장한다.

다만 놓친 부분 하나 — 스킷스티어 캘리브레이션 미이식

fori_serial_bridge_cpp의 킨매틱스는 순수 기하학적 차동구동 공식을 쓴다 (v_left = v - w*wheel_base/2, wheel_base=0.374m, serial_bridge_node.cpp:344-346). 반면 이 레포의 xbox_motor_control.cpp는 06번 문서 Phase 5a에서 실측으로 캘리브레이션한 effective_w=0.87, k_skid=0.51(xbox_motor_control.cpp:1449,1459)을 쓴다 — 이 값들은 스킷스티어 로봇의 실제 타이어-지면 마찰로 인한 유효 회전반경이 순수 기하학적 트랙폭(0.374m)과 다르다는 걸 실측으로 확인한 결과다. fori_ws의 회전 킨매틱스는 이 캘리브레이션을 이식받지 못했다 — 즉 Nav2가 "이만큼 회전해라"라고 지시했을 때 실제 회전량이 계산값과 얼마나 다른지가 검증되지 않은 상태다. 이건 §7.5 Phase 1의 작업 항목으로 넣는다.

7.3 판단 2 — Nav2 글로벌맵 + 로컬맵 기반 동적 장애물 회피

결론: 맞고, 이미 사용자가 제안한 것보다 한 단계 더 나아간 3중 구조로 구현되어 있다.

계층	역할	데이터 소스	근거
global_costmap	정적 전역 지도(static_layer) + 관측된 장애물(obstacle_layer)로 전역 경로 계획	map 토픽(사전 구축 맵) + /scan	fori_nav2_params.yaml:104-127
local_costmap	로봇 주변 3×3m 롤링 윈도우, 실시간 장애물만 반영해 국소 회피	/scan(라이다 실시간)	fori_nav2_params.yaml:75-102
Collision Monitor(추가 안전계층)	Nav2 코스트맵과 별개로 최종 속도 명령 단계에서 급출현 장애물을 감속(SlowZone)·정지(StopZone)·예측정지(FootprintApproach)시키는 마지막 게이트	/scan	fori_collision_monitor_params.yaml

Collision Monitor는 terrain_speed_node가 낸 /cmd_vel_terrain(경사 보정까지 끝난 명령)을 받아 최종 /cmd_vel을 낸다 — 즉 실제 파이프라인은 Nav2 계획 → 경사 보정 → 급출현 장애물 감속/정지 → 실제 모터(§7.5 아키텍처 그림 참고) 순서로, 사용자가 제안한 "글로벌은 지도로, 로컬은 라이다로" 구조를 그대로 따르면서 한 겹의 실시간 안전장치를 더 두고 있다.

알려진 한계 하나: Nav2의 behavior_server(막힘 시 후진/제자리회전 등 복구 동작)가 내는 속도 명령은 nav2_bringup 내부 리맵 구조상 velocity_smoother를 거치지 않아 terrain_speed_node의 경사 보정과 Collision Monitor의 급출현 감속·정지를 우회한다 (fori_nav2.launch.py:128-137 주석에 상세 근거). 다만 복구 동작 자체가 저속·단거리로 제한되고 serial_bridge_node의 target_linear_speed(0.3m/s) 하드 클램프는 경로와 무관하게 항상 적용되므로 위험은 제한적이지만, 완전히 무시할 문제는 아니다.

7.4 질문에 대한 답

Q1. 저장된 맵을 제공하고 그 맵으로 Nav2를 진행해도 실시간 자세 추종이 가능한가?

가능하다. "SLAM(지도를 만들면서 동시에 자세를 추정)"과 "Localization(이미 있는 지도 안에서 자세만 추정)"은 다른 문제다. 후자는 지도를 새로 만들 필요가 없어 계산량이 훨씬 가볍고, 이게 정확히 Nav2의 AMCL(Adaptive Monte Carlo Localization)이 하는 일이다. fori_ws에는 이미 이 구조가 설정되어 있다:

• map_server가 pcd_gunsan_output/map.yaml(사전 구축된 정적 지도)을 서빙
• amcl이 scan_topic: "scan"을 구독 — Mid-360의 3D 포인트클라우드를 pointcloud_to_laserscan으로 2D로 투영한 것(fori_nav2.launch.py:88-103)
• AMCL은 파티클 필터로 이 2D 스캔을 정적 지도와 계속 대조해 /amcl_pose(map 프레임 기준 위치)를 실시간으로 발행

즉 SLAM처럼 지도를 다시 만들 필요 없이, 저장된 지도 하나만 있으면 실시간 위치 추정이 가능 하다는 것이 맞다.

Q2. 그렇다면 라이다로 주행 중 저장된 맵과 계속 대조하며 자세를 추종하는 것인가?

맞다 — 그게 AMCL이 하는 정확한 방식(스캔매칭 기반 파티클 필터)이다. 다만 fori_ws의 실제 구현에는 중요한 예외가 있고, 이게 지금 이 시스템의 가장 중요한 미해결 갭이다.

fori_nav2_params.yaml:25에서 AMCL의 tf_broadcast: false로 꺼져 있다. 이유는 코드 주석에 명시되어 있다:

FAST-LIO(LIDAR SLAM)가 camera_init→aft_mapped→base_link TF를 이미 발행하고 있는데, AMCL이 표준대로 map→odom을 또 발행하면 odom의 부모를 두 곳(FAST-LIO의 정적 브릿지 vs AMCL)이 동시에 주장하게 되어 base_link↔odom 연결을 tf2가 못 찾는 "unconnected trees" 오류가 난다.

그래서 실제로는 camera_init을 odom과 map 양쪽에 정적(변화 없는 identity transform) 으로 동시에 연결해 놓았다(static_tf_camera_init_node, static_tf_camera_init_to_map_node, 둘 다 fori_nav2.launch.py:48-70). 결과적으로:

• 실제 항법(Nav2 코스트맵/플래너가 보는 "map" 프레임)은 FAST-LIO가 만드는 연속적인 LiDAR-관성 오도메트리(LIO) 그 자체다 — 이건 저장된 지도와 무관하게, 매 순간 이전 프레임 대비 상대 이동만 누적하는 방식이라 시간이 지나면 드리프트가 쌓인다.
• AMCL은 여전히 돌고 있고 /amcl_pose(진짜 저장된 지도와 스캔매칭한 결과)를 정상 발행하지만, 이 값은 terrain_speed_node/parking_controller_node가 위치 조회용으로 참고만 할 뿐, Nav2의 TF 트리·코스트맵에는 재반영되지 않는다.

정리하면: 지금은 "저장된 맵과 계속 대조해서 드리프트를 실제로 보정하는" 루프가 완성되어 있지 않다. AMCL은 돌고 있지만 보조적 위치 확인 용도이고, 로봇이 실제로 어디 있다고 "믿고" 주행하는 좌표계는 FAST-LIO의 순수 오도메트리다. 짧은 거리·짧은 시간의 순찰(사람이 대기하며 감독하는 Phase 4~5 수준)에서는 드리프트가 실용적으로 무시할 만하겠지만, 주행 거리·시간이 늘어날수록 "로봇이 믿는 위치"와 "저장된 지도 위 실제 위치"가 벌어질 위험이 있다 — 이건 §7.1에서 확인한 사용자의 요청(저장된 2.5D 맵을 활용한 자율주행)의 정확도에 직접 영향을 준다. 이 TF 충돌을 풀어 AMCL의 보정이 실제로 항법 좌표계에 반영되게 하는 것을 §7.5 Phase 1의 최우선 항목으로 둔다.

7.5 Phase — 남은 작업 (기존 코드 근거 기반으로 재정의)

이전 버전 문서의 Phase 13(인프라 확인/실내 SLAM 검증/야외 검증)은 fori_ws에서 이미 그 이상으로 진행되어 있으므로 폐기한다. 아래는 §7.27.4에서 확인한 구체적 갭을 기준으로 다시 짠 순서다. 06번 문서와 같은 원칙 — 먼저 관측/검증, 그 다음에만 주행 제어 개입 — 을 유지한다.

Phase 1 — 자세 추종 정확도 갭 해소 (최우선, 주행 제어 영향 큼)

• AMCL↔FAST-LIO TF 통합: unconnected trees 문제를 임시 봉합(정적 aliasing)이 아니라 실제로 풀어, AMCL의 스캔매칭 보정이 map→odom 형태로든 다른 형태로든 Nav2가 실제로 쓰는 좌표계에 반영되게 한다. 후보: (a) FAST-LIO 쪽에서 aft_mapped를 odom으로 두고 AMCL이 표준대로 map→odom을 발행하도록 프레임 이름 재배선, (b) robot_localization의 navsat_transform 유사 패턴으로 AMCL 보정치를 주기적으로 오도메트리에 융합. §7.4 Q2에서 확인한 현재 구조(정적 aliasing)로는 저장된 지도 대비 실제 위치 보정이 이름만 있고 실질적으로 동작하지 않는다.
• 스킷스티어 캘리브레이션 이식: §7.2에서 확인한 k_skid=0.51/effective_w=0.87(06번 문서 Phase 5a 실측값)을 fori_serial_bridge_cpp의 회전 킨매틱스에 반영. 지금은 순수 기하학적 트랙폭(0.374m)만 쓰고 있어 Nav2가 지시한 회전량과 실제 회전량이 얼마나 다른지 검증되지 않았다.
• 게이트: 위 두 항목을 고친 뒤, 로봇을 손으로/저속으로 움직이며 /amcl_pose와 로봇의 실제 위치(줄자·표식 등 외부 기준)를 비교해 보정이 실제로 드리프트를 줄이는지 확인해야 다음 단계로 넘어간다.

Phase 2 — 맵/웨이포인트 데이터 검증

• 고도맵 ROI 재확인: elevation_map.yaml의 min_elevation/max_elevation이 실제 군산 부지의 지형 고도차를 다 담고 있는지(§7.1에서 언급한 과거 클리핑 이력) pcd_to_gridmap_node의 min_height/max_height 파라미터를 재확인하고 필요하면 넓혀서 재변환한다.
• 순찰 웨이포인트 설정: parking_controller_node의 patrol_waypoints가 현재 빈 배열이다 (예전 실내 지도 좌표는 새 군산 부지에 안 맞아 이미 비워둔 상태 — parking_controller_node.py:47 주석 참고). rviz2로 새 지도를 열어 순찰 좌표를 실측/지정한다.
• 게이트: rviz에서 라이다 스캔이 정적 지도와 정렬되어 보이고, 지정한 웨이포인트가 지도상 주행 가능 영역(occupancy=free) 안에 있는지 확인.

Phase 3 — 통제 구역 저속 폐루프 테스트

• 울타리 등으로 막힌 좁은 구역, 저속, 사람이 항시 대기하며 즉시 개입 가능한 상태에서 Phase 1~2 적용 후 첫 자율주행 테스트.
• 오르막/내리막 구간을 포함해 terrain_speed_node의 속도 보정이 실제로 등판력 확보/안전한 서행으로 이어지는지 확인(§7.4의 게인 파라미터 uphill_gain=3.0/downhill_gain=4.0이 이 로봇 중량·모터 토크에 적절한지는 아직 실측 검증 전).
• 06번 문서 Phase 4(전신 슬립 감지)가 이 자율주행 경로 위에서도 살아있는지 — 즉 fori_serial_bridge_cpp로 옮겨 갔는지, 아니면 아직 xbox_motor_control.cpp 에만 있는지부터 확인해야 한다. 현재 serial_bridge_node.cpp를 보면 슬립 감지·온도 모니터링 로직은 이식되지 않은 것으로 보인다 — 이식 여부를 이 Phase에서 명시적으로 판단한다.
• 게이트: 짧은 구간 반복 검증이 안정적이어야 범위를 넓힌다.

Phase 4 — 범위 확장 + 동적 장애물 실지 검증

• 더 길고 개방된 구역, 사람/물체가 실제로 튀어나오는 상황에서 Collision Monitor의 StopZone(0.45m)/SlowZone(0.90m)/FootprintApproach(1.5초 예측)가 의도대로 동작하는지 확인.
• §7.3에서 확인한 behavior_server 복구 동작의 안전장치 우회 한계가 실제로 문제되는지(복구가 얼마나 자주 트리거되는지) 관찰.

Phase 5 — (낮은 우선순위, 보류) ros2_control 전환 재검토

§7.2에서 판단했듯 지금은 불필요하다. 다만 이후 표준 ROS2 컨트롤러 생태계(예: 다른 로봇과 코드 공유, moveit류 확장 등) 활용 필요성이 실제로 생기면 그때 재검토한다 — 지금 시점에 선제적으로 할 이유는 없다.

7.6 요약 — 이번 조사에서 바뀐 것

• 이전 문서는 "무엇을 어떻게 새로 만들 것인가"를 물었지만, 실제로는 이미 만들어져 있었다. URDF, 2.5D 맵 변환, 경사 기반 속도 조절, Nav2 글로벌/로컬 코스트맵 + Collision Monitor까지 전부 ~/fori_ws에 구현되어 있다.
• ros2_control은 필요 없다 — 더 가벼운 직접 시리얼 브릿지가 이미 그 역할을 하고 있고, 지금 다시 쓰는 건 순수 재작업이다.
• 가장 중요한 실질적 갭은 두 가지다: (1) AMCL의 지도 대조 보정이 TF 충돌 회피용 임시방편 때문에 실제 항법 좌표계에 반영되지 않고 있다는 것(§7.4 Q2), (2) 06번 문서에서 실측 캘리브레이션한 스킷스티어 보정값(k_skid/effective_w)이 ROS2 스택의 킨매틱스에 이식되지 않았다는 것(§7.2). 이 둘이 Phase 1의 핵심이다.