ROS2でcartographerを使う手順(2) amclの代わりにcartographerで自己位置推定してNavigation2でNavigationする

# はじめに

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することができる。




この記事は以下の記事の続きです。

https://qiita.com/porizou1/items/4ddeb5a0aa6c62a4c294


# Nav2の動作確認

まずNav2のデフォルトの状態で動作確認


```bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
```

turtlebot3 パッケージにあるnavigation2.launchを実行してNav2を立ち上げる
Gazeboで実行する場合、必ず use_sim_time=trueにすること。

```bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=true
```

`2D Pose Estimate`をクリックして初期位置を設定、`Nav2 Goal`でmap上の目標位置をドラッグすると目標位置に向かってTurtlebot3が移動する。

この辺は参考記事がたくさんあるので詳しくは省略（参考の資料を参照）

![rviz_screenshot_2023_01_29-16_53_36.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/fe85bad6-2928-f202-c69a-795591e54685.png)


# 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ファイルの中身の例

```py: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
```


追加した部分

```py
    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に関連する部分は削除した。

```py: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'),
    ])

```

# 実行


```bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
```

```bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_navigation2 navigation2_cartographer.launch.py use_sim_time:=true
```

rviz2画面
最初からGlobal costmapが表示される。

同様にNav2 Goalでmap上の目標位置をドラッグするとゴールに向かってTurtlebot3が移動する。
（初期位置の設定は不要）

![rviz_screenshot_2023_02_15-21_14_51.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/23f7ef1d-9dbc-0da9-2cce-67bc3b6f0fce.png)

実行時のrqt_graphとtf_tree

### rqt_graph

![rosgraph.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/3aa39c65-2bac-797c-9c5a-0613c305a61a.png)


### tf_tree

/map --> /odom のTFを発行するノードがamcl の代わりにcartographer_rosになる。

![frames.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/bc2f0646-2008-9e78-269a-dc8f35c8319c.png)


# おわりに

以前ROS1 でmove_baseとcartographerを組み合わせてcartographerで自己位置推定してロボットを作ったことがあったが、ROS2でも同様の構成で動かすことができた。
amcl は/odomが必須になるが、cartographerの設定を変更すればオドメトリ取れないロボットでもNav2でNavigationが可能になる。

# 参考

https://brain.cc.kogakuin.ac.jp/~kanamaru/lecture/ROS2/index5.html