FollowGPSWaypoints Action

Package: nav2_msgs
Category: Navigation

Navigate robot through GPS-based waypoints for outdoor navigation

Message Definitions

Goal Message

Field Type Description
number_of_loops uint32 How many times to repeat the complete waypoint sequence (0=no looping)
goal_index uint32 Starting waypoint index in the poses array (default 0 for beginning)
gps_poses geographic_msgs/GeoPose[] Array of GPS-based poses defining outdoor navigation waypoints with latitude/longitude coordinates

Result Message

Field Type Description
NONE uint16 Success status code indicating the action completed without errors
UNKNOWN uint16 Generic error code for unexpected or unclassified failures
TASK_EXECUTOR_FAILED uint16 Error code indicating a task executor plugin failed to execute at a waypoint
missed_waypoints MissedWaypoint[] Array of waypoints that could not be reached due to obstacles or navigation failures
error_code int16 Numeric error code indicating specific failure reason (0=success, various codes for different failure types)
error_msg string Human-readable error message describing what went wrong during action execution

Feedback Message

Field Type Description
current_waypoint uint32 feedback

Usage Examples

Python

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from nav2_msgs.action import FollowGPSWaypoints

class Nav2ActionClient(Node):
    def __init__(self):
        super().__init__('nav2_action_client')
        self.action_client = ActionClient(self, FollowGPSWaypoints, 'follow_gps_waypoints')
        
    def send_goal(self):
        goal_msg = FollowGPSWaypoints.Goal()
        from geographic_msgs.msg import GeoPose
        from geometry_msgs.msg import Point
        from geometry_msgs.msg import Quaternion
        
        # Create GPS waypoints
        gps_pose1 = GeoPose()
        gps_pose1.position.latitude = 37.4419    # Example latitude 
        gps_pose1.position.longitude = -122.1430 # Example longitude
        gps_pose1.position.altitude = 0.0
        gps_pose1.orientation.w = 1.0
        
        gps_pose2 = GeoPose()
        gps_pose2.position.latitude = 37.4420
        gps_pose2.position.longitude = -122.1431
        gps_pose2.position.altitude = 0.0
        gps_pose2.orientation.w = 1.0
        
        goal_msg.gps_poses = [gps_pose1, gps_pose2]
        goal_msg.number_of_loops = 1
        goal_msg.goal_index = 0
        
        self.action_client.wait_for_server()
        future = self.action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_callback)
        return future
        
    def feedback_callback(self, feedback_msg):
        self.get_logger().info(f'Received feedback: {feedback_msg.feedback}')

C++

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/follow_gps_waypoints.hpp"

class Nav2ActionClient : public rclcpp::Node
{
public:
    using FollowGPSWaypointsAction = nav2_msgs::action::FollowGPSWaypoints;
    using GoalHandle = rclcpp_action::ClientGoalHandle<FollowGPSWaypointsAction>;

    Nav2ActionClient() : Node("nav2_action_client")
    {
        action_client_ = rclcpp_action::create_client<FollowGPSWaypointsAction>(
            this, "follow_gps_waypoints");
    }

    void send_goal()
    {
        auto goal_msg = FollowGPSWaypointsAction::Goal();
        // Create GPS waypoints
        geographic_msgs::msg::GeoPose gps_pose1, gps_pose2;
        
        gps_pose1.position.latitude = 37.4419;    // Example latitude
        gps_pose1.position.longitude = -122.1430; // Example longitude
        gps_pose1.position.altitude = 0.0;
        gps_pose1.orientation.w = 1.0;
        
        gps_pose2.position.latitude = 37.4420;
        gps_pose2.position.longitude = -122.1431;
        gps_pose2.position.altitude = 0.0;
        gps_pose2.orientation.w = 1.0;
        
        goal_msg.gps_poses = {gps_pose1, gps_pose2};
        goal_msg.number_of_loops = 1;
        goal_msg.goal_index = 0;
        
        action_client_->wait_for_action_server();
        
        auto send_goal_options = rclcpp_action::Client<FollowGPSWaypointsAction>::SendGoalOptions();
        send_goal_options.feedback_callback = 
            std::bind(&Nav2ActionClient::feedback_callback, this, 
                     std::placeholders::_1, std::placeholders::_2);
        
        action_client_->async_send_goal(goal_msg, send_goal_options);
    }

private:
    rclcpp_action::Client<FollowGPSWaypointsAction>::SharedPtr action_client_;
    
    void feedback_callback(GoalHandle::SharedPtr, 
                          const std::shared_ptr<const FollowGPSWaypointsAction::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "Received feedback");
    }
};