【備忘録】ROS2でRTK-GNSS

Ubuntu22.04のROS2 humbleの環境で，RTK-GNSSを試したい〜，というときに毎回設定方法忘れてしまうので，書き留めておく．

# 実行環境

- OS：Ubuntu 22.04.4 LTS
- ミドルウェア：ros2 humble
- RTKモジュール：ublox-f9p

## RTKモジュールキット

RTKモジュールをそのまま使うのは面倒だったので基板になっているセットを購入．
今回はSparkFun GPS-RTK-SMA キットを2個使った．他のキットでもOK．

URL: https://www.switch-science.com/products/8288?_pos=7&_sid=44942958c&_ss=r

# 基準局・移動局を設定するための事前作業

## 1. wineのインストール

以下の記事にしたがって，Wineを導入する．

```sh
sudo dpkg --add-architecture i386
sudo wget -nc -O /etc/apt/keyrings/winehq-archive.key https://dl.winehq.org/wine-builds/winehq.key
sudo wget -nc -P /etc/apt/sources.list.d/ https://dl.winehq.org/wine-builds/ubuntu/dists/jammy/winehq-jammy.sources
sudo apt update
sudo apt install --install-recommends winehq-stable
```

参考：https://www.miraiserver.ne.jp/column/about_ubuntu-wine/

## 2. wineで，/dev/ttyACM*をcom*として認識させる設定

wineではシリアルデバイスはcom*として認識しないと使えない． comは1から番号が振られるので注意．
wineのデバイスは"~/./wine/dosdevices"のシンボリックリンクで管理されてるので，リンクを貼ればOK！
ただし，再起動ごとにリンクの貼り直しが必要になる．

例えば，/dev/ttyACM0をwineでcom1として認識させたい場合は，
```sh
ln -sf /dev/ttyACM0 ~/.wine/dosdevices/com1
```

上記で認識されなくて困ってる，もしくは，毎回めんどい場合はレジストリで定義してしまえばOK！

```sh
wine regedit
```

「HKEY_LOCAL_MACHINE/Software/Wine/Ports」
の中に，
「COM1 REG_SZ /dev/ttyACM0」
のキーを新規作成．その後は再起動する．

![Screenshot from 2024-07-23 10-02-45.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/bd298d87-11cb-9432-7bb6-393b1d3c4758.png)

## 3. u-centerのインストール

以下のサイトからu-center（**u-center2ではない方**）をダウンロードし，wineでインストール．
ここではバージョン24.05をダウンロードした．

u-center: https://www.u-blox.com/en/product/u-center

## 4. F9Pへの読み書き権限を付与

毎回めんどくさいので，USBを刺したら読み書きを許可するようにudevを書き換える．
もちろん，毎回許可を与えるという方針なら，いらない．

```/lib/udev/rules.d/50-udev-default.rules
KERNEL=="tty[A-Z]*[0-9]|pppox[0-9]*|ircomm[0-9]*|noz[0-9]*|rfcomm[0-9]*", GROUP="dialout"
```
↑の箇所を↓に書き換える
```/lib/udev/rules.d/50-udev-default.rules
KERNEL=="tty[A-Z]*[0-9]|pppox[0-9]*|ircomm[0-9]*|noz[0-9]*|rfcomm[0-9]*", GROUP="dialout", MODE="0777"
```

書き換えたら再起動．

参考：https://www.robotech-note.com/entry/2018/07/23/ubuntu%E3%81%A7USB%E3%82%B7%E3%83%AA%E3%82%A2%E3%83%AB%E3%83%87%E3%83%90%E3%82%A4%E3%82%B9%EF%BC%88/dev/ttyACM0%E7%AD%89%EF%BC%89%E6%8E%A5%E7%B6%9A%E6%99%82%E3%81%AB%E8%87%AA%E5%8B%95%E3%81%A7%E6%9B%B8

# 基準局の設定

u-centerを開き，
Receiver->Connectionから，comポートを選択．（ここではcom1）
Receiver->Baudrateを230400に設定．
Receiver->Protocol FilterをGeneration 9に設定．

もしコンフィグを初期化したいなら，
Receiver->Action->Revert Configを押す．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/d7e0feff-f2ec-8c9f-b041-aeaa95818b5c.png)

## GNSSの設定
View->Generation Configuration 9を開いて，
GNSS Configurationを以下のように入力し，
Write to layerのRAM,BBR，FlashにすべてチェックをつけてSend Configurationする．

![Screenshot from 2024-07-23 10-54-36.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/47ddfa49-b471-362d-780a-b7886f91074a.png)


## 出力するNMEAデータの設定

View->Message Viewを開く．これ以降の作業は，このMessage Viewで操作する．

以下の項目を右クリックして，Enable Messageする．
それ以外はDisable Messageする．
GxTXTは消せないので，無視．
- NMEA->GxGGA（時刻や位置とGPS関連の情報をまとめたセンテンス）

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/ab8ed772-6b22-e3eb-5dec-838d11798ee4.png)

## NAV5の設定

UBX->CFG->NAV5の中の設定を変更する．
- Dynamic Model: 0 - Portable

書き換えたら，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/1bea298e-d08a-4612-813e-ee0914f33eaf.png)

※TMODE3を書き込むと，自動的にDynamic Modelが2-Stationaryになる？

## NMEAプロトコルの設定

UBX->CFG->NMEAの中の設定を変更する．
- Mode Flags：High precision mode
- Numbering used for SVs not〜：1- Extended(3 digit)

書き換えたら，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/c857fbad-9693-cf2e-64ab-54c6c9293ec5.png)

## 出力Rateの変更

UBX->CFG->RATEの中の設定を変更する．
- Measurement Period: 1000 ms

書き換えたら，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/e19da141-df8f-90f5-4f9b-5450b6a39aa5.png)

## TMODE3の変更

基準局には，自身の緯度経度のデータが必要だけど，毎回計測するか，固定値を入力するかを選べる．

### 計測する場合（Survey In）

- Mode: 1-Survey-in
- Minimum Observation Time: 60s (最小計測時間)
- Required Position Accurary：1.5m（必要な計測精度）

書き換えたら，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/dd748162-5cf0-b5e9-773a-08dc6c998654.png)

### 固定値を入力する場合

- Mode: 2-Fixed Mode
- Fixed Position: 事前にSurvey Inで得られたデータ

書き換えたら，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/0a53fe1a-7e49-63a0-6957-ff8372e57102.png)

## RTCM3データの設定

基準局から出す補正データの設定をする．

UBX->CFG->VALSETを開き，CFG-MSGOUのGroupにある，RTCM_3X_***のKeyNameを追加していく．
- CFG-MSGOUT-RTCM_3X_TYPE1005_USB (Value：1）
- CFG-MSGOUT-RTCM_3X_TYPE1074_USB (Value：1）
- CFG-MSGOUT-RTCM_3X_TYPE1084_USB (Value：1）
- CFG-MSGOUT-RTCM_3X_TYPE1094_USB (Value：1）
- CFG-MSGOUT-RTCM_3X_TYPE1124_USB (Value：1）
- CFG-MSGOUT-RTCM_3X_TYPE1230_USB (Value：1）

書き換えたら，Set value in layersのFlash,BBR,RAMすべてにチェックを入れて，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/7d6504d1-efbb-1ec1-5c37-69b0c57db9b6.png)

正しく設定できると，RTCM3の中のMSM4系の項目が点灯してくれる．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/b1fd16fd-9ee9-7be9-4f8e-a4aa141cddcf.png)

# 移動局の設定

u-centerを開き，
Receiver->Connectionから，comポートを選択．（ここではcom2）
Receiver->Baudrateを230400に設定．
Receiver->Protocol FilterをGeneration 9に設定．

もしコンフィグを初期化したいなら，
Receiver->Action->Revert Configを押す．

## GNSSの設定
View->Generation Configuration 9を開いて，
GNSS Configurationを以下のように入力し，
Write to layerのRAM,BBR，FlashにすべてチェックをつけてSend Configurationする．

GPS+Galileoの構成にして，10Hzの測定を可能にする．
増やすほど，Hzは減るので，注意．

参考1：https://content.u-blox.com/sites/default/files/documents/ZED-F9P-15B_DataSheet_UBX-23009090.pdf
参考2：宇野敬太, 佐田逹典, 江守央, 高精度衛星測位における Galileo の特性に関する研究. 平成30年度 日本大学理工学部 学術講演会予稿集. url: https://www.cst.nihon-u.ac.jp/research/gakujutu/62/pdf/F1-20.pdf

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/3645e8bb-d466-4b28-578f-28403c04f86c.png)

## 出力するNMEAデータの設定

View->Message Viewを開く．これ以降の作業は，このMessage Viewで操作する．

以下の項目を右クリックして，Enable Messageする．
それ以外はDisable Messageする．
GxTXTは消せないので，無視．
- NMEA->GxGGA（時刻や位置とGPS関連の情報をまとめたセンテンス）

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/86a03b49-aa09-a1a8-3d31-cc83522afabd.png)

## NAV5の設定

UBX->CFG->NAV5の中の設定を変更する．
- Dynamic Model: 0 - Portable

書き換えたら，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

## NMEAプロトコルの設定

UBX->CFG->NMEAの中の設定を変更する．
- Mode Flags：High precision mode
- Numbering used for SVs not〜：1- Extended(3 digit)

書き換えたら，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

## 出力Rateの変更

UBX->CFG->RATEの中の設定を変更する．
- Measurement Period: 100 ms

書き換えたら，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/63e358b3-eebd-0078-81f3-d15c91eb9907.png)

## TMODE3の変更

移動局は必要ないので，
- Mode: 0-Disable

書き換えたら，下のSendボタンを押してから，メニューバーのReceiver->Action->Save Configを押すこと．

# RTKを実行する

書き込めたら，いよいよ基準局接続開始！

## 1. RTKLIBをインストール

```sh
cd ~
git clone -b rtklib_2.4.3 https://github.com/tomojitakasu/RTKLIB.git
cd ~/RTKLIB/lib/iers/gcc/
make
cd ~/RTKLIB/app/consapp
make
sudo make install
```

## 2. 基準局から補正データ配信

/dev/ttyACM0が基準局，ローカルIPを127.0.0.1とすると，
```sh
str2str -in serial://ttyACM0:230400 -out tcpsvr://127.0.0.1:2101
```
このローカルIPを192.168.1.*とかにしても，もちろんOKだよ．

## 3. 移動局で補正データを受信

/dev/ttyACM1が移動局とすると，
```sh
str2str -in tcpcli://127.0.0.1:2101 -out serial://ttyACM1:230400
```

基準局と移動局がつながると，こんな感じになる．
![Screenshot from 2024-07-23 12-12-13.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/6a81a98c-031b-227f-de4c-305e433840f0.png)

# ROS2でGNSSのデータを取得

## 1. ROS2用のノードをインストール

```sh
sudo apt install ros-humble-ublox-gps
```

## 2. 適当なパッケージを作って，launchファイルを作成する

以下のようにlaunchファイル作成する．
もちろんpythonで書いてもいいけど，xmlだけ公開しておきます．

```launch/run.xml
<?xml version='1.0' ?>

<launch>
    <!-- Launch -->
    <node name="gps" namespace="gps" pkg="ublox_gps" exec="ublox_gps_node" output="screen">
        <param name="debug" value="0" />
        <param name="device" value="/dev/ttyACM1" />
        <param name="frame_id" value="gps" />
        <param name="config_on_startup" value="false" />
        <param name="uart1.baudrate" value="230400" />
        <param name="tmode3" value="0" />
        <param name="inf.all" value="false" />
        <param name="publish.all" value="false" />
        <param name="publish.aid.all" value="false" />
        <param name="publish.rxm.all" value="false" />
        <param name="publish.mon.all" value="false" />
        <param name="publish.nav.all" value="false" />
        <param name="publish.esf.all" value="false" />
    </node>
</launch>
```

## 3. 実行する

str2strを別ターミナルで立ち上げた状態で，
```sh
ros2 launch gps_launch run.xml
```

実行すると以下のようになります．
statusが2であれば，RTKでの推定結果になってます．
![Screenshot-1.png.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1876613/54cb949c-abb3-a778-06d7-e218a6fdd3f6.jpeg)