Add LIDAR/SLAM plan and improvements report docs

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
2026-08-24 16:00:39 +09:00
parent 9a97cf9022
commit 6350661c66
3 changed files with 416 additions and 1 deletions
@@ -0,0 +1,207 @@
+# 07. Livox Mid-360S 기반 LiDAR SLAM 도입 계획 (Phase 6, 실행 전 검토용)
+
+> **이 문서는 계획안이다. 코드는 아직 수정하지 않았다.** [06-imu-integration-plan.md](06-imu-integration-plan.md)
+> §Phase 6("장기, 선택")에서 예고된 자율주행 확장을 구체화한 문서로, 아래 내용을 검토하고 방향에
+> 동의하면 그때 실제 작업(ROS2 설치, 패키지 빌드, URDF/ros2_control 작성, 코드 통합)을 시작한다.
+
+## 7.1 현재 상태 (코드 근거)
+
+로봇에는 Livox Mid-360S가 이미 장착되어 있고 `xbox_motor_control.cpp`가 Livox SDK2
+(`livox_lidar_api.h`)로 직접 연동하고 있지만, 이건 **반응형 장애물 회피 전용**이지 SLAM이 아니다.
+
+`LidarObstacleDetector::handlePointCloud()`(`xbox_motor_control.cpp:532`)는 패킷으로 들어오는
+포인트를 하나씩 받아 전방/후방/좌/우 4개 구역의 **최소 거리 스칼라값**으로 즉시 축약하고, 원본
+포인트는 저장하지 않은 채 버린다. 즉:
+
+| 항목 | 현재 | SLAM에 필요 |
+| :--- | :--- | :--- |
+| 포인트클라우드 | 수신 즉시 4개 거리값으로 축약 후 폐기 | 누적·저장 필요 |
+| 자세 추정(6-DOF) | 없음 (단순 거리 임계값 반응) | 매 스캔마다 위치·자세 추정 |
+| 지도 | 없음 | 포인트클라우드 지도 생성/관리 |
+| 루프 클로저 | 해당 없음 | 누적 드리프트 보정 필요 |
+| 로봇 모델(TF) | 없음 — ROS 개념 자체가 없는 단일 C++ 프로그램이라 `base_link`↔바퀴/센서 간 좌표 관계가 코드 상수로만 흩어져 있음 | URDF로 명시해 `robot_state_publisher`가 TF 트리 발행 — Nav2 costmap 풋프린트, RViz 정합, 센서 외부 파라미터(extrinsics)에 모두 필요 |
+| 속도명령 실행 인터페이스 | `setRPMs()` 직접 호출(자체 프로토콜) | Nav2가 내는 `/cmd_vel`을 표준화된 방식으로 받아 실제 바퀴 명령으로 변환하는 레이어 필요 |
+
+## 7.2 Mid-360S는 SLAM에 유리한 센서
+
+Mid-360S는 논-리피티티브(non-repetitive) 스캔 방식 솔리드스테이트 라이다로, Livox가 이 시리즈
+(Mid-360/Avia 등) 전용으로 **FAST-LIO2**, **Point-LIO** 같은 LiDAR-Inertial Odometry(LIO) SLAM
+패키지를 공식 지원·공개하고 있다. 지금 쓰는 SDK2도 이미 이 생태계의 표준이라 하드웨어/펌웨어
+호환성 문제는 적을 것으로 예상된다.
+
+**주의**: Mid-360S 내부에는 **자체 IMU가 내장**되어 있고 LIO 패키지들은 보통 이 내장
+IMU(라이다와 시간 동기화됨)를 쓴다. [06번 문서](06-imu-integration-plan.md)에서 연동한 외부
+WitMotion HWT905와는 별개의 센서이니 혼동하지 않는다.
+
+## 7.3 권장 스택
+
+* **ROS2** + **livox_ros_driver2**(Livox 공식 드라이버, Mid-360S 정식 지원)
+* **FAST-LIO2** 또는 **Point-LIO** (Point-LIO가 상대적으로 최신/고속)
+* **URDF/xacro** — 휠베이스·트랙폭·휠 반경 등 차체 치수와 Mid-360S/WitMotion IMU 마운트 위치를
+  정의해 `robot_state_publisher`로 TF 트리를 발행. Nav2 costmap 풋프린트, RViz 시각화, 센서
+  외부 파라미터(extrinsics) 계산 등 이후 모든 단계의 기반이 된다.
+* **ros2_control** — Nav2가 내는 `/cmd_vel`을 이 로봇의 실제 4바퀴 RPM으로 바꾸는 실행 레이어를
+  ROS2 표준 프레임워크 안에서 구현(자세한 내용은 §7.4.4). 스캔매칭·루프클로저·지도관리를 직접
+  C++로 새로 구현하는 것과 달리, 이 실행 레이어는 이 로봇 고유의 하드웨어/킨매틱스 지식이 필요해
+  검증된 오픈소스로 대체할 수 없는 부분이다 — `xbox_motor_control.cpp`의 기존 로직을
+  ros2_control 플러그인 형태로 옮기는 작업이 된다.
+* 스캔매칭·루프클로저·지도관리를 직접 C++로 새로 구현하는 것은 권장하지 않는다 — 검증된
+  오픈소스 대비 얻는 이득이 적고, [06번 문서](06-imu-integration-plan.md) Phase 1~5a와는
+  작업 성격이 다른(이미 성숙한 알고리즘을 재발명하는) 리스크가 크다.
+
+## 7.4 통합 아키텍처 — 결정이 필요한 지점
+
+`xbox_motor_control.cpp`는 ROS 없이 라이다 SDK를 직접 붙잡는 단일 C++ 프로그램인데, SLAM
+스택은 보통 ROS2 기반이라 아래를 먼저 결정해야 한다.
+
+### 7.4.1 라이다 동시 접근 가능 여부
+
+Livox SDK2는 라이다가 UDP로 데이터를 뿌리는 방식이라, SDK 레벨에서 멀티 클라이언트가 되는지
+먼저 확인이 필요하다. 안 되면 SLAM 프로그램만 라이다를 잡고, `xbox_motor_control.cpp`의
+반응형 회피는 SLAM 노드가 만드는 로컬 코스트맵/장애물 토픽을 구독하는 구조로 바꿔야 한다.
+
+### 7.4.2 통합 방식 두 가지
+
+| 방식 | 설명 | 트레이드오프 |
+| :--- | :--- | :--- |
+| **(A) Nav2로 완전 이관** | ROS2 Navigation Stack이 경로계획+속도명령(`/cmd_vel`)을 전담. [06번 문서](06-imu-integration-plan.md) §6.5의 Husky 대응표가 가리키는 정석 구조. | 아래 §7.4.3 참고 — 전부 버려지는 게 아니라 레이어가 나뉜다. `/cmd_vel`→바퀴 RPM 실행 레이어는 §7.4.4의 URDF+ros2_control로 표준화해서 구현 |
+| **(B) 얇은 브릿지** | SLAM/Nav2는 지도 생성 + 웨이포인트 좌표만 내주고, `xbox_motor_control.cpp`는 지금 구조(RC + 자체 제어루프) 그대로 두고 웨이포인트를 받아 목표 방향으로 이동하는 자율주행 모드를 추가. | 웨이포인트↔RC급 명령 변환 브릿지 코드를 직접 작성해야 함. ROS2 표준 실행 레이어(ros2_control)를 쓰지 않으므로 URDF도 TF/RViz 정합 용도 외에는 필수가 아님 |
+
+### 7.4.3 (A)를 선택해도 지금까지의 제어 로직이 사라지지 않는 이유
+
+Nav2가 대체하는 건 "지도 위에서 어디로 가야 하는지 결정하는" **상위 계획 레이어**(경로 계획,
+웨이포인트 추종, 전역 장애물 회피 → `/cmd_vel` 생성)뿐이다. `/cmd_vel`(목표 v_x, omega)을 **이
+로봇의 특정 하드웨어에 맞는 실제 4바퀴 RPM으로 바꾸는 실행 레이어**는 Nav2가 전혀 모르는
+영역이라 (A)를 선택해도 계속 필요하다:
+
+* **Phase 5a(k_skid/effective_w 캘리브레이션)** — Nav2는 "이만큼 회전해라"만 던질 뿐, 이 로봇이
+  스킷스티어라 회전 시 차동을 얼마나 더 세게 걸어야 하는지는 모른다. 이 변환은 계속 우리 코드의
+  몫이다.
+* **저크 제한 감속 / 급가속 시 반전 방지** — 백EMF 보호, 급정지 시 바퀴 튀는 걸 막는 액추에이터
+  레벨 안전은 Nav2 담당이 아니다. 계속 필요.
+* **Phase 4(전신 슬립 감지), 걸림/들뜸 진단** — 전류·엔코더·IMU로 보는 바퀴 트랙션 이상은
+  라이다 기반 회피(Nav2 담당)와 완전히 다른 종류의 안전장치라 계속 필요.
+* **Phase 2(휠+IMU 융합 오도메트리)** — 오히려 Nav2/`robot_localization`이 정확히 이런 형태의
+  오도메트리 토픽을 입력으로 기대하므로, 없어지는 게 아니라 더 핵심적인 역할을 하게 된다.
+
+**실제로 역할이 애매해지는 건 Phase 3(헤딩홀드)** 정도다 — RC 수동조종 중 직진 편향 보정용인데,
+자율주행 중에는 Nav2의 로컬 플래너가 그 역할을 대신한다. 다만 RC 수동조종 모드를 비상/테스트용
+폴백으로 남겨둔다면(대부분의 실제 로봇이 그렇게 한다) 그 모드에서는 계속 쓰인다.
+
+정리하면 (A)를 선택해도 **RC 수동조종 관련 부분(CRSF 파싱, 헤딩홀드의 RC 트리거 조건)만
+자율주행 중엔 쓰이지 않는 정도**지, 킨매틱스/안전진단/오도메트리는 전부 계속 쓰인다.
+
+## 7.4.4 URDF + ros2_control로 실행 레이어 구현
+
+§7.4.3에서 정리한 "Nav2가 몰라도 계속 필요한" 로직들(Phase 5a 킨매틱스 캘리브레이션, 저크 제한
+S-curve, 전신 슬립 감지, 온도 모니터링)은 없어지지 않되, **`xbox_motor_control.cpp`라는 독립
+C++ 프로그램에서 ROS2 표준 실행 레이어인 ros2_control로 옮겨 담는** 재구성이 필요하다.
+
+* **URDF**: 휠베이스·트랙폭·휠 반경 등 실측 치수와 Mid-360S/WitMotion IMU 마운트 위치를 담아
+  `robot_state_publisher`가 TF 트리(`base_link` → 4개 바퀴 → 라이다/IMU)를 발행하게 한다. 이
+  URDF 안에 `<ros2_control>` 태그로 하드웨어 인터페이스를 선언하는 것이 표준 패턴이므로, URDF
+  작성과 ros2_control 플러그인 작성은 사실상 한 세트로 진행한다.
+* **ros2_control 하드웨어 인터페이스**: `hardware_interface::SystemInterface`를 구현하는
+  플러그인을 새로 작성해, 기존 `xbox_motor_control.cpp`가 갖고 있던 다음 로직을 그대로 이식한다
+  — ZLAC8015D Modbus 통신, [06번 문서](06-imu-integration-plan.md) Phase 5a의 `k_skid`/
+  `effective_w` 실측 캘리브레이션, 저크 제한 감속, 전신 슬립 감지, 모터/드라이버 온도 모니터링.
+  즉 **하드웨어와 직접 통신하는 코드는 거의 그대로 재사용**하고, 그걸 감싸는 인터페이스만
+  ros2_control 표준(`read()`/`write()` 사이클)에 맞춘다.
+* **컨트롤러 선택**: 이 로봇은 4륜 스킷스티어라 ROS2 표준 컨트롤러 중 정확히 맞는 것이 없다.
+  좌우 바퀴 쌍을 각각 하나의 가상 바퀴로 묶어 `diff_drive_controller`를 쓰는 방법과, 스킷스티어
+  전용 커스텀 컨트롤러를 작성하는 방법 중 하나를 골라야 한다 — `diff_drive_controller` 재사용
+  쪽이 구현 비용은 적지만, `k_skid`/`effective_w` 보정이 컨트롤러 레벨(바퀴 속도 변환)과
+  하드웨어 인터페이스 레벨(Phase 5a 캘리브레이션 자체) 중 어디서 적용돼야 하는지 먼저 정리해야
+  이중 보정 같은 실수를 피할 수 있다.
+* **RC 수동조종과의 공존**: CRSF 파싱과 헤딩홀드는 §7.4.3에서 확인했듯 계속 필요하므로, 이
+  부분은 ros2_control 밖(별도 ROS2 노드 또는 하드웨어 인터페이스 내 폴백 모드)에 남기고, RC
+  브레이크(CH5)가 눌리면 ros2_control 경유 명령보다 우선하도록 안전 우선순위를 명확히 설계한다.
+* 이 작업은 Phase 4(§7.5)에서 (A)를 선택했을 때 실제로 손대는 코드이며, 아직 SLAM/Nav2 경로
+  판단이 검증되지 않은 Phase 1~3 단계에서는 시작하지 않는다 — §7.5의 "먼저 관측으로 검증 후
+  개입" 원칙을 여기에도 그대로 적용한다.
+
+## 7.5 단계적 진행 순서 (Phase 별)
+
+[06번 문서](06-imu-integration-plan.md)와 같은 원칙을 따른다 — **먼저 순수 관측으로 검증하고,
+주행 제어에는 그 검증이 끝난 뒤에만 개입한다.** 각 Phase는 이전 Phase의 검증 게이트를 통과해야
+다음으로 넘어간다.
+
+### Phase 1 — 인프라 확인 (관측 없음, 설치/연결 확인만)
+
+* Livox SDK2가 멀티 클라이언트를 지원하는지 확인(§7.4.1) — 지원 안 되면 `xbox_motor_control.cpp`의
+  기존 반응형 회피(`LidarObstacleDetector`)와 라이다를 동시에 잡을 수 없다는 뜻이므로, 이후
+  아키텍처(§7.4.2의 A/B 선택, 회피 로직 재구성 여부)에 직접 영향을 준다. **가장 먼저 확인해야
+  나머지 계획이 헛수고가 되지 않는다.**
+* ROS2 설치 + `livox_ros_driver2` 빌드, Mid-360S 데이터가 ROS2 토픽(`/livox/lidar`,
+  `/livox/imu`)으로 정상 발행되는지 확인. 아직 SLAM 패키지는 안 돌린다 — 순수 드라이버
+  연결 확인 단계.
+* **URDF(xacro) 작성**: 휠베이스·트랙폭·휠 반경 등 실측 치수와 Mid-360S/WitMotion IMU 마운트
+  위치를 반영해 `robot_state_publisher`가 TF 트리(`base_link` → 바퀴/센서)를 발행하도록 한다.
+  아직 ros2_control 태그는 비워두거나 mock 하드웨어로 둔다 — 이 단계에서는 모터 제어와
+  전혀 연결하지 않는다. §7.4.4 참고.
+* **게이트**: `ros2 topic hz`/`rviz2`로 포인트클라우드가 끊김 없이 정상 주파수로 들어오고,
+  내장 IMU 토픽도 같이 나오는 것을 확인해야 다음 단계로 넘어간다. RViz에서 라이다 포인트가
+  URDF 로봇 모델과 정렬되어 보이는지(TF 트리 정합성)도 함께 확인한다.
+
+### Phase 2 — 실내/정적 SLAM 검증 (주행 제어 완전히 미개입)
+
+* FAST-LIO2 또는 Point-LIO를 빌드해, **로봇을 손으로 들거나 실내에서 천천히 밀며** 지도가
+  쌓이는지 확인한다. 아직 로봇의 모터 제어와는 전혀 연결하지 않는다 — 라이다+SLAM 노드만
+  독립적으로 돌리는 단계.
+* 확인 항목: 지도 품질(벽/기둥 등 기준점이 흐트러지지 않는지), 제자리로 돌아왔을 때 루프
+  클로저가 잘 닫히는지, 실시간 처리 속도가 로봇 탑재 컴퓨팅 자원으로 감당되는지.
+* **게이트**: 통제된 실내 환경에서 지도/루프클로저가 안정적으로 나와야 다음 단계로 넘어간다.
+  여기서 불안정하면 그 상태로 야외에 나가봐야 결과가 더 나빠질 뿐이다.
+
+### Phase 3 — 로봇 탑재 야외 검증 (여전히 주행 제어 미개입, RC 수동조종과 병행 관측)
+
+* 실제 로봇에 SLAM 노드를 올리고, **RC로 수동 조종하며** 지금까지 테스트해온 실야외 노면(파인
+  보도블럭/아스팔트/잔디)에서 지도/자세추정이 안정적인지 확인한다. 실내 벤치마크보다 훨씬
+  가혹한 환경(직사광선 반사, 개활지라 루프클로저용 기준점이 적음, 진동)이라 별도 검증이
+  필요하다.
+* [06번 문서](06-imu-integration-plan.md) Phase 2에서 만든 **휠+IMU 융합 오도메트리(x, y,
+  fused_yaw)를 대조군으로 병행 로깅**해 SLAM 결과와 비교한다 — 이미 두 소스(엔코더, WitMotion
+  IMU)로 한 번 검증해본 경험이 있으니, 세 번째 소스(라이다 SLAM)까지 서로 얼마나 일치하는지
+  교차검증하면 어느 쪽이 이 환경에서 더 신뢰할 만한지 판단할 근거가 생긴다.
+* **게이트**: 야외 실주행 중 SLAM 드리프트가 왕복 경로 기준으로 실용적인 수준(예: 수십 미터
+  주행에 수십 cm 이내)인지 확인해야 다음 단계로 넘어간다.
+
+### Phase 4 — 아키텍처 결정 + 저위험 프로토타입 (모터 제어에 처음으로 연결, 자동 개입은 없음)
+
+* §7.4.2의 (A) Nav2 완전 이관 / (B) 얇은 브릿지 중 방향을 확정한다.
+* (A)라면: Nav2 스택(costmap, 플래너)을 세팅하고, §7.4.4에서 설명한 **ros2_control 하드웨어
+  인터페이스(`hardware_interface::SystemInterface`)를 작성**해 `xbox_motor_control.cpp`의
+  ZLAC8015D 통신/킨매틱스/안전로직을 이식하고 URDF의 `<ros2_control>` 태그로 선언한다. 컨트롤러
+  (`diff_drive_controller` 재사용 또는 커스텀 스킷스티어 컨트롤러)까지 붙이되, **경로/속도명령이
+  실제로 잘 나오는지만 확인** — 아직 ros2_control 노드가 실제 모터로 명령을 내보내지 않도록
+  하드웨어 인터페이스 안에서 출력을 로그로만 찍고, RViz 등에서 시각적으로만 검증.
+* (B)라면: 웨이포인트를 받아 방향/거리를 계산하는 브릿지 코드를 작성하되, 처음엔 **콘솔에
+  "이 방향으로 가라"만 출력**하고 실제 `setRPMs()` 호출은 아직 연결하지 않는다. 이 경로는
+  ros2_control 없이도 진행 가능하다(§7.4.2 참고).
+* **게이트**: 두 경우 모두 "이 로봇이 지금 어디로 가야 하는지"에 대한 판단이 사람이 보기에
+  타당한지부터 확인한 뒤에야 실제 구동을 연결한다 — 잘못된 목표를 실제 바퀴에 바로 연결하면
+  위험하다.
+
+### Phase 5 — 통제된 구역 폐루프 자율주행 첫 테스트
+
+* 울타리 등으로 막힌 좁은 구역, 저속, 사람이 항시 대기하며 즉시 개입(CH5 브레이크 또는
+  프로그램 종료)할 수 있는 상태에서 첫 자율주행 테스트를 진행한다.
+* 짧은 웨이포인트(수 미터) 단위로 끊어서 검증 — 한 번에 긴 경로를 맡기지 않는다.
+* [06번 문서](06-imu-integration-plan.md) Phase 4(전신 슬립 감지)가 자율주행 중에도 그대로
+  살아있는지, 즉 SLAM/Nav2 경로를 따라가는 도중에도 슬립 감지 시 속도를 낮추는 안전장치가
+  정상 작동하는지 반드시 같이 확인한다.
+* **게이트**: 짧은 구간에서 반복 검증이 안정적이어야 범위를 넓힌다.
+
+### Phase 6 — 범위 확장
+
+* 더 길고 개방된 구역, 동적 장애물(사람/다른 물체 이동) 대응, Nav2 로컬 코스트맵과 기존
+  전신 슬립 감지·바퀴 이상 진단을 함께 쓰는 이중 안전 체계로 확장한다.
+
+## 7.6 규모감
+
+[06번 문서](06-imu-integration-plan.md)의 Phase 1~5a는 전부 `xbox_motor_control.cpp` 한 파일
+안에서의 점진적 추가였다. 이번 작업은 **완전히 다른 소프트웨어 스택(ROS2)을 새로 설치·구성**하고
+별도 패키지들을 빌드해야 하는, 성격이 다르고 규모가 훨씬 큰 작업이다. (A) 경로를 선택하면
+SLAM 패키지 도입뿐 아니라 **URDF 작성 + `xbox_motor_control.cpp`의 하드웨어/킨매틱스 로직을
+ros2_control 하드웨어 인터페이스로 이식**하는 작업까지 포함되므로, 사실상 이 프로그램의 실행
+레이어 전체를 ROS2 표준 구조로 재작성하는 셈이다 — §7.4.4 참고.