概要
SLAMとnavigation
用語の意味を少し明確にしておきましょうー別に専門書のような定義をするわけではないので安心して下さい。ロボットを自律走行させるために必要な要素はなんでしょうか？先ず、ロボットは自身の位置を知る必要があります。ただ、自律走行の場合は誰かが教えてくれることはないので、ロボット自身が推測する必要があります。これを自己位置推定といいます。しかし、「自分の位置」と言ったときには、その「位置」の基準となるものが必要です。今回はロボットがいるフィールドの「地図」を作成し、これを基準とします。この地図を作成する作業をマッピングと言ったりもします。そして、これらマッピングと自己位置推定を同時に行うアルゴリズムがSLAMです。この２つが同時に行えるメリットは、第三者の助け無しにロボットが自力で地図を作成できるということです。
さて、ではロボットの自己位置と地図、そして目標地点が与えられたとします。ロボットは地図に基づいてゴールまでの道のりを考える必要があります。闇雲に走り回ると、障害物にぶつかってしまう可能性もあるので、最初に経路をある程度決めておくという発想は妥当でしょう。これを経路生成といいます。経路を作成したらその経路を辿ってゴールまで行けば良いのですが、経路が与えられたところでモーターをどのくらい回せばいいのかは非自明です。なので、これもロボットが計算すべき事柄です。これは経路追従といいます。この２つのプロセスを合わせてnavigationと呼んでいるのです。
この記事の内容
この記事ではslam toolboxとnavigation2を用いてgazebo上でSLAMとnavigationを行う方法を説明します。この記事の手順自体は簡単ですが、sdfファイルの記述やbridgeの設定といった前回までの記事で実装した内容がうまくできていないと挙動が不安定になったり、そもそもよく動かなかったりするので注意して下さい。また、この記事で扱った内容を実装したコードは私のgithubに公開しています。今回の内容に相当する部分はリンク先のレポジトリのgz_ws/src/nav_devに収められています。
また、記事の内容に誤りや不備等ございましたら、ご指摘いただけると幸いです。
動作環境・バージョン
動作環境
Ubuntu22.04 ：デュアルブートした実機PC
VScode ：コーディングとターミナルの操作は基本的にVScodeで行った
バージョン
ROS2 : humble
gazebo : ignition fortless ：ROS2公式がhumbleとの連携相手として推奨するバージョン
前提条件
以下のような読者を想定して記事を執筆しました
・上記の動作環境またはそれと同等の機能を持つ環境を有すること
・ 基本的なubuntuの操作が理解できること
・ROS2についての基本的な知識があること
・gazeboの基本的な知識があること
・rvizの基本的な知識があること
コンテンツ
SLAMによる地図作成
slam toolboxの起動
では、実際にSLAMによる地図作成をおこなって行きましょう。今回はslam toolboxを使って実装していきたいと思います。
リンク先のgithubレポジトリに飛んで下さい。やや下の方にインストールのやり方が書いてあるので、それに従って下さい。
次に、launchファイルにslam toolboxの設定を記述していきます。
#slam_toolboxの起動オプション設定
slam_params_file = LaunchConfiguration('slam_params_file')
declare_slam_params_file_cmd = DeclareLaunchArgument(
'slam_params_file',
default_value=os.path.join(get_package_share_directory("nav_dev"),
'params', 'slam_param.yaml'),
description='Full path to the ROS2 parameters file to use for the slam_toolbox node')
#slam_toolboxの起動設定
start_async_slam_toolbox_node = Node(
parameters=[
slam_params_file,
{'use_sim_time': use_sim_time}
],
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen')
今回新たに、パッケージ内にparamsディレクトリを作成して下さい。そしてその中にslam_param.yamlというファイルを作成して以下の内容を貼り付けて下さい。これはslam toolboxの設定パラメータです。
slam toolboxの設定
slam_param.yaml
slam_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.
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
次に、rvizの設定ファイルを以下の通りにして下さい。前回からの変更点はslam toolboxのためのパネルを追加したところです。自分で追加する場合は、画面上部のpanels→Add New Panel→Slam Toolbox Pluginを選択して下さい。
rvizの設定
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Odometry1/Topic1
- /Odometry1/Shape1
- /Map1/Topic1
Splitter Ratio: 0.5
Tree Height: 139
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: LaserScan
- Class: slam_toolbox::SlamToolboxPlugin
Name: SlamToolboxPlugin
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz_default_plugins/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 100
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic:
Depth: 5
Durability Policy: Transient Local
Filter size: 10
History Policy: Keep All
Reliability Policy: System Default
Value: /odom
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0
Min Color: 0; 0; 0
Min Intensity: 0
Name: LaserScan
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /scan
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
base_footprint:
Value: true
chassis:
Value: true
left_wheel:
Value: true
lidar:
Value: true
map:
Value: true
odom:
Value: true
right_wheel:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
map:
odom:
base_footprint:
chassis:
left_wheel:
{}
lidar:
{}
right_wheel:
{}
Update Interval: 0
Value: true
- Alpha: 0.699999988079071
Class: rviz_default_plugins/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map_updates
Use Timestamp: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 11.221535682678223
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.36851271986961365
Y: 0.915789008140564
Z: -1.1618577241897583
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.815398097038269
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 1.2303966283798218
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001c9000002aefc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000117000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000220053006c0061006d0054006f006f006c0062006f00780050006c007500670069006e010000015b000001910000019100ffffff000000010000010f000002aefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002ae000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000027100fffffffb0000000800540069006d00650100000000000004500000000000000000000001cc000002ae00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
SlamToolboxPlugin:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 421
Y: 73
以上のことが終わったらlaunchファイルにslam toolboxを起動するように追記を行って、paramファイルがinstallされるようにCMakeファイルを更新します。この時点でlaunchファイルは以下のようになっていることを想定しています。
launchファイルの確認
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
import xacro
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
world_name = LaunchConfiguration('world_name', default='nav_slam_world')
pkg_share_dir = get_package_share_directory('nav_dev')
model_path = os.path.join(pkg_share_dir, "models")
#ignition gazeboがモデルにアクセスできるように設定
ign_resource_path = SetEnvironmentVariable(
name='IGN_GAZEBO_RESOURCE_PATH',value=[
os.path.join("/opt/ros/humble", "share"),
":" +
model_path])
#ロボットをスポーンさせる設定
ignition_spawn_entity = Node(
package='ros_ign_gazebo',
executable='create',
output='screen',
arguments=['-entity', 'LidarRobo',
'-name', 'LidarRobo',
#ロボットのsdfファイルを指定
'-file', PathJoinSubstitution([
pkg_share_dir,
"models", "LidarRobo", "model.sdf"]),
#ロボットの位置を指定
'-allow_renaming', 'true',
'-x', '0.4',
'-y', '0.4',
'-z', '0.075'],
)
#フィールドをスポーンさせる設定
ignition_spawn_world = Node(
package='ros_ign_gazebo',
executable='create',
output='screen',
#フィールドのsdfファイルを指定
arguments=['-file', PathJoinSubstitution([
pkg_share_dir,
"models", "field", "model.sdf"]),
'-allow_renaming', 'false'],
)
#ワールドのsdfファイルを設定(worldタグのあるsdfファイル)
world = os.path.join(pkg_share_dir, "models", "worlds", "nav_slam.sdf")
#ignition gazeboの起動設定
ign_gz = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_ign_gazebo'),
'launch', 'ign_gazebo.launch.py')]),
launch_arguments=[('ign_args', [' -r -v 3 ' +
world
])])
#ros_ign_bridgeの起動設定
bridge = Node(
package='ros_ign_bridge',
executable='parameter_bridge',
parameters=[{
#brigdeの設定ファイルを指定
'config_file': os.path.join(pkg_share_dir, 'config', 'nav_slam.yaml'),
'qos_overrides./tf_static.publisher.durability': 'transient_local',
'qos_overrides./odom.publisher.durability': 'transient_local',
},{'use_sim_time': use_sim_time}],
remappings=[
("/odom/tf", "tf"),
],
output='screen'
)
#mapトピックの設定
map_static_tf = Node(package='tf2_ros',
executable='static_transform_publisher',
name='static_transform_publisher',
output='log',
arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'map', 'odom'])
#ロボットのsdfファイルのパスを取得
sdf = os.path.join(
get_package_share_directory('nav_dev'),
'models', 'LidarRobo', 'model.sdf')
#xacroでsdfファイルをurdfに変換
doc = xacro.parse(open(sdf))
xacro.process_doc(doc)
#robot_state_publsherの起動設定
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='both',
parameters=[{'use_sim_time': use_sim_time,
'robot_description': doc.toxml()}]) # type: ignore
teleop_node = Node(
package='nav_dev',
executable='teleop_node',
output='screen',
#別ターミナルで起動する設定
prefix="xterm -e"
)
#rviz2の設定フィルのパスを取得
rviz_config_dir = os.path.join(
pkg_share_dir,
'config',
'nav_slam.rviz')
#rviz2の起動設定
rviz2 = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[{'use_sim_time': use_sim_time}],
output='screen')
#slam_toolboxの起動オプション設定
slam_params_file = LaunchConfiguration('slam_params_file')
declare_slam_params_file_cmd = DeclareLaunchArgument(
'slam_params_file',
default_value=os.path.join(get_package_share_directory("nav_dev"),
'params', 'slam_param.yaml'),
description='Full path to the ROS2 parameters file to use for the slam_toolbox node')
#slam_toolboxの起動設定
start_async_slam_toolbox_node = Node(
parameters=[
slam_params_file,
{'use_sim_time': use_sim_time}
],
package='slam_toolbox',
executable='async_slam_toolbox_node',
name='slam_toolbox',
output='screen')
return LaunchDescription([
ign_resource_path,
ignition_spawn_entity,
ignition_spawn_world,
ign_gz,
DeclareLaunchArgument(
'use_sim_time',
default_value=use_sim_time,
description='If true, use simulated clock'),
DeclareLaunchArgument(
'world_name',
default_value=world_name,
description='World name'),
bridge,
map_static_tf,
robot_state_publisher,
teleop_node,
rviz2,
#
#slam toolboxが起動するように追記
#
declare_slam_params_file_cmd,
start_async_slam_toolbox_node,
])
以上のことを完了してlaunchファイルを起動すると以下のような画面になるはずです。
地図の作成と保存
前々回の記事で作成したteleop nodeを用いてgazbeo上のロボットを動かすと、そこから送られてくる情報を基に地図が作成されていきます。rviz上の地図では黒く塗られている場所が障害物として認識された部分、やた白っぽく塗られている領域が何もないと判断された部分、グレーの領域が未知の部分です。teleopによってロボットを操作して未知の部分をマッピングしていきましょう。
ある程度地図が作成できたら、地図を保存しましょう。rvizのslam toolboxのパネルから、簡単に地図を保存できます。save mapボタンの隣にあるテキストボックスに適当な名前を入力してからボタンを押します。すると、slam toolboxが実行されているディレクトリに地図が.pgmファイルとして生成されますーこれはubuntuのファイルアプリで開いて見ることができます。一緒に.yamlファイルも生成されますが、これは地図の設定ファイルです。さらに、serialize mapも試してみましょう。こちらは、地図をslam toolboxが扱う形式そのままで出力します。
localization モードでの起動
(工事中)
Nav2によるNavigation
nav2の起動
それでは、先程作成した地図を基にしてnav2でnavigationをおこなって行きましょう。nav2の公式ドキュメントに従ってnavigation2をインストールして下さい。
次に、launchファイルにnav2の設定を記述していきます。新たにパッケージ内にmapディレクトリを作成し、そこに先程作成した地図を収納して下さい。以下のlaunchファイルの、nav2へ渡す地図のパスを設定している部分を適宜書き換えて下さい。
#mapトピックの設定
map_static_tf = Node(package='tf2_ros',
executable='static_transform_publisher',
name='static_transform_publisher',
output='log',
arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'map', 'odom'])
#nav2の地図のパスを取得
map_dir = LaunchConfiguration(
'map',
default=os.path.join(
get_package_share_directory('nav_dev'),
'maps','test_map2',
'test_map2.yaml'))
#nav2のパラメータのパスを取得
param_file_name = 'nav_nav2.yaml'
param_dir = LaunchConfiguration(
'params_file',
default=os.path.join(
get_package_share_directory('nav_dev'),
'params',
param_file_name))
#nav2のランチファイルのパスを取得
nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
その次に、paramディレクトリの中にslam_param.yamlというファイルを作成して以下の内容を貼り付けて下さい。これはnav2の設定パラメータです。
nav2の設定
nav_nav2.yaml
amcl:
ros__parameters:
use_sim_time: True
alpha1: 0.2
alpha2: 0.2
alpha3: 0.2
alpha4: 0.2
alpha5: 0.2
base_frame_id: "base_footprint"
beam_skip_distance: 0.5
beam_skip_error_threshold: 0.9
beam_skip_threshold: 0.3
do_beamskip: false
global_frame_id: "map"
lambda_short: 0.1
laser_likelihood_max_dist: 2.0
laser_max_range: 100.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 60
max_particles: 2000
min_particles: 500
odom_frame_id: "odom"
pf_err: 0.05
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 1
robot_model_type: "nav2_amcl::DifferentialMotionModel"
save_pose_rate: 0.5
sigma_hit: 0.2
tf_broadcast: true
transform_tolerance: 1.0
update_min_a: 0.2
update_min_d: 0.25
z_hit: 0.5
z_max: 0.05
z_rand: 0.5
z_short: 0.05
scan_topic: scan
bt_navigator:
ros__parameters:
use_sim_time: True
global_frame: map
robot_base_frame: base_link
odom_topic: /odom
bt_loop_duration: 10
default_server_timeout: 20
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_smooth_path_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_assisted_teleop_action_bt_node
- nav2_back_up_action_bt_node
- nav2_drive_on_heading_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_globally_updated_goal_condition_bt_node
- nav2_is_path_valid_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_truncate_path_local_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_path_expiring_timer_condition
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_goal_updated_controller_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
- nav2_controller_cancel_bt_node
- nav2_path_longer_on_approach_bt_node
- nav2_wait_cancel_bt_node
- nav2_spin_cancel_bt_node
- nav2_back_up_cancel_bt_node
- nav2_assisted_teleop_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
- nav2_is_battery_charging_condition_bt_node
bt_navigator_navigate_through_poses_rclcpp_node:
ros__parameters:
use_sim_time: True
bt_navigator_navigate_to_pose_rclcpp_node:
ros__parameters:
use_sim_time: True
controller_server:
ros__parameters:
use_sim_time: True
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: "progress_checker"
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
controller_plugins: ["FollowPath"]
# Progress checker parameters
progress_checker:
plugin: "nav2_controller::SimpleProgressChecker"
required_movement_radius: 0.5
movement_time_allowance: 10.0
# Goal checker parameters
#precise_goal_checker:
# plugin: "nav2_controller::SimpleGoalChecker"
# xy_goal_tolerance: 0.25
# yaw_goal_tolerance: 0.25
# stateful: True
general_goal_checker:
stateful: True
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.25
# DWB parameters
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
debug_trajectory_details: True
min_vel_x: 0.0
min_vel_y: 0.0
max_vel_x: 0.26
max_vel_y: 0.0
max_vel_theta: 1.0
min_speed_xy: 0.0
max_speed_xy: 0.26
min_speed_theta: 0.0
# Add high threshold velocity for turtlebot 3 issue.
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -3.2
vx_samples: 20
vy_samples: 5
vtheta_samples: 20
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.2
xy_goal_tolerance: 0.25
trans_stopped_velocity: 0.25
short_circuit_trajectory_evaluation: True
stateful: True
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 32.0
RotateToGoal.slowing_factor: 5.0
RotateToGoal.lookahead_time: -1.0
local_costmap:
local_costmap:
ros__parameters:
update_frequency: 5.0
publish_frequency: 2.0
global_frame: odom
robot_base_frame: base_link
use_sim_time: True
rolling_window: true
width: 3
height: 3
resolution: 0.05
robot_radius: 0.22
plugins: ["voxel_layer", "inflation_layer"]
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
voxel_layer:
plugin: "nav2_costmap_2d::VoxelLayer"
enabled: True
publish_voxel_map: True
origin_z: 0.0
z_resolution: 0.05
z_voxels: 16
max_obstacle_height: 2.0
mark_threshold: 0
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
always_send_full_costmap: True
global_costmap:
global_costmap:
ros__parameters:
update_frequency: 1.0
publish_frequency: 1.0
global_frame: map
robot_base_frame: base_link
use_sim_time: True
robot_radius: 0.22
resolution: 0.05
track_unknown_space: true
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
obstacle_layer:
plugin: "nav2_costmap_2d::ObstacleLayer"
enabled: True
observation_sources: scan
scan:
topic: /scan
max_obstacle_height: 2.0
clearing: True
marking: True
data_type: "LaserScan"
raytrace_max_range: 3.0
raytrace_min_range: 0.0
obstacle_max_range: 2.5
obstacle_min_range: 0.0
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
inflation_layer:
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: True
map_server:
ros__parameters:
use_sim_time: True
# Overridden in launch by the "map" launch configuration or provided default value.
# To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
yaml_filename: ""
map_saver:
ros__parameters:
use_sim_time: True
save_map_timeout: 5.0
free_thresh_default: 0.25
occupied_thresh_default: 0.65
map_subscribe_transient_local: True
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: True
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: false
allow_unknown: true
smoother_server:
ros__parameters:
use_sim_time: True
smoother_plugins: ["simple_smoother"]
simple_smoother:
plugin: "nav2_smoother::SimpleSmoother"
tolerance: 1.0e-10
max_its: 1000
do_refinement: True
behavior_server:
ros__parameters:
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
spin:
plugin: "nav2_behaviors/Spin"
backup:
plugin: "nav2_behaviors/BackUp"
drive_on_heading:
plugin: "nav2_behaviors/DriveOnHeading"
wait:
plugin: "nav2_behaviors/Wait"
assisted_teleop:
plugin: "nav2_behaviors/AssistedTeleop"
global_frame: odom
robot_base_frame: base_link
transform_tolerance: 0.1
use_sim_time: true
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
robot_state_publisher:
ros__parameters:
use_sim_time: True
waypoint_follower:
ros__parameters:
use_sim_time: True
loop_rate: 20
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 200
velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 20.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.26, 0.0, 1.0]
min_velocity: [-0.26, 0.0, -1.0]
max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2]
odom_topic: "odom"
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
次に、rvizの設定ファイルを以下の通りにして下さい。前回からの変更点はslam toolboxのパネルをnav2のためのパネルで置き換えたところです。自分で追加する場合は、画面上部のpanels→Add New Panel→navigation2 Pluginを選択して下さい。
rvizの設定
nav_nav2.rviz
Panels:
- Class: rviz_common/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /RobotModel1/Status1
- /TF1/Frames1
- /TF1/Tree1
- /Odometry1
- /Odometry1/Topic1
Splitter Ratio: 0.5833333134651184
Tree Height: 448
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: nav2_rviz_plugins/Navigation 2
Name: Navigation 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
Mass Properties:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: false
Visual Enabled: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
base_footprint:
Value: true
chassis:
Value: true
left_wheel:
Value: true
lidar:
Value: true
map:
Value: true
odom:
Value: true
right_wheel:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
map:
odom:
base_footprint:
chassis:
left_wheel:
{}
lidar:
{}
right_wheel:
{}
Update Interval: 0
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0
Min Color: 0; 0; 0
Min Intensity: 0
Name: LaserScan
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /scan
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: ""
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: Bumper Hit
Position Transformer: ""
Selectable: true
Size (Pixels): 3
Size (m): 0.07999999821186066
Style: Spheres
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /mobile_base/sensors/bumper_pointcloud
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Class: rviz_default_plugins/Map
Color Scheme: map
Draw Behind: true
Enabled: true
Name: Map
Topic:
Depth: 1
Durability Policy: Transient Local
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map_updates
Use Timestamp: false
Value: true
- Alpha: 1
Class: nav2_rviz_plugins/ParticleCloud
Color: 0; 180; 0
Enabled: true
Max Arrow Length: 0.30000001192092896
Min Arrow Length: 0.019999999552965164
Name: Amcl Particle Swarm
Shape: Arrow (Flat)
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /particle_cloud
Value: true
- Class: rviz_common/Group
Displays:
- Alpha: 0.30000001192092896
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Global Costmap
Topic:
Depth: 1
Durability Policy: Transient Local
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/costmap
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/costmap_updates
Use Timestamp: false
Value: true
- Alpha: 0.30000001192092896
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Downsampled Costmap
Topic:
Depth: 1
Durability Policy: Transient Local
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /downsampled_costmap
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /downsampled_costmap_updates
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz_default_plugins/Path
Color: 255; 0; 0
Enabled: true
Head Diameter: 0.019999999552965164
Head Length: 0.019999999552965164
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: Arrows
Radius: 0.029999999329447746
Shaft Diameter: 0.004999999888241291
Shaft Length: 0.019999999552965164
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /plan
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 125; 125; 125
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: VoxelGrid
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.05000000074505806
Style: Boxes
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/voxel_marked_cloud
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Class: rviz_default_plugins/Polygon
Color: 25; 255; 0
Enabled: false
Name: Polygon
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/published_footprint
Value: false
Enabled: true
Name: Global Planner
- Class: rviz_common/Group
Displays:
- Alpha: 0.699999988079071
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Local Costmap
Topic:
Depth: 1
Durability Policy: Transient Local
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/costmap
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/costmap_updates
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz_default_plugins/Path
Color: 0; 12; 255
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Local Plan
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_plan
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Trajectories
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /marker
Value: false
- Alpha: 1
Class: rviz_default_plugins/Polygon
Color: 25; 255; 0
Enabled: true
Name: Polygon
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/published_footprint
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: VoxelGrid
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/voxel_marked_cloud
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Name: Controller
- Class: rviz_common/Group
Displays:
- Class: rviz_default_plugins/Image
Enabled: true
Max Value: 1
Median window: 5
Min Value: 0
Name: RealsenseCamera
Normalize Range: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /intel_realsense_r200_depth/image_raw
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: RealsenseDepthImage
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /intel_realsense_r200_depth/points
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: false
Name: Realsense
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /waypoints
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz_default_plugins/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 100
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odom
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
- Class: nav2_rviz_plugins/GoalTool
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Angle: -1.5707999467849731
Class: rviz_default_plugins/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: 194.43797302246094
Target Frame: <Fixed Frame>
Value: TopDownOrtho (rviz_default_plugins)
X: 0
Y: 0
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 932
Hide Left Dock: false
Hide Right Dock: true
Navigation 2:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000348fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000001fe000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e002000320100000242000001440000014400fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000002800ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000034800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
RealsenseCamera:
collapsed: false
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1545
X: 375
Y: 229
以上のことが終わったらlaunchファイルにnav2を起動するように変更を加えてください。この時点でlaunchファイルは以下のようになっていることを想定しています。
launchファイルの確認
nav_nav2.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
import xacro
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
world_name = LaunchConfiguration('world_name', default='nav_slam_world')
pkg_share_dir = get_package_share_directory('nav_dev')
model_path = os.path.join(pkg_share_dir, "models")
#ignition gazeboがモデルにアクセスできるように設定
ign_resource_path = SetEnvironmentVariable(
name='IGN_GAZEBO_RESOURCE_PATH',value=[
os.path.join("/opt/ros/humble", "share"),
":" +
model_path])
#ロボットをスポーンさせる設定
ignition_spawn_entity = Node(
package='ros_ign_gazebo',
executable='create',
output='screen',
arguments=['-entity', 'LidarRobo4',
'-name', 'LidarRobo4',
#ロボットのsdfファイルを指定
'-file', PathJoinSubstitution([
pkg_share_dir,
"models", "LidarRobo4", "model.sdf"]),#LidarRobo4を使用すること！
#ロボットの位置を指定
'-allow_renaming', 'true',
'-x', '0.4',
'-y', '0.4',
'-z', '0.075',
],
)
#フィールドをスポーンさせる設定
ignition_spawn_world = Node(
package='ros_ign_gazebo',
executable='create',
output='screen',
#フィールドのsdfファイルを指定
arguments=['-file', PathJoinSubstitution([
pkg_share_dir,
"models", "field", "model.sdf"]),
'-allow_renaming', 'false'],
)
#ワールドのsdfファイルを設定(worldタグのあるsdfファイル)
world = os.path.join(pkg_share_dir, "models", "worlds", "nav_slam.sdf")
#ignition gazeboの起動設定
ign_gz = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_ign_gazebo'),
'launch', 'ign_gazebo.launch.py')]),
launch_arguments=[('ign_args', [' -r -v 3 ' +
world
])])
#ros_ign_bridgeの起動設定
bridge = Node(
package='ros_ign_bridge',
executable='parameter_bridge',
parameters=[{
#brigdeの設定ファイルを指定
'config_file': os.path.join(pkg_share_dir, 'config', 'nav_slam.yaml'),
'qos_overrides./tf_static.publisher.durability': 'transient_local',
'qos_overrides./odom.publisher.durability': 'transient_local',
},{'use_sim_time': use_sim_time}],
remappings=[
("/odom/tf", "tf"),
],
output='screen'
)
#ロボットのsdfファイルのパスを取得
sdf = os.path.join(
get_package_share_directory('nav_dev'),
'models', 'LidarRobo4', 'model.sdf')
#xacroでsdfファイルをurdfに変換
doc = xacro.parse(open(sdf))
xacro.process_doc(doc)
#robot_state_publsherの起動設定
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='both',
parameters=[{'use_sim_time': use_sim_time,
'robot_description': doc.toxml()}]) # type: ignore
#mapトピックの設定
map_static_tf = Node(package='tf2_ros',
executable='static_transform_publisher',
name='static_transform_publisher',
output='log',
arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'map', 'odom'])
#nav2の地図のパスを取得
map_dir = LaunchConfiguration(
'map',
default=os.path.join(
get_package_share_directory('nav_dev'),
'maps','test_map2',
'test_map2.yaml'))
#nav2のパラメータのパスを取得
param_file_name = 'nav_nav2.yaml'
param_dir = LaunchConfiguration(
'params_file',
default=os.path.join(
get_package_share_directory('nav_dev'),
'params',
param_file_name))
#nav2のランチファイルのパスを取得
nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
#rviz2の設定フィルのパスを取得
rviz_config_dir = os.path.join(
pkg_share_dir,
'config',
'nav_nav2.rviz')
#nav2の起動設定
nav2 = IncludeLaunchDescription(
PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch.py']),
launch_arguments={
'map': map_dir,
'use_sim_time': use_sim_time,
'params_file': param_dir}.items(),
)
#rviz2の起動設定
rviz2 = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[{'use_sim_time': use_sim_time}],
output='screen')
return LaunchDescription([
ign_resource_path,
ignition_spawn_entity,
ignition_spawn_world,
ign_gz,
DeclareLaunchArgument(
'use_sim_time',
default_value=use_sim_time,
description='If true, use simulated clock'),
DeclareLaunchArgument(
'world_name',
default_value=world_name,
description='World name'),
bridge,
map_static_tf,
robot_state_publisher,
DeclareLaunchArgument(
'map',
default_value=map_dir,
description='Full path to map file to load'),
DeclareLaunchArgument(
'params_file',
default_value=param_dir,
description='Full path to param file to load'),
nav2,
rviz2,
])
以上のことを完了してlaunchファイルを起動すると以下のような画面になるはずです。
コストマップ
先程作成された地図になにやら色が付いています。これはコストマップと呼ばれる代物で、外側の紫でハイライトされた部分が通過可能領域、水色でハイライトされた部分が通過非推奨領域、内側の紫の部分が壁と識別されています。また、地図のロボットから遠い部分も同じ配色で薄くハイライトされています。この色の濃度差は、ロボットのLiDARデータを基に作成したコストマップか読み込んだ地図を基に作成したものかの違いです。
navigationを行う
画面上部のnav2 goalボタンを押して、地図上の好きな点を軽くドラッグします。すると緑色の矢印が表示されるはずです。
この矢印の始点がゴールの位置、向きがロボットの目標向きになります。オドメトリの赤い矢印をこの緑の矢印に重ねることを目指すのだと解釈しても良いです。
さて、マウスから指を離すとnav2はゴールまでの経路を自動で計算し、次のように紫色の線を描きます。ロボットはできるだけこの線に沿ってゴールまで向かおうとします。
ただ、注意深く見ていると、シミュレーション上であってもこの経路を完璧にロボットがトレースすることはできないと分かります。それはそれで問題な気もしますがーシミュレーター上で実現できないということはそもそも実行不可能な経路を引いている可能性が高い、その場合でもnav2は誤差を自動で検知して経路をアップデートしてくれます。nav2からは速度司令であるcmd_velーこれはcommand velocityの略でしょうか？、が出力されます。
ロボットが無事にゴールにたどり着けた場合ーそうnav2が判定した場合、左下にあるnav2パネルのfeedbackがreachedになります。
そうなっていればnavigation成功です。お疲れ様でした！
トラブルシューティング
一般論
・とりあえずsudo apt updateする
・gazeboのゾンビ化を疑う
gazeboは乱暴な閉じ方をしたときーそして丁寧に行った場合でもそれなりの頻度で、正常に終了せずプロセスがゾンビ化します
先ず、ps aux|grep ignで検索をかけます。正しく終了できていれば２つのプロセスが表示されます。
３つ以上ならプロセスのゾンビ化が発生しています。kill -9 <プロセス番号>で最初と最後のプロセス以外を強制終了します。
プロセスがゾンビ化しているときの代表的な挙動
・モデルやワールドの記述を変更したのに反映されていない
・何故かロボットが２つ表示される
・シミュレーションの開始ボタンのオンオフが高速で切り替わる
- tfが正常にpublishされているか確認する
tfの確認
記事の内容通りに実装した場合、tfのTreeはーrvizのパネルにあるtfで確認できる、以下のようになっているはずです。
map
odom
base_footprint
base_link
base_scan
wheel_left_link
wheel_right_link
以上のようになっていない場合、sdfファイルにおけるlinkの構成が間違っている可能性があります。
個別の対策
地図の作成に失敗する
次のような状態になって、途中までうまく行っていたのに急にロボットがワープして、地図作成に失敗することがあります。
以下のようなエラーメッセージが出てないか確認して下さい。
[async_slam_toolbox_node-8] W1203 20:01:52.542362 42840 preprocessor.cc:62] Specified options.num_threads: 50 exceeds
maximum available from the threading model Ceres was compiled with: 16. Bounding to maximum number available.
[rviz2-7] [INFO] [1701601313.576266618] [rviz2]: Trying to create a map of size 81 x 139 using 1 swatches
もし出ていた場合、作成しようとした地図が大きすぎることが原因です。作成する地図を小さく留めることで問題を回避できます。しかし、根本的な解決法はまだ見つけられていません(2023/12/3)。
odomが表示されない、qosポリシーがどうとかいうエラー
qosポリシーに関しては、この資料が参考になります。
結論としては、問題となっているトピックのqosポリシーを変更すれば治ることが多いです。例えば/odomトピックについては
'qos_overrides./odom.publisher.durability': 'transient_local',
これを、ros_ign\bridgeのparametersに追加します。
nav2の挙動がおかしい
sdfファイルの構成に不備があるとnav2の挙動がおかしくなることがあります。
例えば、
・ゴールを指定しても経路が生成されない、生成に失敗したというエラーが出る
・経路は生成されるが明後日の方向にロボットが動き出す
ただ、sdfファイルのどの部分が原因となっているのかは解明できていません。もし、自作したsdfファイルを使っている場合は、githubにあるモデルを用いて正常に動作するかを確認してみて下さい。
終わりに
この記事ではslam toolboxを使ってフィールドの地図を作成し、nav2でnavigationを行いました。理論上は実際のロボットでもこのプログラムを用いてnavigationを行うことができます。勿論そこには現実世界ならではの面倒な問題が待ち受けているに違いないのですが。
この記事で「ignition gazebo+Rviz2の開発入門」は一旦終了です。この記事を呼んで、実際に実装してみたという人がいたら、喜ばしい限りです。もし、何か記事の内容についてや実装に詰まった際に質問があれば、記事へのコメントやslackのDMで気軽に聞いて下さい。できる限りの対応をしようと思いますー少なくとも私が機械研にいる間は。
これらの記事の全体像が知りたければ、ignition gazebo+Rviz2の開発環境構築も覗いてみてください。ここにはgazeboとrvizに関する私の書いた記事がまとめられています。
また、この記事で扱った内容を実装したコードは私のgithubに公開しています。今回の内容に相当する部分はリンク先のレポジトリのgz_ws/src/nav_devに収められています。
参考文献
ROS 2 Documenttion
SteveMacenski/slam toolbox
Nav2 Documentation
QoS設定について Moriken's Journal