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

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

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

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

| 저장소 | 역할 | 상태 |
| :--- | :--- | :--- |
| `fori_zltech_motor_test`(이 레포) | RC 수동조종(Xbox/CRSF) + ZLAC8015D **Modbus RTU 프로토콜을 CRC 검증까지 포함해 실기체에서 검증**하는 테스트대. ROS2 없이 단일 C++ 프로그램(`xbox_motor_control.cpp`)으로 동작. | 계속 유지·개발 중([06번 문서](06-imu-integration-plan.md) 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.374`m, `serial_bridge_node.cpp:344-346`). 반면
이 레포의 `xbox_motor_control.cpp`는 [06번 문서](06-imu-integration-plan.md) 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 1~3(인프라 확인/실내 SLAM 검증/야외 검증)은 `fori_ws`에서 이미 그
이상으로 진행되어 있으므로 폐기한다. 아래는 §7.2~7.4에서 확인한 **구체적 갭**을 기준으로 다시
짠 순서다. [06번 문서](06-imu-integration-plan.md)와 같은 원칙 — 먼저 관측/검증, 그 다음에만
주행 제어 개입 — 을 유지한다.

### 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번 문서](06-imu-integration-plan.md) 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의 핵심이다.