ROS2 Jazzy 入門 (6) - SLAM
「SDF」で作ったシンプルな差動二輪ロボットで「SLAM」を実行して「地図生成」と「自己位置推定」し、「rviz2」で視覚化します。
・Ubuntu 24.04
・ROS2 Jazzy
・Gazebo Harmonic
前回
1. SLAM Toolbox
「SLAM Toolbox」は、「SLAM」(Simultaneous Localization and Mapping) の実装をサポートするライブラリです。「SLAM」は、ロボットやドローンなどの移動体が未知の環境下で「自己位置推定」しながら、同時に「地図生成」する技術です。
「SLAM Toolbox」には、次の2つのモードがあります。
・オンラインモード
ロボットが走行しながらリアルタイムに地図生成。このモードではライフロングマッピングが可能で、ループクロージングや地図修正が自動的に行われる。
・オフラインモード
事前に記録したセンサーデータを用いてオフラインで地図生成や修正を行う。
今回は「オンライモード」を使います。次の情報を元に、生成した地図を /map に、推定した自己位置を /tf (map→odom) と /pose に公開します。
・/lidar : LiDARの情報
・/tf : odomとbase_linkの関係
・/tf_static : base_linkとlidarの関係
・/odom : odomの情報
2. Gazebo の起動
(1) ターミナルを開いて「Gazebo」を起動。
「ROS2 Jazzy 入門 (3) - LiDAR」の「SDF」を使います。
$ gz sim simple_robot_lidar.sdf(2) 別のターミナルを開いて「/cmd_vel」「/lidar」「/tf」「/odom」をROS2にブリッジ。
$ ros2 run ros_gz_bridge parameter_bridge /cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist
$ ros2 run ros_gz_bridge parameter_bridge /lidar@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan
$ ros2 run ros_gz_bridge parameter_bridge /model/simple_robot/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V --ros-args -r /model/simple_robot/tf:=/tf
$ ros2 run ros_gz_bridge parameter_bridge /model/simple_robot/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry --ros-args -r /model/simple_robot/odometry:=/odom(3) 別のターミナルを開いて「/tf_static」の公開。
ロボット本体とLiDARの静的な相対関係 (Transform) を公開します。
$ ros2 run tf2_ros static_transform_publisher 0.2 0 0.15 0 0 0 simple_robot/base_link simple_robot/base_link/lidar
(4) 別のターミナルを開いて「teleop_twist_keyboard」を起動。
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard(5) 「Gazebo」左下の▶をクリック。
物理シミュレーションが開始します。
3. SLAM Toolbox の起動
(1) ターミナルを開いて「slam_toolbox」をインストール。
$ sudo apt update
$ sudo apt install ros-jazzy-slam-toolbox(2) 設定ファイルを作成。
・slam_toolbox_params.yaml
slam_toolbox:
ros__parameters:
use_sim_time: true
odom_frame: simple_robot/odom
map_frame: map
base_frame: simple_robot/base_link
map_file_name: ""
resolution: 0.05
mode: "mapping"
scan_topic: "/lidar"・use_sim_time
ROS2のノードにシミュレーション環境から提供される時間を使用させるための設定です。シミュレーション環境は独自の時間管理 (/clock) を使用します。シミュレーション時間は、リアルタイムとは異なり、速度を変更したり、一時停止したりすることが可能です。シミュレーションとROSノード間で時間の不整合の発生を防ぎたい場合に使用します。
(3) 「slam_toolbox」の起動。
今回は、「online_async_launch.py」を使います。
$ ros2 launch slam_toolbox online_async_launch.py slam_params_file:=/home/ubuntu/slam_toolbox_params.yaml4. 地図生成 の動作確認
(1) 「/map」がトピックに追加されていることを確認。
$ ros2 topic list/clicked_point
/clock
/cmd_vel
/goal_pose
/initialpose
/lidar
/map
/map_metadata
/odom
/parameter_events
/pose
/rosout
/slam_toolbox/feedback
/slam_toolbox/graph_visualization
/slam_toolbox/scan_visualization
/slam_toolbox/transition_event
/slam_toolbox/update
/tf
/tf_static
(2) 「/map」のデータ型を確認。
$ ros2 topic info /mapType: nav_msgs/msg/OccupancyGrid
Publisher count: 1
Subscription count: 2(3)「/map」のリアルタイムデータを確認。
$ ros2 topic echo /mapheader:
stamp:
sec: 0
nanosec: 0
frame_id: map
info:
map_load_time:
sec: 0
nanosec: 0
resolution: 0.05000000074505806
width: 99
height: 99
origin:
position:
x: -2.4180000062843146
y: -2.4390841088348387
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
data:
- -1
:
- -1
- '...'5. 自己位置推定 の動作確認
(1) 「/pose」がトピックに追加されていることを確認。
$ ros2 topic list/clicked_point
/clock
/cmd_vel
/goal_pose
/initialpose
/lidar
/map
/map_metadata
/odom
/parameter_events
/pose
/rosout
/slam_toolbox/feedback
/slam_toolbox/graph_visualization
/slam_toolbox/scan_visualization
/slam_toolbox/transition_event
/slam_toolbox/update
/tf
/tf_static
(2) 「/pose」のデータ型を確認。
$ ros2 topic info /poseType: geometry_msgs/msg/PoseWithCovarianceStamped
Publisher count: 1
Subscription count: 1(3)「/pose」のリアルタイムデータを確認。
ロボットを移動させるとpublishされます。
$ ros2 topic echo /poseheader:
stamp:
sec: 31
nanosec: 500000000
frame_id: map
pose:
pose:
position:
x: 0.7731289097391361
y: 0.13792772944264195
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.3744235271580157
w: 0.9272577971149937
covariance:
- 0.0018089936638450103
:
- 0.00012544721796745585
6. rviz2 での視覚化
(1) rviz2 の起動。
$ rviz2(2) 左側の設定項目を以下のように設定。
・Fixed Frame : map
「ros2 topic echo /map」の「frame_id」を指定します。
・Odometry - Keep : 1
見やすさのため指定しています。
(2) 左下の「Add」をクリックし、「By topic → /map → Map」を選択し、「OK」をクリック。
(3) 左下の「Add」をクリックし、「By display type → rviz_default_plugins → 「PoseWithCovariance」を選択し、「OK」をクリック。
(4) 「PoseWithCovariance」を次のように設定。
・Topic : /pose
・Color : 28; 113; 216
見やすさのため指定しています。
(5) 「teleop_twist_keyboard」でロボットを移動。
LiDARの情報に応じて地図生成と自己位置推定が実行されることを確認します。
赤い矢印は /odom で公開されているOdometryによる自己位置推定、青い矢印は /pose で公開されているSLAMによる自己位置推定です。
7. 地図の保存
(1) ロボットを移動させて地図生成。
(2) 地図のPGM (Portable Gray Map) とYAMLの地図の保存。
$ ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap "name:
data: 'my_map'"以下の2つのファイルが保存されます。
・my_map.pgm
グレースケールで各ピクセルが占有状態を表現します。
・my_map.yaml
地図のメタデータです。
image: my_map.pgm
mode: trinary
resolution: 0.05
origin: [-4.97, -4.97, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25(3) 地図のPOSEGRAPHとDATAの保存。
$ ros2 service call /slam_toolbox/serialize_map slam_toolbox/srv/SerializePoseGraph "{filename: 'my_map'}"・my_map.posegraph
ロボットが移動した各位置 (ノード) と、それらの位置間の制約 (エッジ) を表すグラフ構造です。
・my_map.data
各ノードで取得されたレーザースキャンなどのセンサーデータやロボットの自己位置推定に使用されたオドメトリデータです。
8. launchファイル
(1) launchファイルの作成。
・run_simple_robot_map.py
from launch import LaunchDescription
from launch.actions import ExecuteProcess
def generate_launch_description():
return LaunchDescription([
# Gazeboの起動
ExecuteProcess(
cmd=['gz', 'sim', 'simple_robot_lidar.sdf'],
output='log',
log_cmd=True
),
# rviz2の起動
ExecuteProcess(
cmd=['rviz2'],
output='log',
log_cmd=True
),
# ブリッジの起動
ExecuteProcess(
cmd=[
'ros2', 'run', 'ros_gz_bridge', 'parameter_bridge',
'/cmd_vel@geometry_msgs/msg/Twist@gz.msgs.Twist'
],
output='log',
log_cmd=True
),
ExecuteProcess(
cmd=[
'ros2', 'run', 'ros_gz_bridge', 'parameter_bridge',
'/lidar@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan'
],
output='log',
log_cmd=True
),
ExecuteProcess(
cmd=[
'ros2', 'run', 'ros_gz_bridge', 'parameter_bridge',
'/model/simple_robot/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
'--ros-args', '-r', '/model/simple_robot/tf:=/tf'
],
output='log',
log_cmd=True
),
ExecuteProcess(
cmd=[
'ros2', 'run', 'ros_gz_bridge', 'parameter_bridge',
'/model/simple_robot/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry',
'--ros-args', '-r', '/model/simple_robot/odometry:=/odom'
],
output='log',
log_cmd=True
),
# teleop_twist_keyboardの起動
ExecuteProcess(
cmd=['ros2', 'run', 'teleop_twist_keyboard', 'teleop_twist_keyboard'],
output='screen',
prefix='xterm -e'
),
# static_transform_publisherの起動
ExecuteProcess(
cmd=[
'ros2', 'run', 'tf2_ros', 'static_transform_publisher',
'0.2', '0', '0.15', '0', '0', '0',
'simple_robot/base_link', 'simple_robot/base_link/lidar'
],
output='log',
log_cmd=True
),
# slam_toolboxの起動
ExecuteProcess(
cmd=[
'ros2', 'launch', 'slam_toolbox', 'online_async_launch.py',
'slam_params_file:=/home/ubuntu/slam_toolbox_params.yaml'
],
output='log',
log_cmd=True
),
])(2) launchファイルの実行。
ros2 launch ~/run_simple_robot_map.py【おまけ】 OdometryとSLAMの自己位置推定の比較
・Odometryによる自己位置推定
・長所
・高速・リアルタイム性
・簡単なアルゴリズムで位置推定が可能
・計算リソースが少ない
・短所
・累積誤差 (ドリフト ) 問題
・時間が経つにつれて誤差が蓄積し、精度が悪化
・地図生成や環境情報を利用した誤差補正ができない
・センサー
・ホイールオドメトリ（車輪エンコーダ）
・IMU（加速度センサー、ジャイロセンサー）
・Visual Odometry（カメラによる推定）
・SLAMによる自己位置推定
・長所
・環境地図を利用するため、ドリフトを大幅に抑制可能
・未知の環境でも自己位置を推定しつつ地図を作成可能
・カメラやLiDARを使うことで、より詳細な環境情報を得られる
・短所
・計算リソースが高い
・特にリアルタイム処理では、GPUや高性能なCPUが必要
・実装が複雑
・センサー
・カメラ (モノクロ、ステレオ)
・LiDAR (3Dスキャナー)
・IMU (オプションで統合)
【おまけ】 online_async_launch.py のパラメータ
「online_async_launch.py」のパラメータは、以下のコマンドで確認できます。
$ cat /opt/ros/jazzy/share/slam_toolbox/config/mapper_params_online_async.yamlslam_toolbox:
ros__parameters:
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_footprint
scan_topic: /scan
use_map_saver: true
mode: mapping #localization
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: test_steve
# map_start_pose: [0.0, 0.0, 0.0]
#map_start_at_dock: true
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.0
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true【おまけ】 tf をグラフ表記で確認
(1) 以下のコマンドで公開中の tf をグラフ表記で確認。
$ ros2 run tf2_tools view_frames(2) 生成されたPDFを確認。
