この記事の内容
ROS 2で自律移動ロボット開発を行うにあたり、Nav2を使用している方に向けて書いています。
Turtlebot3を使って、RViz2で動かせるようになったけれども、もう少し中身が知りたいという方に向けて書いています。Rviz2で実際にどういうコマンドが実行されているのかを解説していきます。ここから、なんとなくのイメージがつかんでいただけると幸いです。
ナビゲーション時の動作の流れ
RViz2で自律移動させるときの流れとしては、2D Pose Estimateで初期姿勢を設定して、Nav2 Goalでゴール姿勢を設定する流れで操作させることが多いと思います。その流れに沿って、解説をしていきます。
2D Pose Estimate
まず自律移動ロボットを動かすときに初めにやるのが、RViz2で2D PoseEstimateボタンを押して、ロボットの初期姿勢を教えてあげることがだと思います。この機能はRViz2のdefault pluginsに入ってますのでそこから何をしているのか見ていきます。
以下のプログラムが2D PoseEstimateの処理実装の部分になります。
ここでのポイントは、どのように初期姿勢を設定しているかで、プログラムを確認するとどうやら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の点群などから自己位置推定するモジュールで、自己位置推定するために初期姿勢を設定する必要があるようです。
Nav2 Goal
初期姿勢を設定したら、次はゴールの位置を設定するためにNav2 Goalを押下して、ゴールの姿勢を設定するかと思います。2D Pose Estimateと同様に、RViz2のプログラムを確認してみます。
プログラムを確認するとどうやら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をもう少し深堀していきます。プログラムは以下になります。
bt_navigatorの中を見てみると、2つのナビゲータを生成していることがわかります。NavigateToPoseNavigatorとNavigateThroughPoseNavigatorというナビゲータです。
NavigateToPoseNavigatorは、単一の目的地を与えるとそこに向かってナビゲーションしていくナビゲータです。/goal_poseトピックをサブスクライブしているのはこのナビゲータです。
NavigateThroughPoseNavigatorは、途中の経由地点(ウェイポイント)を指定して動作させることができるナビゲータです。
実際にそれぞれのナビゲータを使用した際の経路がどうなるのかをRViz2で表示した画像を以下に示します。
次は、NavigateToPoseNavigatorについてみていきます。
NavigateToPoseNavigator
NavigateToPoseNavigatorのプログラムは以下になります。
goal_poseトピックも以下の箇所で定義があります。
goal_poseトピックでも指定ができるのですが、ROS 2のNavigateToPoseアクションでもナビゲーションを行うことができます。これは、NavigateToPoseNavigatorのヘッダを見てもらえればわかるのですが、このナビゲータは、nav2_bt_navigator::Navigator<class ActionT>を継承しており、NavigateToPoseのアクションサーバになっています。途中のフィードバックや完了した通知が必要であればこちらのアクションを使うとよいかと思います。
ビヘイビアツリーのファイルを読み込んでいることもわかります。デフォルトでは、/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xmlを読み込むようです。
/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xmlについては以下に存在します。
このビヘイビアツリーの処理をものすごく大雑把に説明した内容が以下になります。
ComputePathToPose
ビヘイビアツリーに記載があったComuputePathToPoseは、現在地から目的地までの経路を計算することでした。ビヘイビアツリーの1つ1つのアイテム(ちゃんとした用語だとノードというようです)はビヘイビアツリーのプラグインになっています。ComputePathToPoseのアイテムも以下の箇所にプラグインとして存在します。
このプラグインの中で経路を算出しているわけではなく、パスを算出して返してくれるアクション(/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のアイテムと同様に以下の箇所にプラグインとして存在します。
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シミュレーション時)上記の内容を読んでいただき、一部でも読み解けるようになりましたら幸いです。
コメント