🔌 Goal Navigation API#

Use the Goal Navigation Action in your code#

To demonstrate how to use the /navigate_to_pose in your code, we are going to use a new ROS 2 package called goal_navigation_tutorial, in which we are going to create a new C++ source file called dummy_navigate_to_pose_client.cpp and a Python script called dummy_navigate_to_pose_client_py.py.

#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include <chrono>

#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

class NavigateToPoseActionClient : public rclcpp::Node
{
public:
  using NavigateToPose = nav2_msgs::action::NavigateToPose;
  using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;

  explicit NavigateToPoseActionClient(const rclcpp::NodeOptions & options)
  : Node("navigate_to_pose_action_client", options)
  {
    this->client_ptr_ = rclcpp_action::create_client<NavigateToPose>(
      this,
      "/navigate_to_pose");

    this->timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      std::bind(&NavigateToPoseActionClient::send_goal, this));
  }

  void send_goal()
  {
    using namespace std::placeholders;

    this->timer_->cancel();

    if (!this->client_ptr_->wait_for_action_server()) {
      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
      rclcpp::shutdown();
    }

    auto goal_msg = NavigateToPose::Goal();

    goal_msg.pose.header.frame_id = "map";
    goal_msg.pose.header.stamp = this->now();
    // Goal position
    goal_msg.pose.pose.position.x = 1.0;
    goal_msg.pose.pose.position.y = 1.0;
    goal_msg.pose.pose.position.z = 0.0;
    // Goal orientation (Quaternion)
    goal_msg.pose.pose.orientation.x = 0.0;
    goal_msg.pose.pose.orientation.y = 0.0;
    goal_msg.pose.pose.orientation.z = 0.0;
    goal_msg.pose.pose.orientation.w = 1.0;
    // Behavior tree full path (if empty using "navigate_to_pose_w_replanning_and_recovery.xml")
    goal_msg.behavior_tree = "";
    RCLCPP_INFO(this->get_logger(), "Sending goal");

    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();

    send_goal_options.goal_response_callback =
      std::bind(&NavigateToPoseActionClient::goal_response_callback, this, _1);

    send_goal_options.feedback_callback =
      std::bind(&NavigateToPoseActionClient::feedback_callback, this, _1, _2);

    send_goal_options.result_callback =
      std::bind(&NavigateToPoseActionClient::result_callback, this, _1);

    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<NavigateToPose>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;

  void goal_response_callback(const GoalHandleNavigateToPose::SharedPtr & goal_handle)
  {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    } else {
      RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
    }
  }

  void feedback_callback(
    GoalHandleNavigateToPose::SharedPtr,
    const std::shared_ptr<const NavigateToPose::Feedback> feedback)
  {
    RCLCPP_INFO(
      this->get_logger(),
      "Received feedback: distance remaining: %f", feedback->distance_remaining);
  }

  void result_callback(const GoalHandleNavigateToPose::WrappedResult & result)
  {
    switch (result.code) {
      case rclcpp_action::ResultCode::SUCCEEDED:
        RCLCPP_INFO(this->get_logger(), "Goal was succeeded");
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
        return;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
        return;
      default:
        RCLCPP_ERROR(this->get_logger(), "Unknown result code");
        return;
    }
    RCLCPP_INFO(this->get_logger(), "Result received");
    rclcpp::shutdown();
  }
};  // class NavigateToPoseActionClient

RCLCPP_COMPONENTS_REGISTER_NODE(NavigateToPoseActionClient)

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<NavigateToPoseActionClient>(rclcpp::NodeOptions());
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

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


class NavigateToPoseActionClient(Node):

    def __init__(self):
        super().__init__('navigate_to_pose_client')
        self._action_client = ActionClient(
            self, NavigateToPose, '/navigate_to_pose')

    def send_goal(self):
        goal_msg = NavigateToPose.Goal()

        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        # Goal position
        goal_msg.pose.pose.position.x = 1.0
        goal_msg.pose.pose.position.y = 1.0
        goal_msg.pose.pose.position.z = 0.0
        # Goal orientation
        goal_msg.pose.pose.orientation.x = 0.0
        goal_msg.pose.pose.orientation.y = 0.0
        goal_msg.pose.pose.orientation.z = 0.0
        goal_msg.pose.pose.orientation.w = 1.0
        # Behavior tree full path (if empty using "navigate_to_pose_w_replanning_and_recovery.xml")
        goal_msg.behavior_tree = ''

        self.get_logger().info('Waiting for action server...')
        self._action_client.wait_for_server()

        self.get_logger().info('Sending goal...')
        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg, feedback_callback=self.feedback_callback)
        self._send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info('Goal rejected :(')
            return

        self.get_logger().info('Goal accepted :)')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        future.result()
        self.get_logger().info('Result received')
        rclpy.shutdown()

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        self.get_logger().info(
            f'Received feedback: distance remaining: {feedback.distance_remaining}')


def main(args=None):
    rclpy.init(args=args)
    action_client = NavigateToPoseActionClient()
    action_client.send_goal()
    rclpy.spin(action_client)


if __name__ == '__main__':
    main()

Use the Goal Navigation Topic in your code#

To demonstrate how to use the /goal_pose in your code, you can refer to the goal_navigation_tutorial, in which we are going to create a new C++ source file called dummy_goal_pose_publisher.cpp and a Python script called dummy_goal_pose_publisher_py.py.

#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

class GoalPosePublisher : public rclcpp::Node
{
public:
  GoalPosePublisher()
  : Node("goal_pose_publisher")
  {
    publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("/goal_pose", 10);
    timer_ = this->create_wall_timer(
      std::chrono::seconds(1),
      [this]() {this->timer_callback();}
    );
  }

private:
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;

  void timer_callback()
  {
    auto msg = geometry_msgs::msg::PoseStamped();
    msg.header.stamp = this->get_clock()->now();
    msg.header.frame_id = "map";
    // Goal position
    msg.pose.position.x = 1.5;
    msg.pose.position.y = 1.5;
    msg.pose.position.z = 0.0;
    // Goal orientation (Quaternion)
    msg.pose.orientation.x = 0.0;
    msg.pose.orientation.y = 0.0;
    msg.pose.orientation.z = 0.0;
    msg.pose.orientation.w = 1.0;

    publisher_->publish(msg);
    RCLCPP_INFO(
      this->get_logger(), "Publishing goal pose: [x: %.2f, y: %.2f, z: %.2f]",
      msg.pose.position.x, msg.pose.position.y, msg.pose.position.z);
    timer_->cancel();
  }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);

  auto node = std::make_shared<GoalPosePublisher>();

  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped


class GoalPosePublisher(Node):

    def __init__(self):
        super().__init__('goal_pose_publisher')
        self.publisher = self.create_publisher(
            PoseStamped,
            '/goal_pose',
            10)
        timer_period = 1.0  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):

        msg = PoseStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'map'
        # Goal Position
        msg.pose.position.x = 1.5
        msg.pose.position.y = 1.5
        msg.pose.position.z = 0.0
        # Goal orientation (Quaternion)
        msg.pose.orientation.x = 0.0
        msg.pose.orientation.y = 0.0
        msg.pose.orientation.z = 0.0
        msg.pose.orientation.w = 1.0

        self.publisher.publish(msg)
        self.get_logger().info('Publishing goal pose: [x: %.2f, y: %.2f, z: %.2f]' %
                               (msg.pose.position.x, msg.pose.position.y, msg.pose.position.z))
        self.timer.cancel()


def main(args=None):
    rclpy.init(args=args)

    publisher = GoalPosePublisher()

    rclpy.spin(publisher)

    rclpy.shutdown()


if __name__ == '__main__':
    main()

ROS 2 API#

To interact with the Goal Navigation, you can refer to the following ROS 2 interfaces:

Topics	Actions
/goal_pose	/navigate_to_pose
/cmd_vel	/backup
/global_costmap/costmap	/spin
/global_costmap/footprint	/wait
/local_costmap/costmap	/compute_path_to_pose
/local_costmap/footprint	/compute_path_through_poses
/plan	/follow_path
	/drive_on_heading