diff --git a/doc/07-lidar-slam-plan.md b/doc/07-lidar-slam-plan.md index 5fc132e..f615c38 100644 --- a/doc/07-lidar-slam-plan.md +++ b/doc/07-lidar-slam-plan.md @@ -1,207 +1,235 @@ -# 07. Livox Mid-360S 기반 LiDAR SLAM 도입 계획 (Phase 6, 실행 전 검토용) +# 07. 2.5D 맵 기반 자율주행 — 현재 상태 정리와 다음 단계 (Phase 6) -> **이 문서는 계획안이다. 코드는 아직 수정하지 않았다.** [06-imu-integration-plan.md](06-imu-integration-plan.md) -> §Phase 6("장기, 선택")에서 예고된 자율주행 확장을 구체화한 문서로, 아래 내용을 검토하고 방향에 -> 동의하면 그때 실제 작업(ROS2 설치, 패키지 빌드, URDF/ros2_control 작성, 코드 통합)을 시작한다. +> **문서 성격 변경**: 이전 버전은 "계획안, 코드 미착수"를 전제로 썼다. 그러나 실제로는 별도 +> 워크스페이스 `~/fori_ws`에 이 문서가 계획하려던 내용의 상당 부분이 **이미 구현되어 동작 +> 중**이다(§7.0). 그래서 이 문서는 더 이상 "무엇을 어떻게 새로 만들 것인가"가 아니라 **"무엇이 +> 이미 있고, 무엇이 검증되지 않았고, 무엇을 다음에 해야 하는가"**를 정리하는 문서로 성격을 +> 바꾼다. [06번 문서](06-imu-integration-plan.md)의 RC 통합 작업과는 독립적으로 진행된 별도 +> 트랙이었다는 점에 유의한다. -## 7.1 현재 상태 (코드 근거) +## 7.0 두 저장소의 관계 — 왜 이 문서가 실제와 어긋나 있었는가 -로봇에는 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`을 표준화된 방식으로 받아 실제 바퀴 명령으로 변환하는 레이어 필요 | +| `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, 커스텀 시리얼 브릿지, 웹 대시보드까지 포함. | **이미 상당 부분 구현·통합 완료**, 실기체 검증은 일부만 진행 | -## 7.2 Mid-360S는 SLAM에 유리한 센서 +`~/fori_ws/fori_ws/src/fori_serial_bridge_cpp/src/serial_bridge_node.cpp` 상단 주석: -Mid-360S는 논-리피티티브(non-repetitive) 스캔 방식 솔리드스테이트 라이다로, Livox가 이 시리즈 -(Mid-360/Avia 등) 전용으로 **FAST-LIO2**, **Point-LIO** 같은 LiDAR-Inertial Odometry(LIO) SLAM -패키지를 공식 지원·공개하고 있다. 지금 쓰는 SDK2도 이미 이 생태계의 표준이라 하드웨어/펌웨어 -호환성 문제는 적을 것으로 예상된다. +> "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`." -**주의**: Mid-360S 내부에는 **자체 IMU가 내장**되어 있고 LIO 패키지들은 보통 이 내장 -IMU(라이다와 시간 동기화됨)를 쓴다. [06번 문서](06-imu-integration-plan.md)에서 연동한 외부 -WitMotion HWT905와는 별개의 센서이니 혼동하지 않는다. +즉 두 저장소는 이미 실제로 연결된 형제 프로젝트다 — 이 레포에서 검증한 Modbus 프로토콜을 +`fori_ws`가 가져다 ROS2 노드로 감쌌다. 이 문서(07번)는 그 사실을 모른 채 "URDF부터 새로 +작성해야 한다"는 전제로 쓰여 있었으므로, 이번에 전면 수정한다. -## 7.3 권장 스택 +## 7.1 사용자가 요청한 컨셉과 실제 구현 대조 -* **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와는 - 작업 성격이 다른(이미 성숙한 알고리즘을 재발명하는) 리스크가 크다. +사용자가 이번에 요청한 자율주행 방향 — **이미 가진 2.5D 맵을 활용, 고도 정보로 오르막/내리막 +속도 조절, Mid-360S 그대로 사용, `pcd_gunsan_output` 맵 활용** — 은 다음과 같이 **이미 구현되어 +있다**. -## 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 정합 용도 외에는 필수가 아님 | +| 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` | -### 7.4.3 (A)를 선택해도 지금까지의 제어 로직이 사라지지 않는 이유 +**`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에서 재확인이 필요하다. -Nav2가 대체하는 건 "지도 위에서 어디로 가야 하는지 결정하는" **상위 계획 레이어**(경로 계획, -웨이포인트 추종, 전역 장애물 회피 → `/cmd_vel` 생성)뿐이다. `/cmd_vel`(목표 v_x, omega)을 **이 -로봇의 특정 하드웨어에 맞는 실제 4바퀴 RPM으로 바꾸는 실행 레이어**는 Nav2가 전혀 모르는 -영역이라 (A)를 선택해도 계속 필요하다: +## 7.2 판단 1 — "URDF 먼저, 그 다음 ros2_control" 순서가 맞는가 -* **Phase 5a(k_skid/effective_w 캘리브레이션)** — Nav2는 "이만큼 회전해라"만 던질 뿐, 이 로봇이 - 스킷스티어라 회전 시 차동을 얼마나 더 세게 걸어야 하는지는 모른다. 이 변환은 계속 우리 코드의 - 몫이다. -* **저크 제한 감속 / 급가속 시 반전 방지** — 백EMF 보호, 급정지 시 바퀴 튀는 걸 막는 액추에이터 - 레벨 안전은 Nav2 담당이 아니다. 계속 필요. -* **Phase 4(전신 슬립 감지), 걸림/들뜸 진단** — 전류·엔코더·IMU로 보는 바퀴 트랙션 이상은 - 라이다 기반 회피(Nav2 담당)와 완전히 다른 종류의 안전장치라 계속 필요. -* **Phase 2(휠+IMU 융합 오도메트리)** — 오히려 Nav2/`robot_localization`이 정확히 이런 형태의 - 오도메트리 토픽을 입력으로 기대하므로, 없어지는 게 아니라 더 핵심적인 역할을 하게 된다. +**결론: URDF 부분은 맞고 이미 있다. ros2_control은 불필요하다 — 이미 더 가벼운 대안으로 +대체되어 잘 동작 중이다.** -**실제로 역할이 애매해지는 건 Phase 3(헤딩홀드)** 정도다 — RC 수동조종 중 직진 편향 보정용인데, -자율주행 중에는 Nav2의 로컬 플래너가 그 역할을 대신한다. 다만 RC 수동조종 모드를 비상/테스트용 -폴백으로 남겨둔다면(대부분의 실제 로봇이 그렇게 한다) 그 모드에서는 계속 쓰인다. +### URDF: 있음, 다만 표준 패턴과 다른 구조 -정리하면 (A)를 선택해도 **RC 수동조종 관련 부분(CRSF 파싱, 헤딩홀드의 RC 트리거 조건)만 -자율주행 중엔 쓰이지 않는 정도**지, 킨매틱스/안전진단/오도메트리는 전부 계속 쓰인다. +`~/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"`, 라이다 실측 피치 보정 포함)다. -## 7.4.4 URDF + ros2_control로 실행 레이어 구현 +이건 표준 로봇공학 URDF 패턴(고정된 `base_link`를 루트로 두고 오도메트리가 `odom→base_link`를 +발행)과 다르다 — 대신 "라이다 SLAM이 로봇의 기준 좌표계 자체를 소유하고, 로봇 형상은 그 자세를 +따라간다"는 구조다. 이 선택 자체는 실용적이다(LIO가 어차피 가장 정확한 자세 추정원이므로). +다만 §7.4에서 다루는 TF 트리 충돌 문제의 근본 원인이기도 하다. -§7.4.3에서 정리한 "Nav2가 몰라도 계속 필요한" 로직들(Phase 5a 킨매틱스 캘리브레이션, 저크 제한 -S-curve, 전신 슬립 감지, 온도 모니터링)은 없어지지 않되, **`xbox_motor_control.cpp`라는 독립 -C++ 프로그램에서 ROS2 표준 실행 레이어인 ros2_control로 옮겨 담는** 재구성이 필요하다. +### ros2_control: 불필요 — 더 가벼운 직접 시리얼 브릿지가 이미 그 자리를 대신함 -* **URDF**: 휠베이스·트랙폭·휠 반경 등 실측 치수와 Mid-360S/WitMotion IMU 마운트 위치를 담아 - `robot_state_publisher`가 TF 트리(`base_link` → 4개 바퀴 → 라이다/IMU)를 발행하게 한다. 이 - URDF 안에 `` 태그로 하드웨어 인터페이스를 선언하는 것이 표준 패턴이므로, 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의 "먼저 관측으로 검증 후 - 개입" 원칙을 여기에도 그대로 적용한다. +이전 버전 문서는 `hardware_interface::SystemInterface`를 구현하는 ros2_control 플러그인을 +써야 한다고 했다. 실제로는 그렇게 하지 않았다. 대신 `fori_serial_bridge_cpp::ForiSerialBridge` +(일반 `rclcpp::Node`)가: -## 7.5 단계적 진행 순서 (Phase 별) +* `/cmd_vel`(Twist)을 직접 구독 +* 20Hz 제어 루프에서 가감속 램프 + IMU 요레이트 PI 보정 + 차동 킨매틱스 → 좌우 RPM 산출 +* ZLAC8015D에 RS485 Modbus RTU로 직접 명령(레지스터 `0x2088` 속도 쓰기, `0x20AD` 피드백 읽기) +* `/odom_wheels`, `/joint_states`, `/battery_state` 발행 -[06번 문서](06-imu-integration-plan.md)와 같은 원칙을 따른다 — **먼저 순수 관측으로 검증하고, -주행 제어에는 그 검증이 끝난 뒤에만 개입한다.** 각 Phase는 이전 Phase의 검증 게이트를 통과해야 -다음으로 넘어간다. +이 구조로 이미 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로 다시 감싸는 건 실질적 이득 없이 이미 검증된 경로를 +재작성하는 것**이므로 우선순위가 낮다. 유지를 권장한다. -### 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 트리 정합성)도 함께 확인한다. +`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의 작업 항목으로 넣는다. -### Phase 2 — 실내/정적 SLAM 검증 (주행 제어 완전히 미개입) +## 7.3 판단 2 — Nav2 글로벌맵 + 로컬맵 기반 동적 장애물 회피 -* FAST-LIO2 또는 Point-LIO를 빌드해, **로봇을 손으로 들거나 실내에서 천천히 밀며** 지도가 - 쌓이는지 확인한다. 아직 로봇의 모터 제어와는 전혀 연결하지 않는다 — 라이다+SLAM 노드만 - 독립적으로 돌리는 단계. -* 확인 항목: 지도 품질(벽/기둥 등 기준점이 흐트러지지 않는지), 제자리로 돌아왔을 때 루프 - 클로저가 잘 닫히는지, 실시간 처리 속도가 로봇 탑재 컴퓨팅 자원으로 감당되는지. -* **게이트**: 통제된 실내 환경에서 지도/루프클로저가 안정적으로 나와야 다음 단계로 넘어간다. - 여기서 불안정하면 그 상태로 야외에 나가봐야 결과가 더 나빠질 뿐이다. +**결론: 맞고, 이미 사용자가 제안한 것보다 한 단계 더 나아간 3중 구조로 구현되어 있다.** -### Phase 3 — 로봇 탑재 야외 검증 (여전히 주행 제어 미개입, RC 수동조종과 병행 관측) +| 계층 | 역할 | 데이터 소스 | 근거 | +| :--- | :--- | :--- | :--- | +| `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` | -* 실제 로봇에 SLAM 노드를 올리고, **RC로 수동 조종하며** 지금까지 테스트해온 실야외 노면(파인 - 보도블럭/아스팔트/잔디)에서 지도/자세추정이 안정적인지 확인한다. 실내 벤치마크보다 훨씬 - 가혹한 환경(직사광선 반사, 개활지라 루프클로저용 기준점이 적음, 진동)이라 별도 검증이 - 필요하다. -* [06번 문서](06-imu-integration-plan.md) Phase 2에서 만든 **휠+IMU 융합 오도메트리(x, y, - fused_yaw)를 대조군으로 병행 로깅**해 SLAM 결과와 비교한다 — 이미 두 소스(엔코더, WitMotion - IMU)로 한 번 검증해본 경험이 있으니, 세 번째 소스(라이다 SLAM)까지 서로 얼마나 일치하는지 - 교차검증하면 어느 쪽이 이 환경에서 더 신뢰할 만한지 판단할 근거가 생긴다. -* **게이트**: 야외 실주행 중 SLAM 드리프트가 왕복 경로 기준으로 실용적인 수준(예: 수십 미터 - 주행에 수십 cm 이내)인지 확인해야 다음 단계로 넘어간다. +Collision Monitor는 `terrain_speed_node`가 낸 `/cmd_vel_terrain`(경사 보정까지 끝난 명령)을 +받아 최종 `/cmd_vel`을 낸다 — 즉 실제 파이프라인은 `Nav2 계획 → 경사 보정 → 급출현 장애물 +감속/정지 → 실제 모터`(§7.5 아키텍처 그림 참고) 순서로, 사용자가 제안한 "글로벌은 지도로, +로컬은 라이다로" 구조를 그대로 따르면서 한 겹의 실시간 안전장치를 더 두고 있다. -### Phase 4 — 아키텍처 결정 + 저위험 프로토타입 (모터 제어에 처음으로 연결, 자동 개입은 없음) +**알려진 한계 하나**: 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.2의 (A) Nav2 완전 이관 / (B) 얇은 브릿지 중 방향을 확정한다. -* (A)라면: Nav2 스택(costmap, 플래너)을 세팅하고, §7.4.4에서 설명한 **ros2_control 하드웨어 - 인터페이스(`hardware_interface::SystemInterface`)를 작성**해 `xbox_motor_control.cpp`의 - ZLAC8015D 통신/킨매틱스/안전로직을 이식하고 URDF의 `` 태그로 선언한다. 컨트롤러 - (`diff_drive_controller` 재사용 또는 커스텀 스킷스티어 컨트롤러)까지 붙이되, **경로/속도명령이 - 실제로 잘 나오는지만 확인** — 아직 ros2_control 노드가 실제 모터로 명령을 내보내지 않도록 - 하드웨어 인터페이스 안에서 출력을 로그로만 찍고, RViz 등에서 시각적으로만 검증. -* (B)라면: 웨이포인트를 받아 방향/거리를 계산하는 브릿지 코드를 작성하되, 처음엔 **콘솔에 - "이 방향으로 가라"만 출력**하고 실제 `setRPMs()` 호출은 아직 연결하지 않는다. 이 경로는 - ros2_control 없이도 진행 가능하다(§7.4.2 참고). -* **게이트**: 두 경우 모두 "이 로봇이 지금 어디로 가야 하는지"에 대한 판단이 사람이 보기에 - 타당한지부터 확인한 뒤에야 실제 구동을 연결한다 — 잘못된 목표를 실제 바퀴에 바로 연결하면 - 위험하다. +## 7.4 질문에 대한 답 -### Phase 5 — 통제된 구역 폐루프 자율주행 첫 테스트 +### Q1. 저장된 맵을 제공하고 그 맵으로 Nav2를 진행해도 실시간 자세 추종이 가능한가? -* 울타리 등으로 막힌 좁은 구역, 저속, 사람이 항시 대기하며 즉시 개입(CH5 브레이크 또는 - 프로그램 종료)할 수 있는 상태에서 첫 자율주행 테스트를 진행한다. -* 짧은 웨이포인트(수 미터) 단위로 끊어서 검증 — 한 번에 긴 경로를 맡기지 않는다. -* [06번 문서](06-imu-integration-plan.md) Phase 4(전신 슬립 감지)가 자율주행 중에도 그대로 - 살아있는지, 즉 SLAM/Nav2 경로를 따라가는 도중에도 슬립 감지 시 속도를 낮추는 안전장치가 - 정상 작동하는지 반드시 같이 확인한다. -* **게이트**: 짧은 구간에서 반복 검증이 안정적이어야 범위를 넓힌다. +**가능하다.** "SLAM(지도를 만들면서 동시에 자세를 추정)"과 "Localization(이미 있는 지도 +안에서 자세만 추정)"은 다른 문제다. 후자는 지도를 새로 만들 필요가 없어 계산량이 훨씬 가볍고, +이게 정확히 Nav2의 **AMCL**(Adaptive Monte Carlo Localization)이 하는 일이다. `fori_ws`에는 +이미 이 구조가 설정되어 있다: -### Phase 6 — 범위 확장 +* `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 프레임 기준 위치)를 + 실시간으로 발행 -* 더 길고 개방된 구역, 동적 장애물(사람/다른 물체 이동) 대응, Nav2 로컬 코스트맵과 기존 - 전신 슬립 감지·바퀴 이상 진단을 함께 쓰는 이중 안전 체계로 확장한다. +즉 SLAM처럼 지도를 다시 만들 필요 없이, **저장된 지도 하나만 있으면 실시간 위치 추정이 가능** +하다는 것이 맞다. -## 7.6 규모감 +### Q2. 그렇다면 라이다로 주행 중 저장된 맵과 계속 대조하며 자세를 추종하는 것인가? -[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 참고. +**맞다 — 그게 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의 핵심이다.