【ROS 2】Nav2 Waypoint Followerで複数の向かうポイントを指定する

ROS

概要

Nav2でWaypoint Followerがどのようなものか、使い方を記載します。

まず、Waypointというのは、ゴールに向かっていくときのポイントになります。ここに向かってから、ここに向かってというように、途中途中のポイントを指定していくイメージです。WPと略すこともあります。

そして、WaypointFollowerというのは、Nav2で用意されているプログラムで、ウェイポイントのリストを指定することで、ウェイポイントに沿って動作させることができるようになっています。

参考

Waypoint Follower — Nav2 1.0.0 documentation
navigation.ros.org
navigation2/nav2_waypoint_follower at main · ros-planning/navigation2
ROS2 Navigation Framework and System. Contribute to ros-planning/navigation2 development by creating an account on GitHu...
github.com

環境

以下の環境を前提とします。

Turtlebot3のシミュレーションについては

  • ROS 2 Humble
  • Turtlebot3 Burger, Waffle Piのシミュレーションを使用

Turtlebot3のシミュレーションについては以下を参考に環境構築をします。

ROBOTIS e-Manual
e-Manual wiki
emanual.robotis.com

使用方法

まずは、シンプルに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到達時に、指定した時間一時停止します。

Waypoint Follower — Nav2 1.0.0 documentation
navigation.ros.org

PhotoAtWaypoint：ウェイポイント到達時に写真をとります。

PhotoAtWaypoint — Nav2 1.0.0 documentation
navigation.ros.org

InputAtWaypoint：ウェイポイントでタイムアウト付きで外部入力を待機できるようにします。

InputAtWaypoint — Nav2 1.0.0 documentation
navigation.ros.org

もちろん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に画像が増えていれば成功です。

コメント