本記事では、ChoreonoidとROS2パッケージであるslam-toolboxによる地図生成を行う手順を紹介します。slam-toolboxは、Kartoアルゴリズムをベースに開発されており、オドメトリ情報を用いて地図と自己位置の更新を行います。また、slam-toolbox には Rviz2用のプラグインが用意されており、ユーザーが手動でスキャンの位置を調整することができます。slam-toolboxを用いた地図生成には、async_slam_toolbox_nodeとsync_slam_toolbox_nodeの2種類があります。リアルタイムで地図を作成する場合はasync_slam_toolbox_node、rosbag2などで保存したセンサデータから品質の高い地図を作成する場合はsync_slam_toolbox_nodeを使用します。本記事では、リアルタイムに地図を作成する方法を説明します。なお、本記事はChoreonoidとROS2ディストリビューションのインストールを行っていることを前提としています。インストールを行っていない方は、Choreonoid公式HPを参考に「ROS2のインストール」と「Choreonoid関連パッケージのビルド」を行ってください。
使用した環境
・Ubuntu 22.04 LTS
・Choreonoid 2.2
・ROS2 Humble Hawksbill
・slam-toolbox 2.6.9
slam-toolboxパッケージのインストール
以下のコマンドを実行し、slam-toolboxパッケージをインストールします。
sudo apt install -y ros-humble-slam-toolbox
TurtleBot3関連パッケージのインストール
以下のコマンドを実行しTurtleBot3関連パッケージをダウンロードします。本記事ではUbuntu 22.04を使用しているため、git cloneを行う際に、”-b”オプションを付けてブランチを指定します。
cd ~/ros2_ws/src
git clone -b humble https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git
.bashrcファイルの編集
以下のコマンドを実行し、ワークスペースセットアップスクリプトの読み込みと環境変数の設定を追加します。今回はTURTLEBOT3_MODELにwaffle_piを指定しています。
echo "source ~/ros2_ws/install/setup.bash" >> ~/.bashrc
echo "export TURTLEBOT3_MODEL=waffle_pi" >> ~/.bashrc
Choreonoidのmaterials.yamlファイルの編集
使用するTurtleBot3モデルのマテリアル設定を追加します。以下のコマンドを実行しmaterials.yamlファイルを開きます。
cd ~/ros2_ws/src/choreonoid/share/default
cp -fp materials.yaml materials_bkyyyyMMdd.yaml
※バックアップファイル作成時のyyyyMMddは、編集年月日を指定します。
gedit materials.yaml
「materials」キー(74行目)と「contact_materials」キー(最終行)に以下を追加します。
-
name: Tb3_tire
youngs_modulus: 1e10
roughness: 1.0
viscosity: 1.0
-
name: Tb3_caster
youngs_modulus: 1e10
roughness: 1.0
viscocity: 1.0
-
materials: [ Ground, Tb3_tire ]
youngs_modulus: 1e10
friction: 1.0
restitution: 0.0
friction_model: [ cone, direct ]
-
materials: [ Ground, Tb3_caster ]
youngs_modulus: 1e10
friction: 1.0
restitution: 0.0
friction_model: [ cone, direct ]
cnoid_turtlebot3_pkgsパッケージのダウンロード
cnoid_turtlebot3_pkgs.zipをダウンロードし、~/ros2_ws/src以下に移動したらZIPファイルを解凍します。
ここでは、ダウンロードディレクトリにcnoid_turtlebot3_pkgs.zipファイルが保存されている場合の手順となります。別のディレクトリに格納している場合は適宜ファイルパスを変更してください。
mv ~/ダウンロード/cnoid_turtlebot3_pkgs.zip ~/ros2_ws/src
cd ~/ros2_ws/src
unzip cnoid_turtlebot3_pkgs.zip
シンボリック・リンクの追加
cnoid_turtlebot3_pkgsパッケージのモデルファイルでturtlebot3パッケージとturtlebot3_simulationsパッケージのメッシュファイルを参照しているので、以下のコマンドを実行しシンボリック・リンクを追加します。
cd ~/ros2_ws/ src/cnoid_turtlebot3_pkgs/cnoid_turtlebot3_bringup/model/resource/
ln -s ~/ros2_ws/src/turtlebot3/turtlebot3_description turtlebot3_description
ln -s ~/ros2_ws/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_world/meshes turtlebot3_world
ChoreonoidとTurtleBot3関連パッケージのビルド
以下のコマンドを実行し、ChoreonoidとTurtleBot3関連パッケージのビルドを行います。
cd ~/ros2_ws
colcon build --symlink-install --cmake-args -DBUILD_CHOREONOID_EXECUTABLE=OFF -DCMAKE_BUILD_TYPE=Release
source install/setup.bash
TurtleBot3による2D地図生成
以下のコマンドを実行し、シミュレータChoreonoidとRviz2を起動します。
ros2 launch cnoid_turtlebot3_slam cnoid_turtlebot3_world_slam_2d.launch
2つ目のターミナルを起動し、slam-toolboxを起動します。
ros2 launch cnoid_turtlebot3_slam cnoid_turtlebot3_slam_toolbox.launch
3つ目のターミナルを起動し、遠隔操作ノードを起動します。
ros2 run teleop_twist_keyboard teleop_twist_keyboard
遠隔操作ノードを起動したターミナル上で、キーボードから”i”(直進)、","(後退)、"l"(右旋回)、"j"(左旋回)、"k"(停止)を入力することでTurtleBot3を動かし、2D地図を生成することができます。
図1. Choreonoid画面
図2. Rviz2画面
作成した地図の保存
Rviz2のSlamToolboxPluginの「Save map」ボタンの右側のテキストエリアに保存するファイルパスを入力し、「Save map」ボタンを押し保存します。
図3. SlamToolboxPlugin画面
slam-toolboxのターミナル上で、"Map saved successfully"のメッセージが表示されたら正常に保存されています。
ros2 launch cnoid_turtlebot3_slam cnoid_turtlebot3_slam_toolbox.launch
[INFO] [map_io]: Writing map occupancy data to /home/user/ros2_ws/map.pgm
[INFO] [map_io]: Writing map metadata to /home/user/ros2_ws/map.yaml
[INFO] [map_io]: Map saved
[INFO] [map_saver]: Map saved successfully
図4. SLAMで作成した地図
ChoreonoidとRviz2起動用launchファイルの解説
cnoid_turtlebot3_world_slam_2d.launchの17~25行目でChoreonoidノードを起動しています。引数にプロジェクトファイルを指定することでプロジェクトファイルが読み込まれ、--start-simulationオプションでノード起動時に自動でシミュレーションが開始されます。
<node name="choreonoid" pkg="choreonoid_ros" exec="choreonoid"
args="$(find-pkg-share cnoid_turtlebot3_bringup)/project/$(var model)_turtlebot3_world_2d.launch --start-simulation">
<param name="use_sim_time" value="$(var use_sim_time)" />
<remap from="/choreonoid/joint_states" to="/joint_states" />
<remap form="/choreonoid/LiDAR/scan" to="/scan" />
<remap from="imu" to="/imu" />
<remap from="odom" to="/odom" />
<remap from="cmd_vel" to="/cmd_vel" />
</node>
28～33行目でstatic_transform_publisherノードを起動し、LiDARの静的座標位置を出力しています。引数には、frame_id、child_frame_idを与えており、frame_id->child_frame_idの座標変換を指定しています。
<node name="lidar_broadcaster" pkg="tf2_ros" exec="static_transform_publisher"
args="--frame-id base_link --child-frame-id LiDAR">
<param name="robot_description" value="$(var robot_description)" />
<param name="use_sim_time" value="$(var use_sim_time)" />
</node>
33～38行目でrobot_state_publisherノードを起動し、ロボットの関節位置を出力しています。
<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
<param name="robot_description" value="$(var robot_description)" />
<param name="publish_frequency" value="50.0" />
<param name="use_sim_time" value="$(var use_sim_time)" />
<remap from="joint_states" to="/joint_states" />
</node>
42～43行目でRviz2を起動しています。
<node name="rviz2" pkg="rviz2" exec="rviz2"
args="-d $(find-pkg-share cnoid_turtlebot3_slam)/rviz/cnoid_turtlebot3_slam_toolbox.rviz" />
slam-toolbox用launchファイルの解説
cnoid_turtlebot3_slam_toolbox.launchの10～13行目でasync_slam_toolbox_nodeを起動しています。
<node name="slam_toolbox" pkg="slam_toolbox" exec="async_slam_toolbox_node" output="screen">
<param from="$(var slam_params)" />
<param name="use_sim_time" value="$(var use_sim_time)" />
</node>
いかがでしたでしょうか。
ChoreonoidとROS2でSLAMを行いたい方の参考になれば幸いです。