17
11

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

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

Last updated at Posted at 2022-12-19

概要

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

動作確認環境

OS : ubuntu22.04LTS
ROS : ROS2 Humble

インストール

sudo apt install ros-humble-cartographer 

foxyの場合

sudo apt install ros-foxy-cartographer 

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

sudo apt install ros-humble-cartographer-rviz

動作確認

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

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

Turtlebot3 Gazebo環境のインストール

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

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

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パッケージのインストール

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から入れてもよい

sudo apt install ros-humble-turtlebot3-simulations

Navigation2のインストール

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

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

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

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

turtlebot3のGazeboを起動

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

cartographerノード実行

source ~/ros2_ws/install/setup.bash
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True

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

ros2 run teleop_twist_keyboard teleop_twist_keyboard

マップの保存

ros2 run nav2_map_server map_saver_cli -f ~/map

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

pbstreamファイルの保存

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

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

pure localization (自己位置推定)

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

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

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ファイルを作成。

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を起動

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

cartographerノード実行

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

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

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

rviz2画面 (90度回転させてます)

rviz_screenshot_2022_12_19-22_14_07.png

Gazebo画面

Screenshot from 2022-12-19 22-13-09.png

おわりに

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

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

次の記事

参考

17
11
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
17
11

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?