本記事では、ChoreonoidとROSを使ってナビゲーションを行う手順を紹介します。
今回のナビゲーションに使用したnavigationメタパッケージは、既存地図での自己位置推定、障害物情報の管理、経路計画の機能が備わっています。map_serverで既存地図の配信を行い、amcl (Adaptive Monte Carlo localization)でパーティクルフィルタによる自己位置推定をし、move_baseでナビゲーションタスクを実行します。
これらを実行するためには、「Choreonoid tips ⑦」のChoreonoidやTurtleBot3関連の設定を済ませていることが前提となります。
Choreonoid tips ⑦【gmappingを使ったSLAM】https://rtc-fukushima.jp/technical/6295/
1. 使用した環境
・Ubuntu 20.04 LTS
・Choreonoid 1.8
・ROS Noetic Ninjemys
・navigation 1.17.2
2. ROS Navigation Stack関連パッケージのインストール
以下のコマンドを実行し、navigationメタパッケージをインストールします。
sudo apt install ros-noetic-amcl ros-noetic-move-base ros-noetic-navigation
3. cnoid_turtlebot3_navigationパッケージのダウンロード
以下リンクからcnoid_turtlebot3_navigation.zipをダウンロードします。
cnoid_turtlebot3_navigation.zip
以下のコマンドを実行し、ファイルを所定の場所に配置します。
cd ~/ダウンロード
unzip cnoid_turtlebot3_navigation.zip
mv cnoid_turtlebot3_navigation ~/catkin_ws/src/cnoid_turtlebot3_pkgs/
4. waffle_pi_turtlebot3_world_2dプロジェクトファイルの修正
以下のコマンドを実行し、waffle_pi_turtlebot3_world_2d.cnoidのAISTSimulatorItemのプロパティ値を修正します。
cd ~/catkin_ws/src/cnoid_turtlebot3_pkgs/cnoid_turtlebot3_bringup/project
sed -i".org" -e 's/cullingThresh: 0.01/cullingThresh: 0.005/g' -e 's/contactCorrectionDepth: 0.0001/contactCorrectionDepth: 0.00025/g' waffle_pi_turtlebot3_world_2d.cnoid
5. cnoid_turtlebot3_navigationパッケージのビルド
以下のコマンドを実行し、cnoid_turtlebot3_bringupとcnoid_turtlebot3_navigationパッケージをビルドします。
cd ~/catkin_ws
catkin build
source devel/setup.bash
6. ナビゲーション用launchファイルの起動
以下のコマンドを実行し、ナビゲーション用のlaunchファイルを実行します。launchファイルを実行することで、ChoreonoidとRVizが起動します。
cd ~/catkin_ws
roslaunch cnoid_turtlebot3_navigation cnoid_turtlebot3_navigation.launch
launchファイル実行時に以下のエラーが発生した場合、Navigation Stack関連パッケージのいずれかがインストールされていないので、「ROS Navigation Stack関連パッケージのインストール」のコマンドを実行し、インストールしてください。
【エラー ①】
roslaunch cnoid_turtlebot3_navigation cnoid_turtlebot3_navigation.launch
ERROR: cannot launch node of type [amcl/amcl]: amcl
【エラー ②】
roslaunch cnoid_turtlebot3_navigation cnoid_turtlebot3_navigation.launch
ERROR: cannot launch node of type [move_base/move_base]: move_base
【エラー ③】
roslaunch cnoid_turtlebot3_navigation cnoid_turtlebot3_navigation.launch
[FATAL] [1661237980.906888562, 1.431000000]: Failed to create the dwa_local_planner/DWAPlannerROS planner, are you sure it is properly registered and that the containing library is built? Exception: According to the loaded plugin descriptions the class dwa_local_planner/DWAPlannerROS with base class type nav_core::BaseLocalPlanner does not exist. Declared types are base_local_planner/TrajectoryPlannerROS
図 1. Choreonoid（左）とRViz（右）の画面
7. RVizでロボットの位置姿勢の修正指示を実行
RVizのツールバーの「2D Pose Estimate」をクリックし、ロボット本来の位置からロボットの向いている方向にドラッグすると、黄緑色の矢印が表示され、RViz上の自己位置推定ノードにロボットの位置姿勢の修正を指示することができます。
図 2. ロボットの位置姿勢を修正
8. RVizで目標位置姿勢指示を実行
RVizのツールバーの「2D Navi Goal」をクリックし、ロボットを向かわせたい目標位置から目標姿勢に向かってドラッグすると、ピンク色の矢印が表示され、ナビゲーションノードに目標位置姿勢を指示することができます。
図 3. ロボットの向かう目標位置姿勢を指示
目標位置姿勢の指示を行ったら、ナビゲーションが実行されロボットが目標位置に向かって自律走行を開始します。
図 4. ナビゲーション実行時のChoreonoid（左）とRViz（右）画面
9. ナビゲーション用launchファイルの解説
cnoid_turtlebot3_navigation.launchの11～13行目は、Choreonoidのシンプルコントローラで行っているオドメトリの計算で使用しています。ROSパラメータでロボットの初期位置と姿勢を設定し、その値からオドメトリを計算しています。
<param name="initial_x" value="-2.0" />
<param name="initial_y" value="-0.5" />
<param name="initial_th" value="0.0" />
27行目は、map_serverノードを起動しナビゲーションを行う環境地図の取得を行っています。今回は”map_file”という名前の引数に対して以下のファイルパスを設定しています。
ファイルパス：~/catkin_ws/src/cnoid_turtlebot3_plgs/cnoid_turtlebot3_navigation/maps/map.yaml
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
39行目で自己位置推定用のamcl.launchを読み込み、amclノードを起動して自己位置推定を行います。
42~45行目でナビゲーション用のmove_base.launchを読み込み、ナビゲーションの実行に必要なmove_baseノードを起動しています。
※ amcl.launchとmove_base.launchで指定しているパラメータについては、以下のROS Wikiをご参照ください。
amcl (ROS Wiki) : http://wiki.ros.org/amcl
move_base (ROS Wiki) : http://wiki.ros.org/move_base
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
<!-- move_base -->
<include file="$(find cnoid_turtlebot3_navigation)/launch/move_base.launch">
<arg name="model" value="$(arg model)" />
<arg name="move_forward_only" value="$(arg move_forward_only)"/>
</include>
いかがでしたでしょうか。
前回の記事でSLAMを行ったので、今回はChoreonoidとROSを使ったナビゲーションを行ってみました。皆さまの参考になれば幸いです。