ComputePathThroughPoses Action

Package: nav2_msgs
Category: Planning

Compute an optimal path connecting multiple poses in sequence

Message Definitions

Goal Message

Field Type Description
goals geometry_msgs/PoseStamped[] Array of target poses that the path should connect through in sequence
start geometry_msgs/PoseStamped Starting pose for path planning
planner_id string Name of the specific planning algorithm to use (e.g., “GridBased”, “NavfnPlanner”). If empty with single planner, uses default
use_start bool If false, use current robot pose as path start, if true, use start above instead

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
INVALID_PLANNER uint16 Error code indicating the specified planner plugin is invalid or not loaded
TF_ERROR uint16 Error code indicating a transform/localization failure
START_OUTSIDE_MAP uint16 Error code indicating the start position is outside the known map boundaries
GOAL_OUTSIDE_MAP uint16 Error code indicating the goal position is outside the known map boundaries
START_OCCUPIED uint16 Error code indicating the start position is in an occupied/blocked area
GOAL_OCCUPIED uint16 Error code indicating the goal position is in an occupied/blocked area
TIMEOUT uint16 Error code indicating the action exceeded its maximum allowed time
NO_VALID_PATH uint16 Error code indicating no feasible path could be found between start and goal
NO_VIAPOINTS_GIVEN uint16 Error code indicating no intermediate waypoints were provided for path planning through poses
path nav_msgs/Path Computed navigation path with poses and metadata
planning_time builtin_interfaces/Duration Time spent in path planning phase
error_code uint16 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

No fields defined.

Usage Examples

Python

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

class Nav2ActionClient(Node):
    def __init__(self):
        super().__init__('nav2_action_client')
        self.action_client = ActionClient(self, ComputePathThroughPoses, 'compute_path_through_poses')
        
    def send_goal(self):
        goal_msg = ComputePathThroughPoses.Goal()
        from nav_msgs.msg import Goals
        from geometry_msgs.msg import PoseStamped
        
        # Create goals to connect
        goals = Goals()
        
        pose1 = PoseStamped()
        pose1.header.frame_id = 'map'
        pose1.pose.position.x = 1.0
        pose1.pose.position.y = 1.0
        pose1.pose.orientation.w = 1.0
        
        pose2 = PoseStamped()
        pose2.header.frame_id = 'map'
        pose2.pose.position.x = 3.0
        pose2.pose.position.y = 2.0
        pose2.pose.orientation.w = 1.0
        
        goals.poses = [pose1, pose2]
        goal_msg.goals = goals
        goal_msg.planner_id = 'GridBased'
        goal_msg.use_start = False
        
        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/compute_path_through_poses.hpp"

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

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

    void send_goal()
    {
        auto goal_msg = ComputePathThroughPosesAction::Goal();
        // Create goals to connect
        nav_msgs::msg::Goals goals;
        geometry_msgs::msg::PoseStamped pose1, pose2;
        
        pose1.header.frame_id = "map";
        pose1.pose.position.x = 1.0;
        pose1.pose.position.y = 1.0;
        pose1.pose.orientation.w = 1.0;
        
        pose2.header.frame_id = "map";
        pose2.pose.position.x = 3.0;
        pose2.pose.position.y = 2.0;
        pose2.pose.orientation.w = 1.0;
        
        goals.poses = {pose1, pose2};
        goal_msg.goals = goals;
        goal_msg.planner_id = "GridBased";
        goal_msg.use_start = false;
        
        action_client_->wait_for_action_server();
        
        auto send_goal_options = rclcpp_action::Client<ComputePathThroughPosesAction>::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<ComputePathThroughPosesAction>::SharedPtr action_client_;
    
    void feedback_callback(GoalHandle::SharedPtr, 
                          const std::shared_ptr<const ComputePathThroughPosesAction::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "Received feedback");
    }
};