diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0b33ee6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +# colcon build artifacts — regenerate with `colcon build` +build/ +install/ +log/ + +__pycache__/ +*.pyc diff --git a/docs/SCAN-운용가이드.md b/docs/SCAN-운용가이드.md new file mode 100644 index 0000000..0698b39 --- /dev/null +++ b/docs/SCAN-운용가이드.md @@ -0,0 +1,120 @@ +# 스캔 → 매핑 운용 가이드 (녹화: 트리플 카메라 GUI / 처리: fast_dual_ws) + +> 전체 파이프라인: **녹화**는 `~/fast_ws` 의 스캔 GUI(+`~/fhd_rtk_ws` UM982 드라이버)로, **매핑(SLAM)** +> 은 이 문서가 있는 `~/fast_dual_ws` (FAST-LIVO2, 듀얼/트리플 카메라 지원 빌드)로 처리한다. +> 녹화 도구와 처리 애플리케이션이 서로 다른 워크스페이스이므로 이 문서는 `fast_dual_ws/docs/` +> 에 둔다 (`fast_ws` 는 녹화 GUI 전용 워크스페이스라 전체 파이프라인 문서를 두기에 맞지 않음). + +--- + +## 0. 워크스페이스 역할 정리 + +| 워크스페이스 | 역할 | 비고 | +|---|---|---| +| `~/fhd_rtk_ws` | UM982 RTK GNSS 드라이버(파싱+NTRIP+`/ublox_driver/receiver_pvt` 발행) | 녹화 전용 최소 빌드 | +| `~/fast_ws` | LiDAR + cam1/cam2/cam3 시동 및 `ros2 bag record` GUI (`scan_gui_triple.py`) | `fhd_rtk_ws`/`camera2_ws` 를 함께 source | +| `~/fast_dual_ws` | **FAST-LIVO2 매핑 엔진**(dual/triple 카메라 VIO-LIO 빌드, `fastlivo_mapping`) | 녹화된 bag 을 재생해서 SLAM 돌리는 애플리케이션 | +| `~/FAST-LIVO2-RTK-ROS2` | GNSS-RTK 융합이 포함된 별도 FAST-LIVO2 소스(옵티마이저에 `gpsHandler` 있음) | 아직 `fast_dual_ws` 에 통합 안 됨 — §4 참고 | + +> ⚠️ **중요**: `fast_dual_ws/src` 에 `gnss_comm`/`um982_driver` 패키지가 같이 들어있지만, +> 현재 `fast_dual_ws` 의 `fast_livo` (`package.xml`, `LIVMapper.cpp` 등)는 **GNSS 토픽을 +> 구독하지 않는다** (`gnss_comm`/`GnssPVTSolnMsg` 참조 없음). 즉 녹화 bag 에 `/ublox_driver/receiver_pvt` +> 를 같이 담아도 지금의 `fast_dual_ws` 매핑에는 **아직 반영되지 않는다** — 순수 LiDAR-Inertial-Visual +> 매핑만 수행된다. GNSS 융합이 필요하면 `~/FAST-LIVO2-RTK-ROS2` 통합 작업이 별도로 필요하다. + +--- + +## 1부. 녹화 (fast_ws 스캔 GUI + fhd_rtk_ws UM982) + +### 1.1 사전 준비 +- LiDAR, cam1/2/3, UM982 USB 연결. UM982 안테나는 하늘이 트인 곳에 — 실내면 GNSS `No Fix`(SV 0) 가 정상. +- `/dev/ttyUSB0` 권한 확인: + ```bash + ls -la /dev/ttyUSB0 + # 그룹이 dialout 인데 미가입이면: + sudo usermod -aG dialout $USER # 이후 재로그인 필요(영구) + sudo chmod a+rw /dev/ttyUSB0 # 임시(재부팅/재연결 시 초기화) + ``` +- UM982 최초 설정(또는 FRESET 후): `rtk/config_all.py` 로 `BESTNAVB COM3 0.1`, `GPGGA COM3 1`, baud 460800, `MODE ROVER` 저장 확인. +- 터미널에 남아있는 `um982_driver_node` 프로세스가 있으면 포트를 선점해 GUI GPS 시작이 실패한다: + ```bash + ps aux | grep um982_driver_node | grep -v grep && kill -INT + ``` + +### 1.2 GUI 실행 및 녹화 +바탕화면 **"스캔 GUI (트리플 카메라)"** 아이콘 실행 → 좌측 패널 순서대로: + +1. **시동** — LiDAR 즉시 실행, 5초 후 cam1/cam2/cam3 자동 실행. 우측 미리보기 확인. +2. **GPS 시작** — 상태 라벨/좌표 갱신 확인. 색상 의미: + + | 라벨 | 의미 | + |---|---| + | ● No Fix | 위성 미포착 — 실내 정상 | + | ● GPS / ● DGPS | 단독/차분 측위, RTK 아님 | + | ● RTK Float | 보정 진행 중 | + | ● RTK Fixed | 최고 정밀도(cm급) — **녹화는 이 상태 권장** | + +3. **녹화** — 저장 경로/파일명 입력, **"녹화에 GPS 포함"** 체크(GPS 가 켜져 있어야 실제 포함됨) → 녹화 시작 → 종료 시 정지. + - 저장 위치: `~/bags/<파일이름>/<파일이름>_0.db3` + +### 1.3 녹화 검증 +```bash +source /opt/ros/humble/setup.bash +ros2 bag info ~/bags/<파일이름>/ +``` +- 토픽: `/livox/lidar`, `/livox/imu`, `/cam1/image`, `/cam2/image`, `/cam3/image`(+`camera_info` 3종), GPS 포함 시 `/ublox_driver/receiver_pvt` +- GNSS 메시지 수 ÷ Duration ≈ 10Hz 확인. + +--- + +## 2부. 재생 + 매핑 실행 (fast_dual_ws) + +### 2.1 빌드 확인 (최초 1회 / 소스 수정 시) +```bash +cd ~/fast_dual_ws +colcon build --packages-select fast_livo +source install/setup.bash +``` + +### 2.2 실행 (터미널 2개) +**터미널 A** — 매핑 노드 + RViz: +```bash +source /opt/ros/humble/setup.bash +source ~/fast_dual_ws/install/setup.bash +ros2 launch fast_livo mapping_mid360s_triplecam.launch.py use_rviz:=True +``` + +**터미널 B** — 녹화한 bag 재생 (스페이스바로 재생/일시정지): +```bash +source /opt/ros/humble/setup.bash +ros2 bag play -p ~/bags/<파일이름>/ +``` +- `-p` 는 일시정지 상태로 시작 → RViz 창에서 매핑 노드가 준비된 걸 확인한 뒤 스페이스바로 재생. +- 매핑 노드가 구독하는 토픽(`/livox/lidar`, `/livox/imu`, `/cam1/image`, `/cam2/image`, `/cam3/image`)은 + `scan_gui_triple.py` 녹화 토픽명과 동일하므로 리매핑 불필요. + +### 2.3 결과 확인 +- RViz(`fast_livo2.rviz`)에서 포인트클라우드 누적/궤적 확인. +- `/ublox_driver/receiver_pvt` 는 bag 에 있어도 매핑 결과에 영향 없음(§0 참고) — GNSS 데이터 자체 품질만 + 따로 보고 싶으면 별도로 `ros2 topic echo -b ...` 또는 재생 중 `ros2 topic echo /ublox_driver/receiver_pvt` 로 확인. + +--- + +## 3. 트러블슈팅 + +| 증상 | 원인 | 조치 | +|---|---|---| +| GPS 시작 직후 "GPS 없음"으로 복귀 / gnss.log 에 `시리얼 오픈 실패` | 권한 문제 또는 포트 중복 점유 | §1.1 권한/프로세스 확인 | +| 실내에서 계속 No Fix | 정상(위성 미포착) | 옥외/창가 이동, 그래도 안 되면 안테나 케이블 확인 | +| bag 재생해도 RViz 에 아무것도 안 뜸 | `-p` 로 일시정지 상태 → 스페이스바 안 누름, 또는 토픽명 불일치 | 스페이스바로 재생 시작, `ros2 bag info` 로 토픽명 재확인 | +| GNSS 데이터가 매핑에 반영 안 되는 것 같음 | **현재 `fast_dual_ws` 는 GNSS 미융합** (설계상 아직 없음) | §0 참고 — 필요 시 `FAST-LIVO2-RTK-ROS2` 통합 작업 별도 진행 | + +--- + +## 4. GNSS 융합이 필요해지면 + +`~/FAST-LIVO2-RTK-ROS2/FAST-LIVO2-RTK-ROS2` 소스에는 `optimization.cpp::gpsHandler` 등 GNSS-RTK +융합 로직이 이미 구현돼 있다(원본 `fhd_rtk_ws` 문서가 가리키던 "FAST-LIVO2-RTK 백엔드"가 이것). 현재는 +빌드된 워크스페이스가 아니라 압축 해제된 소스 상태([`__MACOSX`](../../FAST-LIVO2-RTK-ROS2) 잔재로 보아 +zip 압축 해제본). `fast_dual_ws` 의 dual/triple 카메라 지원과 이 GNSS 융합을 합치려면 두 소스를 +비교해 병합하는 별도 작업이 필요하다 — 착수 시 다시 요청. diff --git a/docs/UM982-RTK-Fixed-진단기록_2026-07-30.md b/docs/UM982-RTK-Fixed-진단기록_2026-07-30.md new file mode 100644 index 0000000..582ef74 --- /dev/null +++ b/docs/UM982-RTK-Fixed-진단기록_2026-07-30.md @@ -0,0 +1,160 @@ +# UM982 RTK Fixed 진단 기록 (2026-07-30, 옥상) + +> 목적: `fhd_rtk_ws`(`um982_driver` C++ 드라이버) 기준으로 UM982 로깅 파이프라인을 검증하던 중, +> 옥상에서 RTK Fixed가 안 나오는 문제를 끝까지 추적한 기록. 결론부터 말하면 **안테나 위치(높이/ +> 개방도)가 결정적 원인**이었고, 소프트웨어/설정 쪽에서 발견한 버그 2건은 실제 버그였지만 +> 그것만으론 Fixed가 안 나왔다. +> +> ⚠️ 재현 검증 진행 중: RELIABILITY를 원래 엄격값(`3 1`)으로 되돌린 뒤 난간 위치에서 재테스트 +> 예정(§2.9). 위치만으로 Fixed가 재현되는지, RELIABILITY 완화가 실제로 필요했는지를 분리해서 +> 확인하기 위함. + +--- + +## 1. 결과 요약 + +| 시점 | 안테나 위치 | 결과 | +|---|---|---| +| 옥상, 테이블 위 (여러 차례, 총 20분+) | 테이블(낮음, 난간보다 낮은 높이) | SINGLE ↔ PSRDIFF(위성 5~9개) 반복, **Float/Fixed 도달 못 함** | +| 옥상, 난간 위 | 난간(옥상 가장자리, 탁 트인 위치, 테이블보다 높음) | **NTRIP 접속 후 약 3.4초 만에 RTK FIXED**, 위성 31개, h_acc **2.9cm** | + +두 위치 모두 안테나는 거치된 상태(손으로 든 적 없음)였다. 차이는 **높이와 개방도**다 — +테이블은 난간보다 낮고, 난간은 옥상 가장자리라 하늘이 훨씬 트여 있다. + +``` +[um982_driver]: 상태: SINGLE(pos_type=16) | nSV=28 hσ=1.771m diff_age=0.0s (t+0.03s) +[um982_driver]: NTRIP 접속: RTS1.ngii.go.kr:2101/VRS-RTCM34 (t+0.8s) +[um982_driver]: 상태: OTHER(pos_type=17) | nSV=13 hσ=1.378m diff_age=0.6s (t+1.8s) +[um982_driver]: 상태: RTK FIXED(pos_type=50) | nSV=31 hσ=0.029m diff_age=1.1s (t+3.4s) +``` + +테이블 위치에서는 같은 옥상, 같은 NTRIP 계정/마운트포인트, 같은 RELIABILITY 설정으로 +8분 넘게 시도해도 위성 8~9개, 정확도 4~6m에서 정체됐던 것과 극명히 대비된다. + +--- + +## 2. 조사 경과 (시간순) + +### 2.1 파이프라인 기초 검증 (실내 → 실외 이동 전) +- `ros2 launch um982_driver um982_driver.launch.py` 정상 기동, `/ublox_driver/receiver_pvt` **10Hz** 발행 확인. +- 실내에서는 `num_sv=0`/`fix_type=NONE` — 예상된 정상 동작(위성 미포착). +- `ros2 bag record` 15초 테스트 → 148개 메시지 정상 기록 확인. + +### 2.2 시리얼 권한 문제 +- `/dev/ttyUSB0`가 `dialout` 그룹 소유인데 사용자가 그룹 미가입 → `Permission denied`. +- 임시 조치: `sudo chmod a+rw`. 영구 조치는 `sudo usermod -aG dialout $USER` 권장(재로그인 필요). + +### 2.3 ⚠️ 버그 발견 #1 — install 설정이 stale +- `ros2 launch`는 `src/`가 아니라 `install/share/um982_driver/config/um982.yaml` **복사본**을 읽는데, + 최초 빌드(11:30) 이후 리빌드가 안 돼서 `ntrip_user`/`ntrip_pass`가 계속 **빈 문자열**이었음. +- 즉 그동안의 모든 실행이 **NTRIP 인증 없이** 시도되고 있었을 가능성이 큼. +- 조치: `colcon build --packages-select um982_driver` 리빌드 → 계정 정보 반영 확인. +- 이후 NTRIP 계정을 `hyuk` → `hyuk6578`로 수정(사용자 확인), 재빌드로 반영. + +### 2.4 데이터 무결성 검증 (드라이버와 별개로 원시 데이터 직접 확인) +- BESTNAV 바이너리: SYNC(`AA 44 B5`) + CRC-32(Appendix 1 알고리즘)를 파이썬으로 재구현해 + 원시 시리얼 60프레임 캡처 → **CRC 통과율 100%**. 콘솔에 뜨는 "CRC 불일치" 경고 1회는 + 포트를 여는 시점에 이미 스트리밍 중이던 프레임 중간에 끼어든 것으로, 실제 재현 안 됨(benign). +- GPGGA(`$GNGGA`): 1Hz로 정상 출력, fix quality=1, 체크섬 정상 → NTRIP GGA 부트스트랩 조건 충족. + +### 2.5 NTRIP/RTCM 검증 (드라이버와 별도로 직접 접속) +- 리빌드 후 재실행 → 로그에 `NTRIP 접속: RTS1.ngii.go.kr:2101/VRS-RTCM34` 출력(=서버가 `200 OK` + 응답해야만 찍히는 로그이므로 **인증 성공** 확인). +- 드라이버 코드에 진단 로그 2건 추가(파일: `src/um982_driver/src/um982_driver_node.cpp`): + - 상태 로그에 `pos_type`(정수) + `diff_age` 표시 + - NTRIP 수신 누적 바이트 수(5초 스로틀) +- 이 코드로 확인: RTCM이 **꾸준히 초당 800B 이상** 안정적으로 유입됨. +- 드라이버와 독립적으로 같은 NTRIP 스트림에 파이썬으로 직접 접속해 RTCM3 프레임을 파싱: + 정상적인 VRS 네트워크 RTK 메시지 세트 확인 — + `1005/1007`(기준국 좌표/안테나), `1030~1033`(네트워크 RTK 보조), `1075/1085/1095/1115` + (GPS/GLONASS/Galileo/QZSS MSM5). **북두(BeiDou) 보정 메시지는 이 마운트포인트에 없음** + (단, 이후 §2.7에서 이게 핵심 원인이 아니었음이 드러남 — 수신기가 애초에 북두를 포함해 + 31개까지 위성을 쓸 수 있었기 때문). + +### 2.6 수신기 설정 검증 (`CONFIG` 명령으로 직접 조회) +- `MODE` 쿼리 → `MODE ROVER UAV` 확인(정상, 필수 사전조건 충족). +- `CONFIG SIGNALGROUP 4 5` 확인 → Unicore 매뉴얼 Table 4-32 대조 결과 **UM982 공장 기본값** + 그대로였음(Master=4/Slave=5, BDS+GPS+GLO+GAL+QZSS 5개 위성군 추적하는 넓은 설정). 문제 아님. + +### 2.7 RTK RELIABILITY 조정 (테이블 위치에서 시도) +- `CONFIG` 덤프에서 `CONFIG RTK RELIABILITY 3 1` 확인(매뉴얼 Table 4-10: 파라미터1=RTK + 포지셔닝 엔진 신뢰도, 3=Relatively high=기본값 / 파라미터2=ADR 신뢰도, 1=Low=기본값). +- 과거 기록(`~/rtk/RTK_Fixed_달성기록.md`, 2026-06-09)에 따르면, 위성 27~31개/HDOP 0.5~0.6인 + 좋은 조건에서도 `RELIABILITY 3`이면 **5분 넘게 Float에서 Fixed로 안 넘어갔고**, `1 1`로 + 낮춘 뒤에야 약 3분 만에 Fixed 달성한 전례가 있음. 이 설정은 RAM에만 있으면 전원 재시작/ + FRESET 시 3으로 되돌아간다고 명시돼 있었고, 실제로 되돌아가 있었음(아마 매뉴얼 참고 재설정 + 과정에서 초기화된 것으로 추정). +- 조치: `CONFIG RTK RELIABILITY 1 1` + `SAVECONFIG` 실행, 재조회로 영구 저장 확인. +- **그런데도** 테이블 위치에서는 4분 이상 재시도해도 위성 8~9개, PSRDIFF에서 못 벗어남 → + RELIABILITY만으로는 해결되지 않았고, 그 이전 단계(Float 진입 자체)가 위성 수 부족으로 + 막혀 있었다는 뜻이었음. + +### 2.8 테이블 vs 난간 위치 비교 (결정적 실험) +- 옥상, 옆 건물 있음 → 초기엔 "옆 건물發 멀티패스"로 추정. +- 위성 수 격차(테이블 8~9개 vs 과거 성공 기록 27~31개)가 "옆 건물 정도"로 설명하기엔 + 과도하게 크다고 판단해 다른 원인(안테나 흔들림 등)도 검토했으나, 실제로는 흔들림이 + 아니라 **테이블 자체가 난간보다 낮은 위치**였던 것이 원인이었음(사용자 확인). +- **난간(옥상 가장자리, 트인 위치)으로 옮겨 재시도 → §1의 결과.** 위성 수 8~9개 → 28~31개로 + 즉시 회복, NTRIP 접속 3.4초 만에 RTK FIXED. + +### 2.9 재현 검증 (진행 중) +- 난간 테스트는 RELIABILITY가 `1 1`(완화값)로 설정된 상태에서 이루어짐 → 위치 개선과 + RELIABILITY 완화 중 무엇이 실제로 결정적이었는지 분리가 안 된 상태. +- RELIABILITY를 원래 엄격값 `CONFIG RTK RELIABILITY 3 1` + `SAVECONFIG`로 **되돌림**(확인 완료). +- 이 상태로 난간 위치에서 재테스트 예정 — 위치만으로도 Fixed가 나오는지 확인. + 결과는 추후 이 문서에 추가. + +--- + +## 3. 원인 분석 + +**핵심 원인 (결정적, primary): 안테나 위치 — 테이블(낮음, 상대적으로 덜 트임) vs 난간(옥상 +가장자리, 높고 탁 트임).** +낮은 위치에서는 옆 건물 등에 의한 저고도 위성 신호 가림/멀티패스가 반송파 기반 RTK 모호성 +해석을 방해했을 것으로 추정된다. SPP(단독측위)는 코드 신호만 쓰기 때문에 이 정도 환경엔 +상대적으로 관대해서 SINGLE 상태의 좌표는 비교적 정상으로 보였지만, 그래서 오히려 +"수신기가 위성은 잡는데 왜 Fixed가 안 되지"로 오인하기 쉬웠다. + +**부차 원인 (실제 버그였지만 단독으론 불충분, contributing):** +1. `install/` 설정 파일이 stale해서 한동안 NTRIP 인증이 빈 계정으로 시도됨 — 리빌드로 해결. +2. `RTK RELIABILITY`가 엄격 기본값(3)으로 되돌아가 있었음 — `1 1` + `SAVECONFIG`로 임시 완화. + (§2.9 재현 검증을 위해 이후 다시 `3 1`로 되돌림 — 위치 개선만으로 충분한지 확인 중.) + +이 두 버그는 위치 개선 없이는 어차피 Fixed가 안 나왔을 것이므로 "고쳐도 소용없었다"고 +오인할 수 있지만, RTCM 인증/유입이 정상화돼 있었기 때문에 난간 이동 직후 3.4초라는 +이례적으로 빠른 Fixed가 가능했을 가능성이 있다. RELIABILITY 완화가 실제로 필수였는지는 +§2.9 재현 결과로 확정한다. + +--- + +## 4. 결론 및 권장 운용 방법 + +1. **UM982 안테나는 최대한 높고 트인 위치에 거치할 것.** 옆 건물 등 장애물이 있는 옥상이라면 + 낮은 테이블보다 난간·삼각대 등으로 최대한 높이고, 장애물에서 수평으로도 떨어뜨리는 게 + 좋다. +2. **`fhd_rtk_ws/src/um982_driver/config/um982.yaml`을 수정한 뒤에는 항상 리빌드할 것** + (`colcon build --packages-select um982_driver`). `install/`은 심볼릭 링크가 아니라 + 복사본이라 리빌드 없이는 반영되지 않는다. 아니면 `--symlink-install`로 한 번 다시 + 빌드해두면 이후 src 수정이 즉시 반영된다. +3. **수신기 `CONFIG RTK RELIABILITY` 값을 상황에 맞게 확인.** 기본값은 `3 1`(엄격). 위치가 + 좋은데도 Float에서 Fixed로 안 넘어가면 `CONFIG RTK RELIABILITY 1 1` → `SAVECONFIG`로 + 완화를 시도해볼 수 있다(§2.9에서 위치만으로 충분한지 재검증 중이므로, 필요 여부는 + 재현 결과 확인 후 최종 결론). + FRESET이나 일부 재설정 작업 후 기본값으로 되돌아갈 수 있으니 `CONFIG` 응답의 + `CONFIG RTK RELIABILITY` 라인으로 주기적으로 확인. +4. **진단용 로그가 드라이버에 상시 포함됨** (`um982_driver_node.cpp`, 이번에 추가): + 상태 전환 로그에 `pos_type`/`diff_age`가 함께 찍히고, NTRIP 수신 바이트가 5초마다 + 찍힌다. 다음에 비슷한 문제가 생기면 이 로그로 "NTRIP이 안 붙는지" vs "붙었는데 수신기가 + 못 쓰는지"를 바로 구분할 수 있다. + +--- + +## 5. 관련 파일 + +| 파일 | 내용 | +|---|---| +| `src/um982_driver/src/um982_driver_node.cpp` | pos_type/diff_age/NTRIP 바이트 진단 로그 추가됨 | +| `src/um982_driver/config/um982.yaml` | NTRIP 계정 갱신됨 (자격증명은 리포에 평문 저장 안 함) | +| `docs/Unicore Reference Commands Manual For N4 High Precision Products_V2_EN_R1.4.pdf` | MODE/RELIABILITY/SIGNALGROUP 명령 근거 | +| `~/rtk/RTK_Fixed_달성기록.md` | 2026-06-09 과거 Fixed 달성 기록(RELIABILITY 단서의 출처) | +| `~/rtk/save_reliability.py` | RELIABILITY 저장 스크립트(포트가 `/dev/ttyUSB0`로 하드코딩돼 있어 포트 바뀌면 수정 필요) | diff --git a/docs/Unicore Reference Commands Manual For N4 High Precision Products_V2_EN_R1.4.pdf b/docs/Unicore Reference Commands Manual For N4 High Precision Products_V2_EN_R1.4.pdf new file mode 100644 index 0000000..b97c30e Binary files /dev/null and b/docs/Unicore Reference Commands Manual For N4 High Precision Products_V2_EN_R1.4.pdf differ diff --git a/src/gnss_comm/CMakeLists.txt b/src/gnss_comm/CMakeLists.txt new file mode 100644 index 0000000..024af28 --- /dev/null +++ b/src/gnss_comm/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(gnss_comm) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/GnssTimeMsg.msg" + "msg/GnssPVTSolnMsg.msg" +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/src/gnss_comm/msg/GnssPVTSolnMsg.msg b/src/gnss_comm/msg/GnssPVTSolnMsg.msg new file mode 100644 index 0000000..eaf99f4 --- /dev/null +++ b/src/gnss_comm/msg/GnssPVTSolnMsg.msg @@ -0,0 +1,22 @@ +# This message contains information of UBX-NAV-PVT message. +# reference: [1]. UBX-18010854-R08, page 132 + +GnssTimeMsg time # GNSS time of the navigation epoch +# GNSS fix type (0=no fix, 1=dead reckoning only, 2=2D-fix, 3=3D-fix, +# 4=GNSS+dead reckoning combined, 5=time only fix) +uint8 fix_type +bool valid_fix # if fix valid (1=valid fix) +bool diff_soln # if differential correction were applied (1=applied) +uint8 carr_soln # carrier phase range solution status (0=no carrier phase, 1=float, 2=fix) +uint8 num_sv # number of satellites used in the solution +float64 latitude # latitude [degree] +float64 longitude # longitude [degree] +float64 altitude # height above ellipsoid [m] +float64 height_msl # height above mean sea level [m] +float64 h_acc # horizontal accuracy estimate [m] +float64 v_acc # vertical accuracy estimate [m] +float64 p_dop # Position DOP +float64 vel_n # NED north velocity [m/s] +float64 vel_e # NED east velocity [m/s] +float64 vel_d # NED down velocity [m/s] +float64 vel_acc # speed accuracy estimate [m/s] diff --git a/src/gnss_comm/msg/GnssTimeMsg.msg b/src/gnss_comm/msg/GnssTimeMsg.msg new file mode 100644 index 0000000..ff72f30 --- /dev/null +++ b/src/gnss_comm/msg/GnssTimeMsg.msg @@ -0,0 +1,5 @@ +# This message contains GNSS time expressed in the form of +# GNSS week number and time of week(in seconds) + +uint32 week +float64 tow diff --git a/src/gnss_comm/package.xml b/src/gnss_comm/package.xml new file mode 100644 index 0000000..d76bc41 --- /dev/null +++ b/src/gnss_comm/package.xml @@ -0,0 +1,25 @@ + + + + gnss_comm + 1.0.0 + + Minimal ROS 2 interface package providing the gnss_comm GNSS PVT solution + messages used by FAST-LIVO2-RTK. Field layout matches the original + HKUST-Aerial-Robotics/gnss_comm ROS 1 messages so converted rosbags + (gnss_comm/msg/GnssPVTSolnMsg) play back directly. + + FAST-LIVO2-RTK ROS2 port + GPLv3 + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/um982_driver/CMakeLists.txt b/src/um982_driver/CMakeLists.txt new file mode 100644 index 0000000..2e6005e --- /dev/null +++ b/src/um982_driver/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(um982_driver) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(gnss_comm REQUIRED) + +add_executable(um982_driver_node src/um982_driver_node.cpp) +ament_target_dependencies(um982_driver_node rclcpp gnss_comm) +target_link_libraries(um982_driver_node pthread) + +install(TARGETS um982_driver_node + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/um982_driver/README.md b/src/um982_driver/README.md new file mode 100644 index 0000000..3f19386 --- /dev/null +++ b/src/um982_driver/README.md @@ -0,0 +1,68 @@ +# um982_driver + +UM982(Unicore) → `gnss_comm/msg/GnssPVTSolnMsg` 드라이버 노드. FAST-LIVO2-RTK 백엔드가 +구독하는 토픽(`gps.gps_topic`, 기본 `/ublox_driver/receiver_pvt`)으로 발행한다. + +기존 ublox 드라이버를 대체한다. 자세한 배경/검토는 +[`docs/UM982-명령어검토-및-바이너리포팅.md`](../../docs/UM982-명령어검토-및-바이너리포팅.md), +[`docs/UM982-녹화-및-연동-구현.md`](../../docs/UM982-녹화-및-연동-구현.md) 참고. + +## 한 노드에 다 들어있는 이유 +시리얼 포트는 한 프로세스만 점유할 수 있다. 따라서 **RTCM 주입(NTRIP)** 과 **BESTNAV 파싱/발행**이 +같은 노드 안에 있다: + +``` +NTRIP(VRS) ──RTCM3──▶ UM982 ──BESTNAV(binary)+GGA(ascii)──▶ um982_driver ──GnssPVTSolnMsg──▶ 백엔드 + ▲ │ + └────────────────────── GGA(VRS 유지) ───────────────────┘ +``` + +## 수신기 사전 설정 +드라이버 실행 전에 UM982 가 아래를 출력하도록 저장돼 있어야 한다(리포 루트 `rtk/config_all.py`): +``` +BESTNAVB COM3 0.1 # 10Hz 바이너리 (파싱 소스) +GPGGA COM3 1 # NTRIP VRS 업링크용 +``` + +## 빌드 +```bash +colcon build --packages-select gnss_comm um982_driver +source install/setup.bash +``` + +## 실행 +```bash +# config/um982.yaml 에서 serial_port / NTRIP 자격증명 확인 후 +ros2 launch um982_driver um982_driver.launch.py +# 또는 자격증명을 CLI 로: +ros2 run um982_driver um982_driver_node --ros-args \ + --params-file src/um982_driver/config/um982.yaml \ + -p ntrip_user:= -p ntrip_pass:= +``` +`ros2 topic echo /ublox_driver/receiver_pvt` 로 발행 확인. 콘솔에 `RTK FIXED` 전환 로그가 뜬다. + +## BESTNAV → GnssPVTSolnMsg 매핑 +| GnssPVTSolnMsg | BESTNAV(§7.3.26) | 비고 | +|---|---|---| +| `time.week` / `time.tow` | 헤더 Wn / Ms÷1000 | UNIX 복원 → 센서 시간동기 | +| `latitude` / `longitude` | lat / lon | | +| `altitude` | hgt(MSL) + undulation | **타원체고** (백엔드 ENU 입력) | +| `height_msl` | hgt | | +| `h_acc` / `v_acc` | √(latσ²+lonσ²) / hgtσ | GTSAM 공분산 | +| `vel_n/e/d` | hor·cos(trk) / hor·sin(trk) / −vert | 속력·침로 → NED | +| `carr_soln` | pos_type: 48/49/50→2, 32/33/34→1 | RTK Fixed/Float | +| `fix_type`/`valid_fix`/`diff_soln`/`num_sv` | pos_type / #solnSVs | | + +## 파라미터 (config/um982.yaml) +- `serial_port`, `serial_baud`(460800), `gps_topic` +- `use_ntrip`, `ntrip_host/port/mountpoint/user/pass`, `gga_send_period` +- `publish_fixed_only`(기본 false), `inflate_cov_when_not_fixed`(기본 true) + +> 백엔드는 fix 품질을 거르지 않으므로, RTK Fixed 가 아닌 해는 `inflate_cov_when_not_fixed` +> 로 공분산을 키워 영향력을 줄이거나 `publish_fixed_only` 로 아예 배제한다. + +## 검증 +CRC(Appendix 1) 통과 프레임만 발행한다. 파서 로직은 `rtk/rtk_test.py`(파이썬 프로토타입)와 동일. +```bash +python3 rtk/rtk_test.py # 하드웨어 붙이기 전 파서/파이프라인 사전 검증 +``` diff --git a/src/um982_driver/RTK-녹화-명세.md b/src/um982_driver/RTK-녹화-명세.md new file mode 100644 index 0000000..9070d52 --- /dev/null +++ b/src/um982_driver/RTK-녹화-명세.md @@ -0,0 +1,200 @@ +# RTK(UM982) ROS 2 녹화 명세서 + +> 대상: UM982(WTRTK-982) RTK 수신 데이터를 ROS 2 로 발행·**녹화**하기 위한 이관/구성 명세. +> 구현체: `um982_driver` (C++ ROS 2 노드). 근거 문서: `rtk/UM982-명령어검토-및-바이너리포팅.md`. +> 검증: 국가 통합기준점 U0373/U0374 대비 수평 1.4~2.6cm (rtk/Log.md). + +--- + +## 1. 개요 & 데이터 흐름 + +``` +NTRIP(VRS,KGD2002) ──RTCM3──▶ UM982 ──BESTNAV(binary)+GGA(ascii)──▶ um982_driver + ▲ │ + └──────────────── GGA(VRS 유지, 10s) ◀───────────────────────────┘ + │ GnssPVTSolnMsg (10Hz) + ▼ + /ublox_driver/receiver_pvt + │ + ros2 bag record ──▶ .db3/.mcap +``` + +- 시리얼 포트는 **한 프로세스만 점유** → RTCM 주입과 BESTNAV 파싱이 **한 노드**(`um982_driver`)에 통합. +- 녹화는 이 노드가 발행하는 토픽을 `ros2 bag record` 로 담는다. + +--- + +## 2. 이관 대상 & 워크스페이스 구성 (무엇을 어떻게 옮기나) + +**옮길 패키지 2개** (그 외 의존 없음 — 시리얼=termios, NTRIP=POSIX 소켓): + +| 패키지 | 역할 | 위치(원본) | +|---|---|---| +| `gnss_comm` | `GnssPVTSolnMsg`/`GnssTimeMsg` 메시지 정의 | `FAST-LIVO2-RTK-ROS2/thirdparty/gnss_comm` | +| `um982_driver` | 드라이버 노드(파싱+NTRIP+발행) | `FAST-LIVO2-RTK-ROS2/src/um982_driver` | + +**녹화 전용 최소 워크스페이스** (전체 FAST-LIVO2 빌드 불필요): +``` +fhd_rtk_ws/ +└── src/ + ├── gnss_comm/ ← thirdparty/gnss_comm 복사 + └── um982_driver/ ← src/um982_driver 복사 +``` +```bash +cd fhd_rtk_ws +colcon build --packages-select gnss_comm um982_driver +source install/setup.bash +``` +> ROS 2 Humble, C++17. `um982_driver` 는 `rclcpp`, `gnss_comm` 만 의존. + +--- + +## 3. 사전 조건 — 수신기 설정 (필수) + +드라이버 실행 전 UM982 가 아래를 **저장(SAVECONFIG)** 하고 있어야 한다. 리포 `rtk/config_all.py` 한 번 실행: + +| 항목 | 값 | 이유 | +|---|---|---| +| `BESTNAVB COM3 0.1` | 10Hz 바이너리 | **파싱 소스** | +| `GPGGA COM3 1` | 1Hz | NTRIP VRS 업링크 | +| COM3 baud | `460800` | RTCM+로그 대역 | +| `MODE ROVER` | — | 로버 측위 | +| `CONFIG RTK RELIABILITY 3 1` | 기본 | 오확정 방지 | + +> ⚠️ `FRESET` 시 baud 가 115200 으로 초기화됨 → `rtk/set_baud.py` 재실행 필요. + +--- + +## 4. 토픽 & 메시지 명세 + +### 4.1 토픽 + +| 항목 | 값 | +|---|---| +| 토픽명 | `/ublox_driver/receiver_pvt` (파라미터 `gps_topic`) | +| 타입 | `gnss_comm/msg/GnssPVTSolnMsg` | +| 발행 주기 | BESTNAV 주기 = **10 Hz** (0.1s) | +| QoS | `KeepLast(2000)`, 기본 신뢰성(reliable) | +| 프레임/스탬프 | **메시지에 std_msgs/Header 없음.** 시간은 메시지 내부 `time.week/tow` | + +### 4.2 `GnssPVTSolnMsg` 필드 (드라이버가 채우는 값) + +| 필드 | 타입/단위 | 채움 | BESTNAV 소스 | +|---|---|---|---| +| `time.week` | uint32 (GPS week) | ✅ | 헤더 Wn | +| `time.tow` | float64 (초) | ✅ | 헤더 Ms÷1000 | +| `latitude` | float64 (deg) | ✅ | lat | +| `longitude` | float64 (deg) | ✅ | lon | +| `altitude` | float64 (m, **타원체고**) | ✅ | hgt(MSL)+undulation | +| `height_msl` | float64 (m) | ✅ | hgt | +| `h_acc` | float64 (m) | ✅ | √(latσ²+lonσ²) | +| `v_acc` | float64 (m) | ✅ | hgtσ | +| `vel_n/vel_e/vel_d` | float64 (m/s, NED) | ✅ | hor·cos(trk)/hor·sin(trk)/−vert | +| `vel_acc` | float64 (m/s) | ✅ | Horspd std | +| `num_sv` | uint8 | ✅ | #solnSVs | +| `fix_type` | uint8 (0=no,3=3D) | ✅ | pos_type | +| `valid_fix` | bool | ✅ | pos_type≠NONE | +| `diff_soln` | bool | ✅ | pos_type∉{NONE,SINGLE} | +| `carr_soln` | uint8 (0/1/2) | ✅ | 48/49/50→2, 32/33/34→1, else 0 | +| `p_dop` | float64 | ➖ 0 | BESTNAV 미제공(필요시 PVTSLN) | + +> `carr_soln`: **2=RTK Fixed(NARROW_INT 등), 1=RTK Float, 0=단독/DGPS**. + +--- + +## 5. 드라이버 파라미터 명세 (`config/um982.yaml`) + +| 파라미터 | 기본값 | 설명 | +|---|---|---| +| `serial_port` | `/dev/ttyUSB0` | UM982 COM3(USB). 리눅스 `/dev/ttyUSB*` | +| `serial_baud` | `460800` | 수신기 저장값과 일치 | +| `gps_topic` | `/ublox_driver/receiver_pvt` | 발행 토픽(백엔드 `gps.gps_topic` 와 일치) | +| `use_ntrip` | `true` | false 면 RTCM 주입 안 함(외부 보정 시) | +| `ntrip_host/port/mountpoint` | `RTS1.ngii.go.kr` / `2101` / `VRS-RTCM34` | NTRIP 접속 | +| `ntrip_user/pass` | `""` | ⚠️ 리포 평문 커밋 지양 — yaml/CLI/환경변수로 주입 | +| `gga_send_period` | `10.0` | VRS 유지용 GGA 재전송 주기(s) | +| `publish_fixed_only` | `false` | true=RTK Fixed 해만 발행 | +| `inflate_cov_when_not_fixed` | `true` | fix 아님/코스팅 시 h_acc·v_acc 부풀림 | +| `max_corr_age` | `30.0` | diff_age 초과 시 코스팅 간주(공분산↑). 0=비활성 | + +> NTRIP 끊겨도 UM982 는 `RTK TIMEOUT`(기본 600s) 동안 Fixed 를 코스팅 유지 → `max_corr_age` 로 신뢰 낮춤. + +--- + +## 6. 시간 의미 (중요) + +- 메시지에 Header/stamp 가 **없다.** 시간은 `time.week`(GPS week) + `time.tow`(주 내 초). +- UNIX 복원: `unix = week·604800 + tow + 315964800 − 18(leap)` (백엔드 `optimization.cpp::gpsHandler` 와 동일). +- 유효 조건: 헤더 TimeStatus=`FINE` & week>1 (fix 전에는 week=1/UNKNOWN). +- `ros2 bag` 은 **수신 시각(bag timestamp)** 을 별도로 기록하지만, **권위 시각은 메시지 내부 week/tow**. 센서와의 시각 동기는 이 값 기준. + +--- + +## 7. 녹화 절차 + +### 7.1 실행 +```bash +source install/setup.bash +ros2 launch um982_driver um982_driver.launch.py # 또는 아래 run +# 자격증명을 CLI 로 주입: +ros2 run um982_driver um982_driver_node --ros-args \ + --params-file src/um982_driver/config/um982.yaml \ + -p ntrip_user:= -p ntrip_pass:= +``` +콘솔에 `RTK FIXED` 전환 로그가 뜨고, `ros2 topic hz /ublox_driver/receiver_pvt` ≈ 10Hz 확인. + +### 7.2 GNSS 단독 녹화 +```bash +ros2 bag record -o rtk_$(date +%F_%H%M) /ublox_driver/receiver_pvt +``` + +### 7.3 센서 + GNSS 동시 녹화 (FAST-LIVO2 후처리용) +```bash +ros2 bag record -o run_$(date +%F_%H%M) \ + /livox/lidar /livox/imu /left_camera/image \ + /ublox_driver/receiver_pvt \ + --storage mcap # 없으면 sqlite3(기본) +``` +> 초기 수십 초 **RTK Fixed + 충분한 이동** 후 녹화(백엔드 SVD 정합 전제). `rtk/UM982-녹화-및-연동-구현.md` §7 참조. + +### 7.4 (권장) 원시 BESTNAV 동시 녹화 — 재처리 대비 +현재 드라이버는 파싱 결과만 발행한다. 파싱 로직을 나중에 고쳐 재생성하려면 **원시 바이트도 함께** 남기는 것을 권장(별도 옵션 필요 — §10). + +--- + +## 8. 녹화 데이터 포맷 & 재생 + +- 저장: ROS 2 bag (`sqlite3` `.db3` 기본, 또는 `mcap` `.mcap`). +- 확인: `ros2 bag info run_YYYY-MM-DD_HHMM` → 토픽/메시지 수/기간. +- 재생: `ros2 bag play run_YYYY-MM-DD_HHMM` → `/ublox_driver/receiver_pvt` 재발행. +- FAST-LIVO2 백엔드는 이 토픽을 구독해 첫 fix 를 ENU 원점으로 잡고 GTSAM 융합(`gps.gps_topic` 일치 필요). + +--- + +## 9. 품질/필터 (녹화되는 데이터의 신뢰) + +- 백엔드에 fix 품질 필터가 **없으므로** 드라이버 단에서 처리: + - `publish_fixed_only=true` → RTK Fixed 해만 녹화(가장 보수적). + - `inflate_cov_when_not_fixed=true` + `max_corr_age` → fix 아님/코스팅 시 `h_acc/v_acc` 를 키워 후처리 가중 약화. +- CRC(Unicore Appendix 1) 통과 프레임만 발행 → 손상 프레임 녹화 안 됨. + +--- + +## 10. 확장(옵션) — 원시 스트림 동시 녹화 + +재처리·디버깅을 위해 원시 BESTNAV/GGA 바이트를 별도 토픽으로 녹화하려면 드라이버에 +`std_msgs/msg/ByteMultiArray`(또는 String) 발행부를 추가하면 된다(예: `/um982/raw`). +필요 시 요청 → 드라이버에 `publish_raw` 파라미터로 추가. + +--- + +## 11. 검증 체크리스트 + +- [ ] `rtk/config_all.py` 로 수신기 설정 저장(BESTNAVB/GPGGA, 460800, MODE ROVER) +- [ ] `colcon build --packages-select gnss_comm um982_driver` 성공 +- [ ] `serial_port` / NTRIP 자격증명(config or CLI) 확인 +- [ ] `ros2 topic hz /ublox_driver/receiver_pvt` ≈ 10Hz, 콘솔 `RTK FIXED` +- [ ] `ros2 topic echo` 로 lat/lon/altitude, time.week(>1)/tow, carr_soln=2 확인 +- [ ] `ros2 bag record` → `ros2 bag info` 로 토픽·기간 확인 +- [ ] (동시녹화) 센서 4종 + GNSS 토픽 모두 bag 에 존재 +``` diff --git a/src/um982_driver/config/um982.yaml b/src/um982_driver/config/um982.yaml new file mode 100644 index 0000000..dd75087 --- /dev/null +++ b/src/um982_driver/config/um982.yaml @@ -0,0 +1,23 @@ +um982_driver: + ros__parameters: + # ── 시리얼 (UM982 COM3 = USB) ── + serial_port: "/dev/ttyUSB0" # 실제 포트로. macOS 는 /dev/tty.usbserial-*, 리눅스 /dev/ttyUSB0 + serial_baud: 460800 # config_all.py / set_baud.py 와 일치 + + # ── 발행 토픽 (백엔드 gps.gps_topic 와 일치) ── + gps_topic: "/ublox_driver/receiver_pvt" + + # ── NTRIP (VRS) ── + use_ntrip: true + ntrip_host: "RTS1.ngii.go.kr" + ntrip_port: 2101 + ntrip_mountpoint: "VRS-RTCM34" + ntrip_user: "" # ⚠️ 여기에 채우거나 launch 인자/환경변수로 주입(리포에 평문 저장 지양) + ntrip_pass: "" + gga_send_period: 10.0 # VRS 유지용 GGA 재전송 주기(초) + + # ── 품질 처리 (백엔드에 fix 품질 필터가 없음) ── + publish_fixed_only: false # true = RTK Fixed(carr_soln=2) 해만 발행 + inflate_cov_when_not_fixed: true # fix 아니면(또는 코스팅 중) h_acc/v_acc 부풀려 GTSAM 가중 약화 + max_corr_age: 30.0 # 보정 나이(diff_age) 초과 시 코스팅으로 간주해 공분산 부풀림. 0=비활성 + # NTRIP 끊겨도 UM982 는 RTK TIMEOUT(기본 600s) 동안 Fixed 유지하므로 필요 diff --git a/src/um982_driver/launch/um982_driver.launch.py b/src/um982_driver/launch/um982_driver.launch.py new file mode 100644 index 0000000..60dca70 --- /dev/null +++ b/src/um982_driver/launch/um982_driver.launch.py @@ -0,0 +1,29 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + default_cfg = os.path.join( + get_package_share_directory('um982_driver'), 'config', 'um982.yaml') + cfg = LaunchConfiguration('config') + + return LaunchDescription([ + DeclareLaunchArgument('config', default_value=default_cfg, + description='파라미터 yaml 경로'), + Node( + package='um982_driver', + executable='um982_driver_node', + name='um982_driver', + output='screen', + parameters=[cfg], + ), + ]) + +# NTRIP 자격증명은 config yaml 에 넣거나(리포에 평문 커밋 지양), 실행 시 덮어쓰기: +# ros2 launch um982_driver um982_driver.launch.py +# ros2 run um982_driver um982_driver_node --ros-args \ +# --params-file .yaml -p ntrip_user:= -p ntrip_pass:= diff --git a/src/um982_driver/package.xml b/src/um982_driver/package.xml new file mode 100644 index 0000000..299f5b3 --- /dev/null +++ b/src/um982_driver/package.xml @@ -0,0 +1,24 @@ + + + + um982_driver + 0.1.0 + + UM982 (Unicore) GNSS/RTK driver for FAST-LIVO2-RTK. Reads BESTNAV binary + over serial, feeds NTRIP RTCM3 corrections back to the receiver, and + publishes gnss_comm/msg/GnssPVTSolnMsg on the topic the backend subscribes to. + + FAST-LIVO2-RTK + GPLv3 + + ament_cmake + + rclcpp + gnss_comm + + ros2launch + + + ament_cmake + + diff --git a/src/um982_driver/src/um982_driver_node.cpp b/src/um982_driver/src/um982_driver_node.cpp new file mode 100644 index 0000000..a99da2a --- /dev/null +++ b/src/um982_driver/src/um982_driver_node.cpp @@ -0,0 +1,374 @@ +// UM982 (Unicore) driver for FAST-LIVO2-RTK (ROS 2 Humble) +// --------------------------------------------------------------------------- +// 한 노드에서: +// 1) UM982 시리얼(COM3) 오픈 — BESTNAV(binary) + GGA(ascii) 혼합 스트림 수신 +// 2) NTRIP(VRS) 접속 → RTCM3 을 같은 시리얼로 주입(별도 스레드, 자동 재접속) +// 3) 첫 유효 GGA 로 VRS 부트스트랩, 이후 주기적 재전송 +// 4) BESTNAV 를 파싱해 gnss_comm/msg/GnssPVTSolnMsg 로 발행 (gps.gps_topic) +// +// 시리얼 포트는 한 프로세스만 점유 가능하므로 RTCM 주입과 파싱이 반드시 한 노드에 있어야 한다. +// 파서 오프셋 근거: Unicore N4 Reference Commands Manual, §7.3.26(BESTNAV)/Table 7-49(헤더). +// 대상 필드 근거: optimization.cpp::gpsHandler (latitude/longitude/altitude, time, vel, h/v_acc). +// +// 의존성: rclcpp, gnss_comm (시리얼=termios, NTRIP=POSIX 소켓 — 외부 라이브러리 불필요) +// 가정: 호스트가 little-endian (x86_64/aarch64 리눅스 — Unicore 바이너리도 LE). + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using gnss_comm::msg::GnssPVTSolnMsg; + +namespace { + +// ── BESTNAV 바이너리 레이아웃 (little-endian) ────────────────────────────── +constexpr uint8_t SYNC0 = 0xAA, SYNC1 = 0x44, SYNC2 = 0xB5; +constexpr size_t HDR_LEN = 24; // 동기 3B 포함 헤더 총 길이 +constexpr uint16_t BESTNAV_ID = 2118; + +// 헤더 오프셋 +constexpr size_t OFF_MSGID = 4, OFF_MSGLEN = 6, OFF_TSTAT = 9, OFF_WEEK = 10, OFF_MS = 12; +// 본문(절대) 오프셋 = HDR_LEN + 표(§3.3) 상대오프셋 +constexpr size_t OFF_POSTYPE = 28; +constexpr size_t OFF_LAT = 32, OFF_LON = 40, OFF_HGT = 48, OFF_UNDU = 56; +constexpr size_t OFF_LATSTD = 64, OFF_LONSTD = 68, OFF_HGTSTD = 72; +constexpr size_t OFF_DIFFAGE = 80, OFF_NSOLN = 89; +constexpr size_t OFF_HORSPD = 112, OFF_TRKGND = 120, OFF_VERTSPD = 128, OFF_HORSPDSTD = 140; + +// Position/Velocity Type (Table 0-4) +constexpr int32_t POS_NONE = 0, POS_SINGLE = 16; +inline bool is_rtk_fixed(int32_t t) { return t == 48 || t == 49 || t == 50; } +inline bool is_rtk_float(int32_t t) { return t == 32 || t == 33 || t == 34; } + +template +inline T rd(const uint8_t *p, size_t off) { T v; std::memcpy(&v, p + off, sizeof(T)); return v; } + +// NovAtel/Unicore 32-bit CRC (Appendix 1) +uint32_t crc32_value(int i) { + uint32_t crc = static_cast(i); + for (int j = 8; j > 0; --j) + crc = (crc & 1) ? (crc >> 1) ^ 0xEDB88320UL : (crc >> 1); + return crc; +} +uint32_t block_crc32(const uint8_t *buf, size_t n) { + uint32_t crc = 0; + while (n-- != 0) { + uint32_t t1 = (crc >> 8) & 0x00FFFFFFUL; + uint32_t t2 = crc32_value((static_cast(crc) ^ *buf++) & 0xFF); + crc = t1 ^ t2; + } + return crc; +} + +speed_t to_speed(int baud) { + switch (baud) { + case 9600: return B9600; case 19200: return B19200; + case 38400: return B38400; case 57600: return B57600; + case 115200: return B115200; case 230400: return B230400; + case 460800: return B460800; case 921600: return B921600; + default: return B460800; + } +} + +std::string base64(const std::string &in) { + static const char *T = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + std::string out; + int val = 0, bits = -6; + for (unsigned char c : in) { + val = (val << 8) + c; bits += 8; + while (bits >= 0) { out.push_back(T[(val >> bits) & 0x3F]); bits -= 6; } + } + if (bits > -6) out.push_back(T[((val << 8) >> (bits + 8)) & 0x3F]); + while (out.size() % 4) out.push_back('='); + return out; +} + +} // namespace + +class Um982Driver : public rclcpp::Node { + public: + Um982Driver() : Node("um982_driver") { + // ── 파라미터 ── + serial_port_ = declare_parameter("serial_port", "/dev/ttyUSB0"); + serial_baud_ = declare_parameter("serial_baud", 460800); + gps_topic_ = declare_parameter("gps_topic", "/ublox_driver/receiver_pvt"); + ntrip_host_ = declare_parameter("ntrip_host", "RTS1.ngii.go.kr"); + ntrip_port_ = declare_parameter("ntrip_port", 2101); + ntrip_user_ = declare_parameter("ntrip_user", ""); + ntrip_pass_ = declare_parameter("ntrip_pass", ""); + ntrip_mp_ = declare_parameter("ntrip_mountpoint", "VRS-RTCM34"); + use_ntrip_ = declare_parameter("use_ntrip", true); + fixed_only_ = declare_parameter("publish_fixed_only", false); + inflate_cov_ = declare_parameter("inflate_cov_when_not_fixed", true); + max_corr_age_ = declare_parameter("max_corr_age", 30.0); // 보정 나이 초과 시 코스팅으로 간주 + gga_period_ = declare_parameter("gga_send_period", 10.0); + + pub_ = create_publisher(gps_topic_, rclcpp::QoS(rclcpp::KeepLast(2000))); + + if (!open_serial()) { + RCLCPP_FATAL(get_logger(), "시리얼 오픈 실패: %s", serial_port_.c_str()); + throw std::runtime_error("serial open failed"); + } + RCLCPP_INFO(get_logger(), "UM982 %s @ %d → 발행 %s", + serial_port_.c_str(), serial_baud_, gps_topic_.c_str()); + + running_ = true; + serial_thread_ = std::thread(&Um982Driver::serial_loop, this); + if (use_ntrip_) ntrip_thread_ = std::thread(&Um982Driver::ntrip_loop, this); + } + + ~Um982Driver() override { + running_ = false; + if (serial_thread_.joinable()) serial_thread_.join(); + if (ntrip_thread_.joinable()) ntrip_thread_.join(); + if (serial_fd_ >= 0) ::close(serial_fd_); + if (ntrip_fd_ >= 0) ::close(ntrip_fd_); + } + + private: + // ── 시리얼 ────────────────────────────────────────────────────────────── + bool open_serial() { + serial_fd_ = ::open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (serial_fd_ < 0) return false; + termios tio{}; + if (tcgetattr(serial_fd_, &tio) != 0) return false; + cfmakeraw(&tio); + cfsetispeed(&tio, to_speed(serial_baud_)); + cfsetospeed(&tio, to_speed(serial_baud_)); + tio.c_cflag |= (CLOCAL | CREAD); + tio.c_cflag &= ~CRTSCTS; + tio.c_cc[VMIN] = 0; + tio.c_cc[VTIME] = 0; + return tcsetattr(serial_fd_, TCSANOW, &tio) == 0; + } + + void serial_loop() { + std::vector buf; + buf.reserve(1 << 16); + uint8_t tmp[4096]; + while (running_ && rclcpp::ok()) { + ssize_t n = ::read(serial_fd_, tmp, sizeof(tmp)); + if (n > 0) buf.insert(buf.end(), tmp, tmp + n); + else { std::this_thread::sleep_for(std::chrono::milliseconds(5)); } + demux(buf); + if (buf.size() > (1 << 18)) buf.erase(buf.begin(), buf.end() - 256); // 안전장치 + } + } + + // BESTNAV(binary) 프레임과 GGA(ascii) 라인을 분리 + void demux(std::vector &buf) { + size_t search = 0; + while (true) { + // 다음 동기 위치 + size_t sync = find_sync(buf, search); + // 동기 이전(head)에서 GGA 추출 + size_t head_end = (sync == std::string::npos) ? buf.size() : sync; + scan_gga(buf, search, head_end); + + if (sync == std::string::npos) { + if (buf.size() > 2) buf.erase(buf.begin(), buf.end() - 2); // 동기 경계 보존 + return; + } + if (buf.size() - sync < HDR_LEN) { buf.erase(buf.begin(), buf.begin() + sync); return; } + uint16_t mlen = rd(buf.data(), sync + OFF_MSGLEN); + size_t total = HDR_LEN + mlen + 4; + if (buf.size() - sync < total) { buf.erase(buf.begin(), buf.begin() + sync); return; } + + const uint8_t *f = buf.data() + sync; + if (rd(f, OFF_MSGID) == BESTNAV_ID) { + uint32_t got = rd(f, HDR_LEN + mlen); + if (block_crc32(f, HDR_LEN + mlen) == got) handle_bestnav(f); + else RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "BESTNAV CRC 불일치 — 스킵"); + } + buf.erase(buf.begin(), buf.begin() + sync + total); + search = 0; + } + } + + static size_t find_sync(const std::vector &b, size_t from) { + for (size_t i = from; i + 2 < b.size(); ++i) + if (b[i] == SYNC0 && b[i + 1] == SYNC1 && b[i + 2] == SYNC2) return i; + return std::string::npos; + } + + // buf[begin,end) 구간에서 $G?GGA 라인을 찾아 최신 GGA(fix 유효분) 갱신 + void scan_gga(const std::vector &buf, size_t begin, size_t end) { + for (size_t p = begin; p + 4 < end; ++p) { + if (buf[p] != '$') continue; + if (!(p + 6 < end && buf[p + 3] == 'G' && buf[p + 4] == 'G' && buf[p + 5] == 'A')) continue; + size_t e = p; + while (e < end && buf[e] != '\r' && buf[e] != '\n') ++e; + if (e >= end) break; // 종결문자 없음 = 라인 미완 → 다음 read 까지 대기(잘린 GGA 전송 방지) + std::string line(reinterpret_cast(&buf[p]), e - p); + // field 6 = fix quality + int comma = 0; size_t i = 0; std::string fix; + for (; i < line.size(); ++i) { + if (line[i] == ',') { if (++comma == 6) { size_t s = i + 1; while (s < line.size() && line[s] != ',') fix.push_back(line[s++]); break; } } + } + if (!fix.empty() && fix != "0") { + std::lock_guard lk(gga_mtx_); + latest_gga_ = line + "\r\n"; + } + p = e; + } + } + + void handle_bestnav(const uint8_t *f) { + int32_t pos_type = rd(f, OFF_POSTYPE); + if (fixed_only_ && !is_rtk_fixed(pos_type)) return; + + GnssPVTSolnMsg m; + m.time.week = rd(f, OFF_WEEK); + m.time.tow = rd(f, OFF_MS) / 1000.0; + + double lat = rd(f, OFF_LAT); + double lon = rd(f, OFF_LON); + double hgt = rd(f, OFF_HGT); // MSL + float undu = rd(f, OFF_UNDU); + m.latitude = lat; + m.longitude = lon; + m.altitude = hgt + undu; // 타원체고 (백엔드 ENU 입력) + m.height_msl = hgt; + + float lat_s = rd(f, OFF_LATSTD); + float lon_s = rd(f, OFF_LONSTD); + float hgt_s = rd(f, OFF_HGTSTD); + float diff_age = rd(f, OFF_DIFFAGE); // 보정 나이(s). 커지면 코스팅 중 + m.h_acc = std::hypot(lat_s, lon_s); + m.v_acc = hgt_s; + + double hor = rd(f, OFF_HORSPD); + double trk = rd(f, OFF_TRKGND) * M_PI / 180.0; + double vert = rd(f, OFF_VERTSPD); + m.vel_n = hor * std::cos(trk); + m.vel_e = hor * std::sin(trk); + m.vel_d = -vert; // BESTNAV vert(+up) → NED down + m.vel_acc = rd(f, OFF_HORSPDSTD); + + m.num_sv = f[OFF_NSOLN]; + m.fix_type = (pos_type == POS_NONE) ? 0 : 3; + m.valid_fix = (pos_type != POS_NONE); + m.diff_soln = (pos_type != POS_NONE && pos_type != POS_SINGLE); + m.carr_soln = is_rtk_fixed(pos_type) ? 2 : (is_rtk_float(pos_type) ? 1 : 0); + m.p_dop = 0.0; + + // 신뢰 낮은 해는 공분산 부풀려 백엔드 가중 약화(백엔드에 품질 필터 없음): + // (1) RTK Fixed 가 아니거나, (2) 보정이 끊겨 코스팅 중(diff_age 과다) + bool coasting = (max_corr_age_ > 0.0 && diff_age > max_corr_age_); + if (inflate_cov_ && (m.carr_soln != 2 || coasting)) { + m.h_acc = std::max(m.h_acc, 0.5); + m.v_acc = std::max(m.v_acc, 1.0); + } + + pub_->publish(m); + + if (pos_type != last_type_) { + RCLCPP_INFO(get_logger(), "상태: %s(pos_type=%d) | lat=%.7f lon=%.7f alt=%.2f hσ=%.3fm nSV=%u diff_age=%.1fs", + is_rtk_fixed(pos_type) ? "RTK FIXED" : is_rtk_float(pos_type) ? "RTK FLOAT" + : pos_type == POS_SINGLE ? "SINGLE" : pos_type == POS_NONE ? "NONE" : "OTHER", + pos_type, lat, lon, m.altitude, m.h_acc, m.num_sv, diff_age); + last_type_ = pos_type; + } + } + + // ── NTRIP ─────────────────────────────────────────────────────────────── + void ntrip_loop() { + // 첫 유효 GGA 대기(VRS 부트스트랩) + while (running_ && rclcpp::ok() && latest_gga().empty()) + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + auto last_send = std::chrono::steady_clock::now(); + while (running_ && rclcpp::ok()) { + if (ntrip_fd_ < 0) { + if (!ntrip_connect()) { std::this_thread::sleep_for(std::chrono::seconds(2)); continue; } + RCLCPP_INFO(get_logger(), "NTRIP 접속: %s:%d/%s", ntrip_host_.c_str(), ntrip_port_, ntrip_mp_.c_str()); + last_send = std::chrono::steady_clock::now(); + } + uint8_t buf[4096]; + ssize_t n = ::recv(ntrip_fd_, buf, sizeof(buf), 0); + if (n > 0) { + ::write(serial_fd_, buf, n); // RTCM 주입 + ntrip_bytes_total_ += n; + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, + "NTRIP 수신 누적 %ld bytes (최근 recv %zd bytes)", + ntrip_bytes_total_, n); + } else if (n == 0) { + RCLCPP_WARN(get_logger(), "NTRIP 연결 종료 → 재접속"); + ::close(ntrip_fd_); ntrip_fd_ = -1; continue; + } + // VRS 유지용 GGA 주기 재전송 + auto now = std::chrono::steady_clock::now(); + if (std::chrono::duration(now - last_send).count() > gga_period_) { + std::string g = latest_gga(); + if (!g.empty()) ::send(ntrip_fd_, g.data(), g.size(), MSG_NOSIGNAL); + last_send = now; + } + } + } + + bool ntrip_connect() { + addrinfo hints{}, *res = nullptr; + hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM; + if (getaddrinfo(ntrip_host_.c_str(), std::to_string(ntrip_port_).c_str(), &hints, &res) != 0) return false; + int fd = ::socket(res->ai_family, res->ai_socktype, res->ai_protocol); + timeval tv{5, 0}; + setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); + bool ok = (::connect(fd, res->ai_addr, res->ai_addrlen) == 0); + freeaddrinfo(res); + if (!ok) { ::close(fd); return false; } + + std::string auth = base64(ntrip_user_ + ":" + ntrip_pass_); + std::string req = "GET /" + ntrip_mp_ + " HTTP/1.0\r\n" + "User-Agent: NTRIP um982_driver/1.0\r\nAccept: */*\r\n" + "Connection: close\r\nAuthorization: Basic " + auth + "\r\n\r\n"; + ::send(fd, req.data(), req.size(), MSG_NOSIGNAL); + char resp[1024]; ssize_t r = ::recv(fd, resp, sizeof(resp) - 1, 0); + if (r <= 0) { ::close(fd); return false; } + resp[r] = 0; + if (!strstr(resp, "200 OK") && !strstr(resp, "ICY 200 OK")) { ::close(fd); return false; } + std::string g = latest_gga(); + if (!g.empty()) ::send(fd, g.data(), g.size(), MSG_NOSIGNAL); + ntrip_fd_ = fd; + return true; + } + + std::string latest_gga() { std::lock_guard lk(gga_mtx_); return latest_gga_; } + + // 파라미터 + std::string serial_port_, gps_topic_, ntrip_host_, ntrip_user_, ntrip_pass_, ntrip_mp_; + int serial_baud_{}, ntrip_port_{}; + bool use_ntrip_{}, fixed_only_{}, inflate_cov_{}; + double gga_period_{}, max_corr_age_{}; + + rclcpp::Publisher::SharedPtr pub_; + int serial_fd_{-1}; + std::atomic ntrip_fd_{-1}; + long ntrip_bytes_total_{0}; + std::atomic running_{false}; + std::thread serial_thread_, ntrip_thread_; + std::mutex gga_mtx_; + std::string latest_gga_; + int32_t last_type_{-999}; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}