🥅 Goal Navigation#

The Goal Navigation is a functionality that make a robot navigate autonomously from a Start pose to a Goal pose. The Start pose can coincide with the robot’s current pose or can be chosen arbitrarily while the Goal pose has to be specified by end-user.

The Goal Navigation is a common use case for autonomous robots, where the robot is required to navigate to a specific location. The purpose of this chapter is to provide an overview of the Goal Navigation functionality, including how to configure and use it, as well as how to create custom logics for Goal Navigation.

Behavior Trees#

The navigation logic is regulated by a Behavior Tree which defines how the robot navigates and behaves during its journey from Start pose to Goal pose . A Behavior Tree is a modular and hierarchical structure that allows for flexible and reactive decision-making, it is particularly useful for handling complex robot behaviors and ensuring robust and adaptable navigation strategies. The Behavior Tree structure allows the navigation process to be broken down into smaller, manageable tasks (nodes), which are executed in a specific sequence or in parallel based on conditions and priorities. This modular approach enhances the robot’s ability to handle dynamic environments and unexpected situations, ensuring robust and adaptive navigation.

For more information about Behavior Trees and their usage, check BehaviorTree.CPP.

Usage#

The Goal Navigation uses the bt_navigator module which implements the Nav2 NavigateToPose task interface. This task interface is responsible for loading and executing a certain navigation logic defined in a Behavior Tree. The Goal Navigation offers several interfaces to start using its functionalities:

ROS 2 Action#

The Goal Navigation can be started through the /navigate_to_pose. By sending an Action Goal to this Server containing the desired Goal position, it will trigger the Navigation system that will make the robot reach the desired position. Thus, in this Action Goal, you need to specify the desired Goal Pose and the Behavior Tree to be used.

Send an action goal to the NavigateToPose action server#

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "pose:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: 'map' # The frame in which the pose is expressed
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
behavior_tree: ''" # Full/path/to/the/desired/Behavior/Tree

Note

Change the values of the position and orientation fields to the desired Goal Pose you want to reach. To use a custom Behavior Tree, specify the full path to the desired Behavior Tree file in the behavior_tree field. By default, if the behavior_tree field is empty, the navigate_to_pose_w_replanning_and_recovery.xml is used.

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()

ROS 2 Topic#

Alternatively, to trigger the Goal Navigation, one can simply publish a Goal Pose to the /goal_pose.

Publish a Goal Pose on the goal_pose topic#

ros2 topic pub /goal_pose geometry_msgs/msg/PoseStamped "header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: 'map' # The frame in which the pose is expressed
pose:
  position:
    x: 0.0
    y: 0.0
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0"

Note

Change the values of the position and orientation fields to the desired Goal Pose you want to reach.

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()

RViz#

It is also possible to use the RViz GUI to trigger the Goal Navigation by setting the desired goal position using the “2D Nav Goal” tool. This will send a new Action Goal to the /navigate_to_pose Server.