概要
Nav2でWaypoint Followerがどのようなものか、使い方を記載します。
まず、Waypointというのは、ゴールに向かっていくときのポイントになります。ここに向かってから、ここに向かってというように、途中途中のポイントを指定していくイメージです。WPと略すこともあります。
そして、WaypointFollowerというのは、Nav2で用意されているプログラムで、ウェイポイントのリストを指定することで、ウェイポイントに沿って動作させることができるようになっています。
参考
環境
以下の環境を前提とします。
Turtlebot3のシミュレーションについては
- ROS 2 Humble
- Turtlebot3 Burger, Waffle Piのシミュレーションを使用
Turtlebot3のシミュレーションについては以下を参考に環境構築をします。
使用方法
まずは、シンプルにRViz2からWayPointを指定して動かしていきます。そのために、GazeboとNav2を起動していきます。
Gazeboの起動。
export TURTLEBOT3_MODEL=burger ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
Nav2の起動。
export TURTLEBOT3_MODEL=burger ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True
実は、これでWaypoint Followerも使えるようになってます。ros2 node listで見てみると、/waypoint_followerというROS 2ノードが起動しており、これがWaypoint Followerになります。
$ ros2 node list /amcl /behavior_server /bt_navigator /bt_navigator_navigate_through_poses_rclcpp_node /bt_navigator_navigate_to_pose_rclcpp_node /controller_server /gazebo ~~~ 中略 ~~~ /waypoint_follower
ros2 node info /waypoint_followerでwaypoint_followerノードの情報を確認してみます。
すると、/follow_waypointsというアクションサーバを持っていることがわかります。このアクションを呼び出すことでWaypointを指定して動作させることができます。waypoint_followerの処理としては、各WayPoint到着時に次の/navigate_to_poseアクションを呼び出して動作するといったイメージです。
ros2 node info /waypoint_follower /waypoint_follower Subscribers: /bond: bond/msg/Status /parameter_events: rcl_interfaces/msg/ParameterEvent Publishers: /bond: bond/msg/Status /parameter_events: rcl_interfaces/msg/ParameterEvent /rosout: rcl_interfaces/msg/Log /waypoint_follower/transition_event: lifecycle_msgs/msg/TransitionEvent Service Servers: /waypoint_follower/change_state: lifecycle_msgs/srv/ChangeState /waypoint_follower/describe_parameters: rcl_interfaces/srv/DescribeParameters /waypoint_follower/get_available_states: lifecycle_msgs/srv/GetAvailableStates /waypoint_follower/get_available_transitions: lifecycle_msgs/srv/GetAvailableTransitions /waypoint_follower/get_parameter_types: rcl_interfaces/srv/GetParameterTypes /waypoint_follower/get_parameters: rcl_interfaces/srv/GetParameters /waypoint_follower/get_state: lifecycle_msgs/srv/GetState /waypoint_follower/get_transition_graph: lifecycle_msgs/srv/GetAvailableTransitions /waypoint_follower/list_parameters: rcl_interfaces/srv/ListParameters /waypoint_follower/set_parameters: rcl_interfaces/srv/SetParameters /waypoint_follower/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically Service Clients: Action Servers: /follow_waypoints: nav2_msgs/action/FollowWaypoints Action Clients: /navigate_to_pose: nav2_msgs/action/NavigateToPose
実際の動きを見てみましょう。follow_waypointアクションを呼び出すプログラムを書いてもよいですが、ここでは、Navigation2のRViz2のPanelを使ってみます。おそらくRViz2にすでに表示されているのではないかと思います。非表示になっている場合はメニューのPanels > Navigation2を選択することで表示されるかと思います。
Navigation2のPanelから、Waypoint/NavThrough Poses Modeを押して、モードを切り替えます。
その後、Nav2 Goalを指定していくと、WayPointを指定することができます。
WayPoint指定後に、Start Waypoint Followingボタンを押下することで、ウェイポイントに沿って動作します。
以下のように動作するかと思います。(一部早送りしてます)
Waypoint Follower Plugin
Waypoint Followerではもっと面白いことができるようになってます。
Waypoint Followerのプラグインがあります。これを使うことで、WayPointに到達したときに、写真を撮ったり、入力待ちをしたりすることができるようになっています。
以下のプラグインが存在します。
WaitAtWaypoint:Waypoint到達時に、指定した時間一時停止します。
PhotoAtWaypoint:ウェイポイント到達時に写真をとります。
InputAtWaypoint:ウェイポイントでタイムアウト付きで外部入力を待機できるようにします。
もちろんPluginなので、オリジナルのプラグインを作成することもできます。(nav2_core::WaypointTaskExecutorを継承して作成する)
上記のPluginのうち、PhotoAtWaypointを使用して、ウェイポイントで写真を撮って動くようにしていこうと思います。
PhotoAtWaypointを使ってみる
ではPhotoAtWaypointを使ってみます。ロボットについているカメラ情報が欲しいため、burgerではなく、waffle_piを使用していきます。waffle_piのwaypoint_followerの設定を以下のように変更します。
/opt/ros/humble/share/turtlebot3_navigation2/param/waffle_pi.yaml
waypoint_follower: ros__parameters: loop_rate: 200 stop_on_failure: false waypoint_task_executor_plugin: "photo_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" enabled: False waypoint_pause_duration: 200 photo_at_waypoint: plugin: "nav2_waypoint_follower::PhotoAtWaypoint" enabled: True image_topic: "/camera/image_raw" save_dir: "/tmp/waypoint_images" image_format: "png"
この設定ではWayPoint到達時に/camera/image_rawトピックの画像情報を、/tmp/waypoint_imagesにpngファイルを保存する形になります。
Gazeboの起動。
export TURTLEBOT3_MODEL=waffle_pi ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
Nav2の起動。
export TURTLEBOT3_MODEL=waffle_pi ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True
RViz2でWayPointを指定して動作させていきます。
WayPoint到達時に/tmp/waypoint_imagesに画像が増えていれば成功です。
コメント