ROS2でcartographerを使う手順(1) SLAMからPure Localizationまで

# 概要

ROS2でcartographerを動かす記事はいくつかあったがLocalizationまでやっていた記事はなかったので、マップ作成から自己位置推定に使用するまでの手順をまとめる。
基本的には使い方はROS1の場合と全く同じ。
動作確認にはTurtlebot3のGazeboシミュレーションを使用した。

# 動作確認環境

OS : ubuntu22.04LTS
ROS : ROS2 Humble

# インストール

```bash
sudo apt install ros-humble-cartographer 
```

foxyの場合

```bash
sudo apt install ros-foxy-cartographer 
```

rviz2 プラグインのインストール（必要な場合）


```bash
sudo apt install ros-humble-cartographer-rviz
```




# 動作確認

ROS1 ではbackpack_2dデモ用のrosbagファイルがダウンロードできて、これを実行することで動作確認できていたがROS2のrosbag2版は存在しない。

https://google-cartographer-ros.readthedocs.io/en/latest/demos.html

以下の記事にbackpack 2dのrosbagファイル.bagをROS2のrosbagに変換してデモ実行する内容を書いたので、興味のある方は試してみてください。

https://qiita.com/porizou1/items/a8004ed1f238ea0242d9

# Turtlebot3 Gazebo環境のインストール

## Turtlebot3 ROS2 パッケージのインストール

https://github.com/ROBOTIS-GIT/turtlebot3/tree/humble-devel

あとでファイルを編集するのでaptからではなくワークスペースにダウンロードしてcolcon buildする。
foxyの場合はhumbleをfoxyに変える。（foxyでは動作未確認）

```bash
mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src/
git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
cd ~/ros2_ws/
source /opt/ros/humble/setup.bash
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
colcon build --symlink-install
```

## Turtlebot3 Gazeboパッケージのインストール

```bash
cd ~/ros2_ws/src/
git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
cd ~/ros2_ws && colcon build --symlink-install
```

こちらは変更しないのでaptから入れてもよい

```bash
sudo apt install ros-humble-turtlebot3-simulations
```

## Navigation2のインストール

この記事内ではmapの保存のためにmap_serverのみ使用する


```bash
sudo apt install ros-humble-navigation2
sudo apt install ros-humble-nav2-bringup
```

# マップ作成とマップの保存

マップ作成からmap_saverでマップ保存までは以下のTurtlebot3 emanual Foxyのページの通り。ここではコマンドのみ記載。

https://emanual.robotis.com/docs/en/platform/turtlebot3/slam_simulation/


turtlebot3のGazeboを起動

```bash
source ~/ros2_ws/install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
```

cartographerノード実行
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True
```

teleop_twist_keyboardなどで適当にロボットを動かして地図を作成する

```bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard
```


マップの保存

```bash
ros2 run nav2_map_server map_saver_cli -f ~/map
```

実行したディレクトリにmap.pgmとmap.yamlがあればOK

### pbstreamファイルの保存

`write_state`のServiceをCallしてpbstreamファイルを保存
filename に保存したいパスとファイル名を書く。
後述するNav2の自己位置推定にamclを用いる場合はこの手順は不要。

```bash
ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "{filename: '/home/porizou/map.pbstream'}"
```

# pure localization　（自己位置推定）


cartographer は事前に作成したpbstreamファイルを読み込んで純粋な自己位置推定をすることができる。
(Simultaneous Localization and MappingのLocalizationのみ実行しているイメージ)

詳細は公式ドキュメントを参照。

https://google-cartographer-ros.readthedocs.io/en/latest/tuning.html


## launchファイルとluaファイルの作成


localization用にros2_ws/src/turtlebot3/turtlebot3_cartographer/configに新しく設定ファイルを作成


```turtlebot3_lds_2d_localization.lua
include "turtlebot3_lds_2d.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options
```

上の設定ファイルとpbstreamファイルを読み込むためcartographer.launch.pyを元にして新しくlaunchファイルを作成。

```python:cartographer_localization.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_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import ThisLaunchFileDir


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    turtlebot3_cartographer_prefix = get_package_share_directory('turtlebot3_cartographer')
    cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', default=os.path.join(
                                                  turtlebot3_cartographer_prefix, 'config'))
    configuration_basename = LaunchConfiguration('configuration_basename',
                                                 default='turtlebot3_lds_2d_localization.lua')

    resolution = LaunchConfiguration('resolution', default='0.05')
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')

    rviz_config_dir = os.path.join(get_package_share_directory('turtlebot3_cartographer'),
                                   'rviz', 'tb3_cartographer.rviz')
    pbstream_path = "/home/porizou/map.pbstream"
    return LaunchDescription([
        DeclareLaunchArgument(
            'cartographer_config_dir',
            default_value=cartographer_config_dir,
            description='Full path to config file to load'),
        DeclareLaunchArgument(
            'configuration_basename',
            default_value=configuration_basename,
            description='Name of lua file for cartographer'),
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        Node(
            package='cartographer_ros',
            executable='cartographer_node',
            name='cartographer_node',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-configuration_directory', cartographer_config_dir,
                       '-configuration_basename', configuration_basename,
                       '-load_state_filename', pbstream_path]),
        DeclareLaunchArgument(
            'resolution',
            default_value=resolution,
            description='Resolution of a grid cell in the published occupancy grid'),

        DeclareLaunchArgument(
            'publish_period_sec',
            default_value=publish_period_sec,
            description='OccupancyGrid publishing period'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/occupancy_grid.launch.py']),
            launch_arguments={'use_sim_time': use_sim_time, 'resolution': resolution,
                              'publish_period_sec': publish_period_sec}.items(),
        ),

        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
    ])

```

## 実行

turtlebot3のGazeboを起動

```bash
source ~/ros2_ws/install/setup.bash
export TURTLEBOT3_MODEL=burger
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
```

cartographerノード実行
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch turtlebot3_cartographer cartographer_localization.launch.py use_sim_time:=True
```

rviz2を見るとpbstreamファイルが読み込まれ、先程作成したmapが表示されていることが分かる。

![rviz_screenshot_2022_12_19-22_10_35.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/c7c4d9a6-cc3e-140d-9a2b-dd42b92906b5.png)

次に自己位置推定の動作確認

適当にTurtlebot3を動かしてGazeboのTurtlebot3の位置とrviz2のTF (base_link)の位置を比較して大体同じ位置にあればOK

rviz2画面　（90度回転させてます）

![rviz_screenshot_2022_12_19-22_14_07.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/d3d4f7f8-6571-78ec-2714-4ca9087dd479.png)

Gazebo画面

![Screenshot from 2022-12-19 22-13-09.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/569a4245-cb56-8f33-294f-5c4cf92bd95b.png)





# おわりに

この記事ではROS2環境においてcartographerの動作をTurtlebot3 Gazeboシミュレーションを使って動作確認しました。
本当は2D LiDARで実機を使用してやりたかったのですが、Localizationが思った通りに動きませんでした。
実物のLiDARとロボットを持っているかたはturtlebot3_gazebo起動部分を実機に置き換えて試してみてください。

次の記事ではcartographerをamclの代わりに自己位置推定ノードとして使用してNavigation2を動かしてみます。

次の記事

https://qiita.com/porizou1/items/70cd05be433ad02b7423

# 参考

https://ros2-industrial-workshop.readthedocs.io/en/latest/_source/navigation/ROS2-Cartographer.html

https://zenn.dev/tasada038/articles/c4fff08deae9dd


https://www.abudorilab.com/entry/2021/08/29/093209


https://qiita.com/nnn112358/items/f5495f6b9abaf5ceb117