ROS 2 Nav2 Waypoint Follower Guide 2026

Waypoint Follower Architecture

Installation

sudo apt install ros-jazzy-nav2-waypoint-follower
sudo apt install ros-jazzy-nav2-bringup     # includes waypoint follower launch

The waypoint follower is included in the standard nav2_bringup launch.

follow_waypoints Action Interface

# Action type: nav2_msgs/action/FollowWaypoints

# Goal
geometry_msgs/PoseStamped[] poses   # ordered list of waypoints

# Result
int32[] missed_waypoints            # indices of waypoints that failed

# Feedback
uint32 current_waypoint             # index currently being navigated to

Python Action Client

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from nav2_msgs.action import FollowWaypoints
from geometry_msgs.msg import PoseStamped
from rclpy.duration import Duration

class WaypointSender(Node):
    def __init__(self):
        super().__init__("waypoint_sender")
        self._client = ActionClient(self, FollowWaypoints, "follow_waypoints")

    def send_waypoints(self, poses: list[PoseStamped]):
        self._client.wait_for_server()
        goal = FollowWaypoints.Goal()
        goal.poses = poses
        future = self._client.send_goal_async(
            goal,
            feedback_callback=self._on_feedback,
        )
        future.add_done_callback(self._goal_response_cb)

    def _goal_response_cb(self, future):
        handle = future.result()
        if not handle.accepted:
            self.get_logger().error("Goal rejected!")
            return
        self.get_logger().info("Goal accepted")
        handle.get_result_async().add_done_callback(self._result_cb)

    def _result_cb(self, future):
        result = future.result().result
        missed = result.missed_waypoints
        if missed:
            self.get_logger().warn(f"Missed waypoints: {missed}")
        else:
            self.get_logger().info("All waypoints reached!")

    def _on_feedback(self, feedback_msg):
        idx = feedback_msg.feedback.current_waypoint
        self.get_logger().info(f"Navigating to waypoint {idx}")


def make_pose(x, y, yaw_deg=0.0, frame="map") -> PoseStamped:
    import math
    p = PoseStamped()
    p.header.frame_id = frame
    p.pose.position.x = x
    p.pose.position.y = y
    half = math.radians(yaw_deg) / 2.0
    p.pose.orientation.z = math.sin(half)
    p.pose.orientation.w = math.cos(half)
    return p


def main():
    rclpy.init()
    node = WaypointSender()
    waypoints = [
        make_pose(1.0,  0.0),
        make_pose(1.0,  1.0, yaw_deg=90),
        make_pose(0.0,  1.0, yaw_deg=180),
        make_pose(0.0,  0.0, yaw_deg=270),
    ]
    node.send_waypoints(waypoints)
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()

waypoint_follower YAML Parameters

# In nav2_params.yaml
waypoint_follower:
  ros__parameters:
    loop_rate: 20                   # Hz — check rate
    stop_on_failure: false          # continue to next waypoint on failure
    action_server_result_timeout: 900.0  # max seconds total mission

    # Task executor plugin (optional)
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200  # milliseconds to pause at each waypoint

stop_on_failure Behavior

WaypointTaskExecutor Plugins

Custom Task Executor

#include "nav2_core/waypoint_task_executor.hpp"

class MyTaskExecutor : public nav2_core::WaypointTaskExecutor {
public:
  void initialize(
    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
    const std::string & plugin_name) override
  {
    node_ = parent;
    // load parameters from plugin_name namespace
  }

  bool processAtWaypoint(
    const geometry_msgs::msg::PoseStamped & curr_pose,
    const int & curr_waypoint_index) override
  {
    // Custom logic: take photo, call service, etc.
    RCLCPP_INFO(node_.lock()->get_logger(),
      "At waypoint %d — running custom task", curr_waypoint_index);
    return true;  // false = waypoint failed
  }
};

// plugin registration (in .cpp file)
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(MyTaskExecutor, nav2_core::WaypointTaskExecutor)

Cancel / Pause a Mission

# Cancel the active follow_waypoints goal
ros2 action cancel /follow_waypoints

# From Python — cancel via goal handle
goal_handle.cancel_goal()

# Pause by cancelling, then re-send from missed_waypoints index
# (Nav2 has no built-in pause — implement via cancel + resume pattern)

GPS Global Waypoints

Combine with robot_localization to send lat/lon GPS waypoints converted to map frame:

# In nav2_params.yaml — enable GPS waypoint follower
gps_waypoint_follower:
  ros__parameters:
    action_server_result_timeout: 900.0
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 0

# Python GPS mission client
from nav2_msgs.action import FollowGPSWaypoints
from geographic_msgs.msg import GeoPose

client = ActionClient(node, FollowGPSWaypoints, "follow_gps_waypoints")

goal = FollowGPSWaypoints.Goal()
gp = GeoPose()
gp.position.latitude  = 37.5665
gp.position.longitude = 126.9780
gp.position.altitude  = 50.0
goal.gps_poses = [gp]
client.send_goal_async(goal)

Common Issues