Navigation2を使ってロボットを動的な目的地に向かわせる

はじめに

　本記事ではROS2環境でロボットを目的地を動的に設定して移動させる方法についてNavigation2のDynamic Object Followingの記事をもとに紹介いたします。

Navigation2

　Navigation2とは移動ロボットを目的の場所へ移動するための移動計画とその操作命令を行うだけでなく、ビヘイビアツリーを使用した複雑なロボットビヘイビアの構築や、障害発生時の回復動作などロボットの行動に関する細かなナビゲーションを行えるシステムツールです。下図は、Nav2を使う上で必要となるデータや処理によって出力されるトピックなどを表したものです。詳しくは公式を参照ください。

実行環境

Windows PC

項目	スペック
CPU	Core i7-9750H
OS	Ubuntu 20.04LTS
ROS2	Foxy Fitzroy

Raspberry Pi 4

項目	スペック
CPU	1.5GHz クアッドコア Cortex-A72(ARMv8、64bit L1=32KB、L2=1M)
OS	Ubuntu 20.04LTS
ROS2	Foxy Fitzroy
Turtlebot3	Waffle Pi

シミュレータ環境で動かしてみる

　基本的にはNav2の公式チュートリアルに準拠して、それに加えて筆者が躓いた部分、解決した部分を加筆するかたちで解説します。

1.インストール

※ROS2のインストールはすでに済んでいることを前提としています。
まずはaptでfoxy用のnav2をインストールすることから始めます。

sudo apt install ros-foxy-navigation2 ros-foxy-nav2-bringup '~ros-foxy-turtlebot3-.*'

　次にNav2用のワークスペースを作成し、ビルドをします。

mkdir -p ~/nav2_ws/src
cd ~/nav2_ws/src
git clone https://github.com/ros-planning/navigation2.git --branch foxy-devel
cd ~/nav2_ws
rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy
colcon build --symlink-install

2.ビヘイビアツリーを作成する。

　ビヘイビアツリーとは、ツリー構造を用いて行動決定を記述したものです。ゲームにおけるAIやNPCなどの行動決定などを記述する際に多用されているようです。Navigation2では、ゲームではなく、リアルなロボットに対しての行動指針をビヘイビアツリーを用いて記述することで多くの行動パターンを設計できます。

　それではまず、以下のxmlファイルを作成・保存しましょう。筆者は、$HOME/btdata/sample.xmlとして保存しています。

bt_sample.xml

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <RateController hz="1.0">
        <Sequence>
          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
            <ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
          </GoalUpdater>
         <TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
        </Sequence>
      </RateController>
      <KeepRunningUntilFailure>
        <FollowPath path="{truncated_path}" controller_id="FollowPath"/>
      </KeepRunningUntilFailure>
    </PipelineSequence>
  </BehaviorTree>
</root>

3.クリックポイントの設定

　シミュレータ環境における追随対象のオブジェクトの位置をクリックした場所を検出した場所とします。
　具体的には、Rviz上でPublishPoint状態でマップ上でクリックした位置への経路を生成するというものです。そのためにはまず、PublishPointの座標をPoseStampedに変換する必要があり、それを行うために、このリポジトリのワークスペースを参照します。

cd ~/nav2_ws/src
git clone https://github.com/fmrico/nav2_test_utils
cd ../
colcon build

新しいターミナルにて

cd ~/nav2_ws
. install/setup.bash
ros2 run nav2_test_utils clicked_point_to_pose

とくにログは出ないので、エラーがなければパッケージはうまく動いているはずです。

4.Gazeboを使ってシミュレーションする

Gazeboを立ち上げます。

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

続いて、Navigation2をブリングアップしましょう。ビヘイビアツリーのデータを筆者とは異なる階層、名前で保存している方は、default_bt_xml_filename:=(ここにパス)で書き換えてください。

ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True map:=/opt/ros/foxy/share/nav2_bringup/maps/turtlebot3_world.yaml default_bt_xml_filename:=$HOME/btdata/sample.xml

そして、Rvizを立ち上げましょう。

ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/rviz/nav2_default_view.rviz

もしこの時点でclicked_point_to_poseを起動していなければ起動しましょう。

ros2 run nav2_test_utils clicked_point_to_pose

ここまで済んだら、2d pose estimateで初期位置を登録して、publish pointを押してから地図上の任意の点をクリックすると、そこへの経路設計とその実行が行われるはずです。公式にあるスクリーンキャプチャも参考になるのでぜひ活用ください。

※公式のチュートリアルでは以下のコマンドを実行して同じことを実行していますが、Gazeboのバージョンの問題なのかGazeboがマップのモデルをうまくロードできず、Gazebo上からLidarデータを取得できない状態になりました。それと加えて、Gazeboが立ち上がるのに非常に長い時間がかかります。

###非推奨のブリングアップ###
ros2 launch nav2_bringup tb3_simulation_launch.py default_bt_xml_filename:=$HOME/btdata/sample.xml

リアルワールドで動かしてみる

sshで実機にログインして実機のブリングアップを済ませましょう。

ssh ubuntu@{IP_ADDRESS_OF_RASPBERRY_PI}

ros2 launch turtlebot3_bringup robot.launch.py

次にnavigationの支度をしましょう。navigation2のブリングアップのオプションを少々変えただけで先ほどと同じ手順をたどります。リアルワールドの地図のパスを与えてあげる必要があるのでもしすでに作成済みの地図があるのであれば、map:=(path_to_your_map) のパスを記述してください。ほかに先ほどと違うのはシミュレータの持つ時間を使わないので、use_sim_time:=Falseに設定してあります。

ros2 launch nav2_bringup bringup_launch.py use_sim_time:=False map:=(path_to_your_map) default_bt_xml_filename:=$HOME/btdata/sample.xml

ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/rviz/nav2_default_view.rviz

ros2 run nav2_test_utils clicked_point_to_pose

デモ

実機で試しているときの、実際の様子とRvizのキャプチャです。人が通っても経路をきちんと再計画してます。

所感

正直、実機を使用する際のRviz自体の動作安定性が怪しく、目を離したすきに以下のエラーとともにクラッシュすることがかなりな頻度で発生していました。

[ERROR] [1646444626.719327318] [rviz2]: InternalErrorException: Cannot create GL vertex buffer in GLHardwareVertexBuffer::GLHardwareVertexBuffer at /tmp/binarydeb/ros-foxy-rviz-ogre-vendor-8.2.6/obj-x86_64-linux-gnu/ogre-v1.12.1-prefix/src/ogre-v1.12.1/RenderSystems/GL/src/OgreGLHardwareVertexBuffer.cpp (line 46)

一旦の解決策（Rvizを使用しないで初期位置と目的地を設定する方法）として、「UnityでNavigation2のゴールを送信するGUIを作る」にて紹介していますのでよろしければ合わせてご覧ください。

参考資料

Navigation2公式ドキュメント