はじめに
ROS2 のNavigation2ではamclとmap_serverを使って自己位置推定し自律移動を行う。
map_serverは/mapトピックをpublishし、amclは/mapトピックとLiDARの/scan, オドメトリ /odomの情報から自己位置推定して /map --> /odomまでの/tfをpublishする。
amcl , map_serverをそれぞれ cartographer_node ,cartographer_occupancy_grid_nodeに置き換えることで、
amclの代わりにcartographerをpure localizationで動かして自己位置推定しながらNavigationすることができる。
この記事は以下の記事の続きです。
Nav2の動作確認
まずNav2のデフォルトの状態で動作確認
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
turtlebot3 パッケージにあるnavigation2.launchを実行してNav2を立ち上げる
Gazeboで実行する場合、必ず use_sim_time=trueにすること。
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=true
2D Pose Estimateをクリックして初期位置を設定、Nav2 Goalでmap上の目標位置をドラッグすると目標位置に向かってTurtlebot3が移動する。
この辺は参考記事がたくさんあるので詳しくは省略（参考の資料を参照）
cartographerとNav2を起動するlaunchファイルの作成
navigation2.launch.py の中でNav2にあるbringup_launch.pyが呼び出され、bringup_launchの中でamcl とmap_serverを実行するlocalization_launch.pyを実行している。
localization_launchの代わりに前回の記事で作成したcartographer_localization.launch.pyを実行するようにbringup_launch.pyを書き換えたlaunchファイルを新規に作成する。
turtlebot3_navigation2/launch以下に新規にlaunchファイルを作成
以下launchファイルの中身の例
bringup_launch_cartographer.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (DeclareLaunchArgument, GroupAction,
IncludeLaunchDescription, SetEnvironmentVariable)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.actions import PushRosNamespace
from nav2_common.launch import RewrittenYaml
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav2_bringup')
launch_dir = os.path.join(bringup_dir, 'launch')
cartographer_launch_dir = os.path.join(get_package_share_directory('turtlebot3_cartographer'), 'launch')
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
slam = LaunchConfiguration('slam')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
autostart = LaunchConfiguration('autostart')
use_composition = LaunchConfiguration('use_composition')
use_respawn = LaunchConfiguration('use_respawn')
log_level = LaunchConfiguration('log_level')
remappings = [('/tf', 'tf'),
('/tf_static', 'tf_static')]
configured_params = RewrittenYaml(
source_file=params_file,
root_key=namespace,
param_rewrites={},
convert_types=True)
stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
declare_namespace_cmd = DeclareLaunchArgument(
'namespace',
default_value='',
description='Top-level namespace')
declare_use_namespace_cmd = DeclareLaunchArgument(
'use_namespace',
default_value='false',
description='Whether to apply a namespace to the navigation stack')
declare_map_yaml_cmd = DeclareLaunchArgument(
'map',
default_value='',
description='Full path to map yaml file to load')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true')
declare_params_file_cmd = DeclareLaunchArgument(
'params_file',
default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
description='Full path to the ROS2 parameters file to use for all launched nodes')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart', default_value='true',
description='Automatically startup the nav2 stack')
declare_use_composition_cmd = DeclareLaunchArgument(
'use_composition', default_value='True',
description='Whether to use composed bringup')
declare_use_respawn_cmd = DeclareLaunchArgument(
'use_respawn', default_value='False',
description='Whether to respawn if a node crashes. Applied when composition is disabled.')
declare_log_level_cmd = DeclareLaunchArgument(
'log_level', default_value='info',
description='log level')
# Specify the actions
bringup_cmd_group = GroupAction([
PushRosNamespace(
condition=IfCondition(use_namespace),
namespace=namespace),
Node(
condition=IfCondition(use_composition),
name='nav2_container',
package='rclcpp_components',
executable='component_container_isolated',
parameters=[configured_params, {'autostart': autostart}],
arguments=['--ros-args', '--log-level', log_level],
remappings=remappings,
output='screen'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(cartographer_launch_dir,
'cartographer_localization.launch.py'))),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
launch_arguments={'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'use_composition': use_composition,
'use_respawn': use_respawn,
'container_name': 'nav2_container'}.items()),
])
# Create the launch description and populate
ld = LaunchDescription()
# Set environment variables
ld.add_action(stdout_linebuf_envvar)
# Declare the launch options
ld.add_action(declare_namespace_cmd)
ld.add_action(declare_use_namespace_cmd)
ld.add_action(declare_map_yaml_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_params_file_cmd)
ld.add_action(declare_autostart_cmd)
ld.add_action(declare_use_composition_cmd)
ld.add_action(declare_use_respawn_cmd)
ld.add_action(declare_log_level_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd_group)
return ld
追加した部分
cartographer_launch_dir = os.path.join(get_package_share_directory('turtlebot3_cartographer'), 'launch')
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(cartographer_launch_dir,
'cartographer_localization.launch.py'))),
bringup_launch_cartographer.launch.pyを起動するbringup_launchを新しく作成
※map.yaml pgmは不要なのでmap_dirに関連する部分は削除した。
navigation2_cartographer.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
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
param_file_name = TURTLEBOT3_MODEL + '.yaml'
param_dir = LaunchConfiguration(
'params_file',
default=os.path.join(
get_package_share_directory('turtlebot3_navigation2'),
'param',
param_file_name))
nav2_launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_navigation2'), 'launch')
rviz_config_dir = os.path.join(
get_package_share_directory('nav2_bringup'),
'rviz',
'nav2_default_view.rviz')
return LaunchDescription([
DeclareLaunchArgument(
'params_file',
default_value=param_dir,
description='Full path to param file to load'),
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
IncludeLaunchDescription(
PythonLaunchDescriptionSource([nav2_launch_file_dir, '/bringup_launch_cartographer.launch.py']),
launch_arguments={
'use_sim_time': use_sim_time,
'params_file': param_dir}.items(),
),
Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[{'use_sim_time': use_sim_time}],
output='screen'),
])
実行
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_navigation2 navigation2_cartographer.launch.py use_sim_time:=true
rviz2画面
最初からGlobal costmapが表示される。
同様にNav2 Goalでmap上の目標位置をドラッグするとゴールに向かってTurtlebot3が移動する。
（初期位置の設定は不要）
実行時のrqt_graphとtf_tree
rqt_graph
tf_tree
/map --> /odom のTFを発行するノードがamcl の代わりにcartographer_rosになる。
おわりに
以前ROS1 でmove_baseとcartographerを組み合わせてcartographerで自己位置推定してロボットを作ったことがあったが、ROS2でも同様の構成で動かすことができた。
amcl は/odomが必須になるが、cartographerの設定を変更すればオドメトリ取れないロボットでもNav2でNavigationが可能になる。
参考