概要

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ボタンを押して、地図上の好きな点を軽くドラッグします。すると緑色の矢印が表示されるはずです。

Screenshot from 2023-12-03 20-40-00.png (245.1 kB)

　この矢印の始点がゴールの位置、向きがロボットの目標向きになります。オドメトリの赤い矢印をこの緑の矢印に重ねることを目指すのだと解釈しても良いです。
さて、マウスから指を離すとnav2はゴールまでの経路を自動で計算し、次のように紫色の線を描きます。ロボットはできるだけこの線に沿ってゴールまで向かおうとします。

Screenshot from 2023-12-03 20-33-56.png (255.9 kB)

　ただ、注意深く見ていると、シミュレーション上であってもこの経路を完璧にロボットがトレースすることはできないと分かります。それはそれで問題な気もしますがーシミュレーター上で実現できないということはそもそも実行不可能な経路を引いている可能性が高い、その場合でもnav2は誤差を自動で検知して経路をアップデートしてくれます。nav2からは速度司令であるcmd_velーこれはcommand velocityの略でしょうか？、が出力されます。
　ロボットが無事にゴールにたどり着けた場合ーそうnav2が判定した場合、左下にあるnav2パネルのfeedbackがreachedになります。

Screenshot from 2023-12-03 20-34-12.png (356.9 kB)

そうなっていれば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の構成が間違っている可能性があります。

個別の対策

地図の作成に失敗する

次のような状態になって、途中までうまく行っていたのに急にロボットがワープして、地図作成に失敗することがあります。

Screenshot from 2023-12-03 20-02-03.png (283.9 kB)

以下のようなエラーメッセージが出てないか確認して下さい。

[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