【ROS 2】Nav2の動作概要

ROS

この記事の内容

ROS 2で自律移動ロボット開発を行うにあたり、Nav2を使用している方に向けて書いています。

Turtlebot3を使って、RViz2で動かせるようになったけれども、もう少し中身が知りたいという方に向けて書いています。Rviz2で実際にどういうコマンドが実行されているのかを解説していきます。ここから、なんとなくのイメージがつかんでいただけると幸いです。

ナビゲーション時の動作の流れ

RViz2で自律移動させるときの流れとしては、2D Pose Estimateで初期姿勢を設定して、Nav2 Goalでゴール姿勢を設定する流れで操作させることが多いと思います。その流れに沿って、解説をしていきます。

2D Pose Estimate

まず自律移動ロボットを動かすときに初めにやるのが、RViz2で2D PoseEstimateボタンを押して、ロボットの初期姿勢を教えてあげることがだと思います。この機能はRViz2のdefault pluginsに入ってますのでそこから何をしているのか見ていきます。

以下のプログラムが2D PoseEstimateの処理実装の部分になります。

https://github.com/ros2/rviz/blob/humble/rviz_default_plugins/src/rviz_default_plugins/tools/pose_estimate/initial_pose_tool.cpp

ここでのポイントは、どのように初期姿勢を設定しているかで、プログラムを確認するとどうやらinitialposeというトピックをパブリッシュしているようですね。

では、このinitialposeをサブスクライブしているのは誰なのかを確認してみます。ros2 topic infoコマンドで誰がサブスクライバなのかを確認します。

$ ros2 topic info /initialpose -v
Type: geometry_msgs/msg/PoseWithCovarianceStamped

Publisher count: 1

Node name: rviz2
Node namespace: /
Topic type: geometry_msgs/msg/PoseWithCovarianceStamped
Endpoint type: PUBLISHER
GID: 01.0f.e7.c6.b9.5c.dd.52.01.00.00.00.00.00.42.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 1

Node name: amcl
Node namespace: /
Topic type: geometry_msgs/msg/PoseWithCovarianceStamped
Endpoint type: SUBSCRIPTION
GID: 01.0f.e7.c6.b7.5c.39.da.01.00.00.00.00.00.89.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

amclというノードがサブスクライブしていることがわかりました。amclはSLAMで取得したマップや2DLiDARの点群などから自己位置推定するモジュールで、自己位置推定するために初期姿勢を設定する必要があるようです。

AMCL — Nav2 1.0.0 documentation

Nav2 Goal

初期姿勢を設定したら、次はゴールの位置を設定するためにNav2 Goalを押下して、ゴールの姿勢を設定するかと思います。2D Pose Estimateと同様に、RViz2のプログラムを確認してみます。

https://github.com/ros2/rviz/blob/humble/rviz_default_plugins/src/rviz_default_plugins/tools/goal_pose/goal_tool.cpp

プログラムを確認するとどうやらgoal_poseというトピックをパブリッシュしているようですね。

goal_poseをサブスクライブしているのは誰なのかを確認してみます。

$ ros2 topic info /goal_pose -v
Type: geometry_msgs/msg/PoseStamped

Publisher count: 0

Subscription count: 1

Node name: bt_navigator
Node namespace: /
Topic type: geometry_msgs/msg/PoseStamped
Endpoint type: SUBSCRIPTION
GID: 01.0f.e7.c6.b7.5c.39.da.01.00.00.00.00.02.41.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

bt_navigatorノードがサブスクライブしていることがわかりました。bt_navigatorは、bt(Behavior Trees)を使用したナビゲータです。

bt_navigatorはナビゲーション機能をつかさどる親玉的な役割で、Controller ServerやPlanner Server Behaivior Serverなどに指示を与えていきます。Behavior Treesはどのようにしてナビゲーションしていくのかが書いてある指示書のようなものです。

Nav2で以下の構成が参考になります。

引用元: https://navigation.ros.org/

bt_navigator

bt_navigatorをもう少し深堀していきます。プログラムは以下になります。

https://github.com/ros-planning/navigation2/blob/humble/nav2_bt_navigator/src/bt_navigator.cpp

bt_navigatorの中を見てみると、2つのナビゲータを生成していることがわかります。NavigateToPoseNavigatorとNavigateThroughPoseNavigatorというナビゲータです。

NavigateToPoseNavigatorは、単一の目的地を与えるとそこに向かってナビゲーションしていくナビゲータです。/goal_poseトピックをサブスクライブしているのはこのナビゲータです。

NavigateThroughPoseNavigatorは、途中の経由地点(ウェイポイント)を指定して動作させることができるナビゲータです。

実際にそれぞれのナビゲータを使用した際の経路がどうなるのかをRViz2で表示した画像を以下に示します。

次は、NavigateToPoseNavigatorについてみていきます。

NavigateToPoseNavigator

NavigateToPoseNavigatorのプログラムは以下になります。

https://github.com/ros-planning/navigation2/blob/humble/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

goal_poseトピックも以下の箇所で定義があります。

goal_poseトピックでも指定ができるのですが、ROS 2のNavigateToPoseアクションでもナビゲーションを行うことができます。これは、NavigateToPoseNavigatorのヘッダを見てもらえればわかるのですが、このナビゲータは、nav2_bt_navigator::Navigator<class ActionT>を継承しており、NavigateToPoseのアクションサーバになっています。途中のフィードバックや完了した通知が必要であればこちらのアクションを使うとよいかと思います。

https://github.com/ros-planning/navigation2/blob/humble/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp

ビヘイビアツリーのファイルを読み込んでいることもわかります。デフォルトでは、/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xmlを読み込むようです。

/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xmlについては以下に存在します。

https://github.com/ros-planning/navigation2/blob/humble/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml

このビヘイビアツリーの処理をものすごく大雑把に説明した内容が以下になります。

ComputePathToPose

ビヘイビアツリーに記載があったComuputePathToPoseは、現在地から目的地までの経路を計算することでした。ビヘイビアツリーの1つ1つのアイテム(ちゃんとした用語だとノードというようです)はビヘイビアツリーのプラグインになっています。ComputePathToPoseのアイテムも以下の箇所にプラグインとして存在します。

https://github.com/ros-planning/navigation2/blob/humble/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp

このプラグインの中で経路を算出しているわけではなく、パスを算出して返してくれるアクション(/comupute_path_to_pose)があるのでそれを呼び出しています。compute_path_to_poseのサーバがどのノードで起動しているのかをコマンドで確認してみます。

$ ros2 action info /compute_path_to_pose -t
Action: /compute_path_to_pose
Action clients: 1
    /bt_navigator_navigate_to_pose_rclcpp_node [nav2_msgs/action/ComputePathToPose]
Action servers: 1
    /planner_server [nav2_msgs/action/ComputePathToPose]

すると、/planner_serverが提供しているということがわかりました。ここから、前述した構成図で、BT Navigator ServerとPlanner Serverがどのようにつながっているかが確認できました。

FollowPath

FollowPathは、計算された経路に沿ってロボットをナビゲートする機能になります。

FollowPathのアイテムも、ComputePathToPoseのアイテムと同様に以下の箇所にプラグインとして存在します。

https://github.com/ros-planning/navigation2/blob/humble/nav2_behavior_tree/plugins/action/follow_path_action.cpp

follow_path_actionのプラグインでは、/follow_pathアクションを呼び出して、ロボットの移動を行っています。

follow_pathのサーバがどのノードで起動しているのかをコマンドで確認してみます。

$ ros2 action info /follow_path -t
Action: /follow_path
Action clients: 2
    /bt_navigator_navigate_to_pose_rclcpp_node [nav2_msgs/action/FollowPath]
    /bt_navigator_navigate_through_poses_rclcpp_node [nav2_msgs/action/FollowPath]
Action servers: 1
    /controller_server [nav2_msgs/action/FollowPath]

すると、/controller_server が提供しているということがわかりました。ここから、前述した構成図で、BT Navigator ServerとController Serverがどのようにつながっているかが確認できました。

ノードグラフ

最後にTurtlebot3をNav2で動作できるように起動した際のノードグラフを張り付けておきます。(Gazeboシミュレーション時)上記の内容を読んでいただき、一部でも読み解けるようになりましたら幸いです。

コメント