azmo/fhd_rtk_ws
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add um982_driver + gnss_comm (renamed from rtk_ws to fhd_rtk_ws)

Browse Source 
UM982 RTK GNSS driver workspace used by the triple-camera scan GUI for
recording /ublox_driver/receiver_pvt alongside LiDAR/camera bags. Ported
from FAST-LIVO2-RTK-ROS2's um982_driver spec, adapted for this UM982
receiver instead of the original repo's GNSS module.
This commit is contained in:
hjkim
2026-08-07 14:15:06 +09:00
parent 0c47bb4909
commit be9e83d2f4
15 changed files with 1100 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+7
View File
@@ -0,0 +1,7 @@
# colcon build artifacts — regenerate with `colcon build`
build/
install/
log/
__pycache__/
*.pyc
docs/SCAN-운용가이드.md
+120
View File
@@ -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 <PID>
```
### 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 융합을 합치려면 두 소스를
비교해 병합하는 별도 작업이 필요하다 — 착수 시 다시 요청.
docs/UM982-RTK-Fixed-진단기록_2026-07-30.md
+160
View File
@@ -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`로 하드코딩돼 있어 포트 바뀌면 수정 필요) |
docs/Unicore Reference Commands Manual For N4 High Precision Products_V2_EN_R1.4.pdf
BIN
View File
Binary file not shown.
src/gnss_comm/CMakeLists.txt
+17
View File
@@ -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()
src/gnss_comm/msg/GnssPVTSolnMsg.msg
+22
View File
@@ -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]
src/gnss_comm/msg/GnssTimeMsg.msg
+5
View File
@@ -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
src/gnss_comm/package.xml
+25
View File
@@ -0,0 +1,25 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>gnss_comm</name>
<version>1.0.0</version>
<description>
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.
</description>
<maintainer email="noreply@example.com">FAST-LIVO2-RTK ROS2 port</maintainer>
<license>GPLv3</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
src/um982_driver/CMakeLists.txt
+26
View File
@@ -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()
src/um982_driver/README.md
+68
View File
@@ -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:=<id> -p ntrip_pass:=<pw>
```
`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 # 하드웨어 붙이기 전 파서/파이프라인 사전 검증
```
src/um982_driver/RTK-녹화-명세.md
+200
View File
@@ -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:=<id> -p ntrip_pass:=<pw>
```
콘솔에 `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 에 존재
```
src/um982_driver/config/um982.yaml
+23
View File
@@ -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 유지하므로 필요
src/um982_driver/launch/um982_driver.launch.py
+29
View File
@@ -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 <cfg>.yaml -p ntrip_user:=<id> -p ntrip_pass:=<pw>
src/um982_driver/package.xml
+24
View File
@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>um982_driver</name>
<version>0.1.0</version>
<description>
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.
</description>
<maintainer email="noreply@example.com">FAST-LIVO2-RTK</maintainer>
<license>GPLv3</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>gnss_comm</depend>
<exec_depend>ros2launch</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
src/um982_driver/src/um982_driver_node.cpp
+374
View File
@@ -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 <rclcpp/rclcpp.hpp>
#include <gnss_comm/msg/gnss_pvt_soln_msg.hpp>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <netdb.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <algorithm>
#include <atomic>
#include <cmath>
#include <cstring>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
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 <typename T>
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<uint32_t>(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<int>(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<std::string>("serial_port", "/dev/ttyUSB0");
serial_baud_ = declare_parameter<int>("serial_baud", 460800);
gps_topic_ = declare_parameter<std::string>("gps_topic", "/ublox_driver/receiver_pvt");
ntrip_host_ = declare_parameter<std::string>("ntrip_host", "RTS1.ngii.go.kr");
ntrip_port_ = declare_parameter<int>("ntrip_port", 2101);
ntrip_user_ = declare_parameter<std::string>("ntrip_user", "");
ntrip_pass_ = declare_parameter<std::string>("ntrip_pass", "");
ntrip_mp_ = declare_parameter<std::string>("ntrip_mountpoint", "VRS-RTCM34");
use_ntrip_ = declare_parameter<bool>("use_ntrip", true);
fixed_only_ = declare_parameter<bool>("publish_fixed_only", false);
inflate_cov_ = declare_parameter<bool>("inflate_cov_when_not_fixed", true);
max_corr_age_ = declare_parameter<double>("max_corr_age", 30.0); // 보정 나이 초과 시 코스팅으로 간주
gga_period_ = declare_parameter<double>("gga_send_period", 10.0);
pub_ = create_publisher<GnssPVTSolnMsg>(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<uint8_t> 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<uint8_t> &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<uint16_t>(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<uint16_t>(f, OFF_MSGID) == BESTNAV_ID) {
uint32_t got = rd<uint32_t>(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<uint8_t> &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<uint8_t> &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<const char *>(&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<std::mutex> lk(gga_mtx_);
latest_gga_ = line + "\r\n";
}
p = e;
}
}
void handle_bestnav(const uint8_t *f) {
int32_t pos_type = rd<int32_t>(f, OFF_POSTYPE);
if (fixed_only_ && !is_rtk_fixed(pos_type)) return;
GnssPVTSolnMsg m;
m.time.week = rd<uint16_t>(f, OFF_WEEK);
m.time.tow = rd<uint32_t>(f, OFF_MS) / 1000.0;
double lat = rd<double>(f, OFF_LAT);
double lon = rd<double>(f, OFF_LON);
double hgt = rd<double>(f, OFF_HGT); // MSL
float undu = rd<float>(f, OFF_UNDU);
m.latitude = lat;
m.longitude = lon;
m.altitude = hgt + undu; // 타원체고 (백엔드 ENU 입력)
m.height_msl = hgt;
float lat_s = rd<float>(f, OFF_LATSTD);
float lon_s = rd<float>(f, OFF_LONSTD);
float hgt_s = rd<float>(f, OFF_HGTSTD);
float diff_age = rd<float>(f, OFF_DIFFAGE); // 보정 나이(s). 커지면 코스팅 중
m.h_acc = std::hypot(lat_s, lon_s);
m.v_acc = hgt_s;
double hor = rd<double>(f, OFF_HORSPD);
double trk = rd<double>(f, OFF_TRKGND) * M_PI / 180.0;
double vert = rd<double>(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<float>(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<double>(m.h_acc, 0.5);
m.v_acc = std::max<double>(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<double>(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<std::mutex> 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<GnssPVTSolnMsg>::SharedPtr pub_;
int serial_fd_{-1};
std::atomic<int> ntrip_fd_{-1};
long ntrip_bytes_total_{0};
std::atomic<bool> 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<Um982Driver>());
rclcpp::shutdown();
return 0;
}