【ROS2】Septentrio社GNSS向けROSドライバー septentrio_gnss_driver を使ってみた

Septentrio社GNSS向けの公式ROSドライバー septentrio_gnss_driver を使ってみたのでその記録です。

まず、ROSドライバーが公式から提供されているのは、とてもうれしいポイントです。
履歴を見る限りではしっかりメンテナンスされており、安心。
apt-get install でインストールできるのでインストールは簡単。
最初の設定ファイル作成とLaunchファイルだけがネック。。。
サンプル設定ファイルの種類がもう少し充実していれば、簡単に始めれそうなのが惜しいところ。

動作確認環境
インストール方法
septentrio_gnss_driverを使うときのポイント
起動方法
トラブルシューティング
パラメータ
リンク

動作確認環境

OS: Ubuntu 22.04
ROS: Humble
GNSS: mosaic-go CLAS評価キット

インストール方法

sudo apt-get install ros-${ROS_DISTRO}-septentrio-gnss-driver

septentrio_gnss_driverを使うときのポイント

設定ファイルのconfigure_rxがtrueだと、ドライバ起動時に受信機の設定を書き換える。自分で同様の設定を行うのは無理なので、素直にtrueにしておく方がいい。
設定ファイルで受信機が対応していない機能をtrueにすると、ドライバが正常に動作しない。問題があった場合は、エラーレベルのログで確認できる。
サンプルとして提供される設定ファイルは、INS受信機やマルチアンテナが前提となっている。該当しない環境でない限り、正常に動作しない。
設定ファイルのactivate_debug_logをtrueにしておくと、septentrio_gnss_driverと受信機のやり取りを見ることができる。問題発生時に原因を切り分けやすい。
ドライバが出力するログは、受信機の設定や読み取りに関する内容だけなので見やすい。
受信機がWebUIをサポートしているので、WebUIにアクセスすれば状況を把握しやすい

起動方法

設定ファイルの用意

標準の設定ファイルはそのままでは mosaic-go CLAS評価キットの場合使えない。
なので、適当なパスに設定ファイルを作成。

cd /home/taro/septentrio_config
vi rover.yaml

作った rover.yaml の中身。login情報は書き換える必要がある。

device: tcp://192.168.3.1:28784

serial:
  baudrate: 921600

configure_rx: true

login:
  user: "登録済みのユーザー名を設定"
  password: "ユーザー名に対応するパスワードを設定"

frame_id: gnss

polling_period:
  pvt: 100
  rest: 500

use_gnss_time: false
ntp_server: true
ptp_server_clock: false
latency_compensation: true

publish:
  publish_only_valid: true
  navsatfix: true
  diagnostics: false

activate_debug_log: true

Launchファイルの用意

標準のLaunchファイルはTFなども設定して使いにくかったので、参考にしつつ、適当なフォルダに作成。
設定ファイルのパスを埋め込んでいるので、他の設定ファイルを使う場合は注意。

vi rover_custom.launch.py

rover_custom.launch.pyの中身

import os
from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
import yaml

os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message}'
# Verbose log:
#os.environ['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = '{time}: [{name}] [{severity}]\t{message} ({function_name}() at {file_name}:{line_number})'

def generate_launch_description():
    
    # Read original yaml file and pass parameters directly
    with open('/home/taro/septentrio_config/rover.yaml', 'r') as f:
        config_data = yaml.safe_load(f)

    composable_node = ComposableNode(
        name='septentrio_gnss_driver',
        package='septentrio_gnss_driver', 
        plugin='rosaic_node::ROSaicNode',
        parameters=[config_data])

    container = ComposableNodeContainer(
        name='septentrio_gnss_driver_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container_isolated',
        emulate_tty=True,
        composable_node_descriptions=[composable_node],
        output='screen'
    )

    return LaunchDescription([container])

起動

ros2 launch rover_custom.launch.py

動作確認

$ ros2 topic hz /navsatfix
average rate: 9.994
        min: 0.099s max: 0.101s std dev: 0.00034s window: 12
average rate: 9.999
        min: 0.099s max: 0.101s std dev: 0.00030s window: 23

トラブルシューティング

ログに「$R? setSBFOutput: Argument 'Messages' is invalid!」が出力され、正常に動作しない

原因：設定ファイルで受信機がサポートしていない機能を指定した場合に起こる。
解決方法：設定ファイルの内容を適切に設定すれば解消される。
ログの例：

[septentrio_gnss_driver] [DEBUG] handleCd: IP10>
[septentrio_gnss_driver] [DEBUG] AsyncManager sent the following 128 bytes to the Rx: sso, Stream3, IP10, +PVTGeodetic +PosCovGeodetic +VelCovGeodetic +AttEuler +AttCovEuler +MeasEpoch +ChannelStatus +DOP, msec100
[septentrio_gnss_driver] [ERROR] Invalid command just sent to the Rx! The Rx's response contains 51 bytes and reads:
$R? setSBFOutput: Argument 'Messages' is invalid!

ログに「$R? sga, none : Invalid command!」が出力される

原因：ドライバから受信機に「マルチアンテナではない」という内容を送っているが、受信機が未対応のコマンド。
解決方法：ドライバ側はマルチアンテナではないことを把握できている。気にしない
ログの例：

[septentrio_gnss_driver] [ERROR] Invalid command just sent to the Rx! The Rx's response contains 34 bytes and reads:
$R? sga, none : Invalid command!

ログに「$R? sto, -0.000, -0.000 : Invalid command!」が出力される

原因：ドライバから受信機に「アンテナのオフセット設定」を送っているが、受信機が未対応のコマンド。
解決方法：アンテナのオフセットを扱わない限り、気にしない
ログの例：

[component_container_isolated-1] 1757159448.214935091: [septentrio_gnss_driver] [ERROR] Invalid command just sent to the Rx! The Rx's response contains 44 bytes and reads:
[component_container_isolated-1]  $R? sto, -0.000, -0.000 : Invalid command!

パラメータ

パラメータ	デフォルト値	解説
stream_device.tcp.ip_server	""	""の場合はdeviceパラメータで設定した内容が使われる。
stream_device.tcp.port	0	データストリームにTCPを使う場合のみ利用
stream_device.udp.ip_server	""	""の場合はdeviceパラメータで設定した内容が使われる。
stream_device.udp.port	0	データストリームにUDPを使う場合のみ利用
stream_device.udp.unicast_ip	""	同上
serial.hw_flow_control	"off"	仮想ではないシリアル接続時にハードウェアフロー制御を使うか。文字列型なので""で囲む必要がある。
configure_rx	true	設定ファイルに従って受信機の設定を上書きするか。true推奨。
custom_commands_file	""	カスタムコマンドを送る場合に設定する
osnma.mode	off	OSNMA機能の設定
polling_period.pvt	500	位置関連？のポーリング間隔。単位ミリ秒。
polling_period.rest	500	上記以外のポーリング間隔。単位ミリ秒。
multi_antenna	false	受信機が複数アンテナを持つかどうか
lock_utm_zone	true	UTM ゾーンを最初に決まった内容で固定するか。10km以上の移動を扱うなら気にする必要があるかも。
use_gnss_time	false	ROSメッセージヘッダーのunix epoch timeフィールドをGPS時刻から構築するか。GPS時刻を使う場合は、NTPタイムサーバーまたはPTPクロックへの同期が必須
ntp_server	false	NTPサーバーを有効化するかどうか
ptp_server_clock	false	PTPサーバークロックを有効化するかどうか。ファーム4.15.0で有効化するとハマったので注意。
latency_compensation	false	時刻情報から受信機の受信処理で発生したレイテンシを差し引くかどうか。use_gnss_time = trueの場合、時刻情報はGNSS時刻なので不要
poi_to_arp.delta_e	0	POI-ARP Offset。固定受信機用
poi_to_arp.delta_n	0	POI-ARP Offset。固定受信機用
poi_to_arp.delta_u	0	POI-ARP Offset。固定受信機用
datum	Default	座標が参照すべき測地系
receiver_type	gnss	受信機のタイプgnss or ins。mosaic-go CLASはgnss。
use_ros_axis_orientation	true	ボディ関連フレームと地理フレームに対してROS REP 103に従ったROS軸方位を使用するかどうか
multi_antenna	false	受信機が複数アンテナを接続しているか。mosaic-go CLASは複数アンテナ非対応。
att_offset.heading	0.0	アンテナオフセット
att_offset.pitch	0.0	アンテナオフセット
ant_type	"Unknown"	メインGNSSアンテナのタイプ
ant_serial_nr	Unknown	メインGNSSアンテナのシリアルNo.
ant_aux1_type	Unknown	Aux1アンテナのタイプ
ant_aux1_serial_nr	Unknown	Aux1アンテナのシリアルNo.
aux1_frame_id	aux1	マルチアンテナ用。mosaic-go CLASは未対応
vsm_frame_id	vsm	INS受信機用。mosaic-go CLASは未対応
imu_frame_id	imu	同上
poi_frame_id	base_link	同上
vehicle_frame_id	(poi_frame_idの値)	同上
local_frame_id	odom	同上
insert_local_frame	false	同上
get_spatial_config_from_tf	false	シングルアンテナのmosaic-go CLASは未対応
publish.auto_publish	false	全部のtopicを公開するかどうか
publish.publish_only_valid	false	時刻情報(TOW)が含まれる場合のみpublishする設定
publish.gpgga	false	NMEA関連。汎用ROSメッセージnmea_msgs/GPGGA.msg型
publish.gprmc	false	NMEA関連。汎用ROSメッセージnmea_msgs/GPRMC.msg型
publish.gpgsa	false	NMEA関連。汎用ROSメッセージnmea_msgs/GPGSA.msg型
publish.gpgsv	false	NMEA関連。汎用ROSメッセージnmea_msgs/GPGSV.msg型
publish.gpsfix	false	汎用ROSメッセージ[`gps_msgs/GPSFix.msg`]型。ただし、mosaic-go CLASはAttEulerに対応していないので出力できない。
publish.navsatfix	false	汎用ROSメッセージ[`sensor_msgs/NavSatFix.msg`]型。
publish.diagnostics	false	汎用ROSメッセージ[`diagnostic_msgs/DiagnosticArray.msg`]型。
publish.gpst	false	sensor_msgs/TimeReference.msg型
publish.aimplusstatus	false	AIM+のステータス
publish.galauthstatus	false	Galileoの認証(OSNMA) ステータス
publish.atteuler	false	姿勢用。mosaic-go CLASは未対応
publish.attcoveuler	false	姿勢用。mosaic-go CLASは未対応
publish.pose	false	位置、姿勢用。mosaic-go CLASは未対応
publish.twist	false	速度用。mosaic-go CLASは未対応
publish.measepoch	false	生データ確認用
publish.pvtcartesian	false	生データ確認用
publish.pvtgeodetic	false	生データ確認用
publish.basevectorcart	false	生データ確認用
publish.basevectorgeod	false	生データ確認用
publish.poscovcartesian	false	生データ確認用
publish.poscovgeodetic	false	生データ確認用
publish.velcovcartesian	false	生データ確認用
publish.velcovgeodetic	false	生データ確認用
publish.insnavcart	false	For INS Rx onlyと書かれているので、mosaic-go CLASは対象外
publish.insnavgeod	false	同上
publish.extsensormeas	false	同上
publish.imusetup	false	同上
publish.velsensorsetup	false	同上
publish.exteventinsnavcart	false	同上
publish.exteventinsnavgeod	false	同上
publish.imu	false	同上
publish.localization	false	同上
publish.tf	false	同上
publish.localization_ecef	false	同上
publish.tf_ecef	false	同上
activate_debug_log	false	septentrio_gnss_driverと受信機のやり取りをログ出力するかどうか

リンク

septentrio_gnss_driver