はじめに
ROSのGPSを使った自己位置認識では、公開されているrobot_localizationパッケージを使用することが出来ます。
Github：https://github.com/cra-ros-pkg/robot_localization
公式ドキュメント：http://docs.ros.org/en/melodic/api/robot_localization/html/index.html
Presentation：https://vimeo.com/142624091
robot_localization概要
robot_localizationは、主に次のパッケージで構成されています。
- navsat_transform_node：GPS（緯度経度）をマップ座標系に変換
- ekf_localization_node：odomやIMU等の複数データを拡張カルマンフィルタで統合
- ukf_localization_node：odomやIMU等の複数データをunscented カルマンフィルタで統合
今回は、navsat_transform_nodeとekf_localization_nodeを使用して、GPSによる自己位置認識を行います。
robot_localizationのインストール
apt installでもインストール出来ますが、今回はgit cloneでcatkin_wsにソースコードをcloneしてビルドします。
$ cd catkin_ws/src
$ git clone https://github.com/cra-ros-pkg/robot_localization.git
$ catkin build
ビルドしたらエラーが出たので、下記の2つをインストールします。
$ sudo apt-get install ros-melodic-geographic-msgs
$ sudo apt-get install libgeographic-dev
無事にビルド出来ました。
ノードの概要
navsat_transform_nodeは、GPSによる緯度経度情報（sensor_msgs/NavSatFix）を、ロボットのmap座標系に変換します。
Subscribeするtopicは、次の3つです。
①”/gps/fix” (sensor_msgs/NavSatFix)、②”/odom” (nav_msgs/Odometry)、
③”/imu” (sensor_msgs/Imu) *IMUデータは無くても可
map座標系に変換する仕組みは、公式のドキュメントが参考になります。map座標系への変換手順は、まず、GPSの緯度経度情報をUTM座標系に変換します。ロボットが時刻0の初期位置にいる場合、map、odom、base_linkの3つのframeは一致しています。そのため、初期位置におけるIMUの磁気偏角（コンパス）を利用し、UTM座標系に対するロボットの向きを求めることで、ロボットが移動した後の初期位置に対する座標を計算することが出来ます。
今回は、Gazeboプラグインを使用してIMUのトピックをPublishしました。
yamlファイルの設定
IMUの磁気偏角を使用しない場合は、下記のパラメータを設定します。
use_odometry_yawをtrueにすることで、IMUデータを参照しないようになります。datumパラメータに、初期位置の緯度経度とロボットの方向を入力します。
use_odometry_yaw: true
wait_for_datum: true
datum: [49.90000, 8.899999, 0.0] #[55.944904, -3.186693, 0.0]yamlファイル
# Frequency of the main run loop
frequency: 30 # 30
# Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially
# important if you have use_odometry_yaw set to true. Defaults to 0.;
delay: 0.0
# PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame.
# Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using
# it.
# If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it,
# see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory.
magnetic_declination_radians: 1.57 # 0
# Your IMU's yaw, once the magentic_declination_radians value is added to it, should report 0 when facing east. If it
# doesn't, enter the offset here. Defaults to 0.
yaw_offset: 0.0
# If this is true, the altitude is set to 0 in the output odometry message. Defaults to false.
zero_altitude: true # false
# If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false.
broadcast_utm_transform: false # false
# If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform.
# Note that broadcast_utm_transform still has to be enabled. Defaults to false.
broadcast_utm_transform_as_parent_frame: false
# If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as
# /gps/filtered. Defaults to false.
publish_filtered_gps: false
# If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the
# /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this!
# The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw
# if your yaw data is based purely on integrated velocities. Defaults to false.
use_odometry_yaw: true #false
# If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists,
# navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message.
wait_for_datum: false #true
# Instead of using the first GPS location and IMU-based heading for the local-frame origin, users can specify the
# origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees,
# and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east.
datum: [49.90000, 8.899999, 0.0] #[55.944904, -3.186693, 0.0]Publishされるtopic
navsat_transform_nodeを実行すると、”/odometry/gps” (nav_msgs/Odometry)トピックがPublishされます。（設定でUTM座標系のtfをBroadcastすることも可能です。）
このトピックには、map座標系における、ロボットの初期位置（mapｎ原点）に対するGPS座標が格納されています。
ekf_localization_node
ノードの概要
このノードでは、複数のodometryやIMU等のセンサ情報を、拡張カルマンフィルタにより統合することが出来ます。
今回は、navsat_transform_nodeでPublishされた”/odometry/gps”、 Gazeboから出る”/odom”、Gazeboプラグインによる”imu”の3つを入れました。
yamlファイルの設定
yamlファイルは次のように設定しました。各センサ情報において、位置や方向・速度・加速度など、どの値を使用するかを設定することが出来ます。まだ設定しきれていませんが、適宜修正しようと思います。
frequency: 5 #30
silent_tf_failure: false
sensor_timeout: 0.1
two_d_mode: true #false
transform_time_offset: 0.5 #0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false
permit_corrected_publication: false
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_link # Defaults to "base_link" if unspecified
world_frame: map # Defaults to the value of odom_frame if unspecified
odom0: odom
odom0_config: [ true, true, false,
false, false, true,
true, true, false,
false, false, true,
false, false, false]
odom0_queue_size: 2
odom0_nodelay: false
odom0_differential: false
odom0_relative: false
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1
odom1: odometry/gps
odom1_config: [ true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_differential: false
odom1_relative: true
odom1_queue_size: 2
odom1_pose_rejection_threshold: 2
odom1_twist_rejection_threshold: 0.2
odom1_nodelay: false
imu0: imu
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8 #
imu0_linear_acceleration_rejection_threshold: 0.8 #
imu0_remove_gravitational_acceleration: true
use_control: true
stamped_control: false
control_timeout: 0.2
control_config: [true, false, false, false, false, true]
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]Publishされるtopic
ekf_localization_nodeからは、”odometry/filtered”がPublishされます。また、yamlのworld_frameをodomに設定すると、odom – base_linkのTFが、mapに設定するとmap – odomのTFがBroadcastされます。
デバッグ
次のエラーによりなかなか実行が上手く行きませんでしたが、こちらのQ&Aを参考にして、transform_time_offsetパラメータを変更することで、無事実行することができました。
