Interface nav2 with custom ROS2 nodes

Interface `nav2` with custom ROS2 nodes#

Added in version Jazzy: This section.

Warning

This topic is under construction and this might not even be its final form. Please feel free to open an issue if you spot any typos or other problems.

Objective#

We are going to create custom code to interact with the navigation stack, nav2. This will be based on the demo tb3_simulation_launch.py, but it is generally applicable. In the example, rviz2 is used for interactivity. In this example, we will be able to operate nav2 automatically, without relying on the visualizer.

Create a publisher to send the initial pose.
Create an action client to send navigation goals.

Important

Differently from everything else used in this tutorial, this tb3_simulation_launch.py demo does not always finishes cleanly on my machines. It has hanged sometimes, sometimes Gazebo does not show, and sometimes it does not shutdown properly.

It is highly recommended to use a docker container, such as UoMMScRobotics/sfr_gazebo_nav2, to minimise disappointment.

When dealing with a new stack you might not be familiar with, the first step is to try to make sense of the interfaces available.

Initial pose topic#

When the tb3_simulation_launch.py example is running, if we run ros2 topic list you will see a long list of topics. Let us filter them out with the keyword pose, giving that we’re trying to find things related to pose. We can do so with the command below.

ros2 topic list | grep pose

The output of that will be the following topics.

/amcl_pose
/detected_dock_pose
/dock_pose
/filtered_dock_pose
/goal_pose
/initialpose
/staging_pose

This is convenient for us because /initialpose describes exactly what we want to do in the first step. If you are annoyed by the fact that the words initial and pose are not separated by a _, so am I.

We can check if this is in fact the correct topic used by rviz2. With the demo running, we run, in another terminal, the following command.

ros2 topic echo /initialpose

Then, we send the initial position through rviz2. We can see that in fact rviz2 used this topic to set the initial pose, therefore our guess was correct. Below is not representative output.

We can understand more about the topic with the following command.

ros2 topic info /initialpose

This will show us that the topic uses geometry_msgs/msg/PoseWithCovarianceStamped. Thence, all we have to do is create a subscriber with that message type to the topic /initialpose.

Type: geometry_msgs/msg/PoseWithCovarianceStamped
Publisher count: 1
Subscription count: 1

Although it might look somewhat trivial at this stage, there are many ROS2 concepts at play so that these connections can be understood.

Navigate to pose action#

When operating the demo through rviz2, you might have noticed that sending navigation goals behaves as an action. It has a goal, feedback, and a result. With that in mind, we look through the active action, once more filtering for pose. We can do so with the following command.

ros2 action list | grep pose

This results in the following actions.

/compute_path_through_poses
/compute_path_to_pose
/navigate_through_poses
/navigate_to_pose

Good sense should indicate that, given that we want to navigate to a pose, the correct topic in question is /navigate_to_pose. We can check more information about it with the following command.

ros2 action info /navigate_to_pose

The information clearly states that rviz2 is connected to it, which is a good indicator that we are on the right path.

Action: /navigate_to_pose
Action clients: 5
    /rviz
    /rviz_navigation_dialog_action_client
    /bt_navigator
    /waypoint_follower
    /docking_server
Action servers: 1
    /bt_navigator

With the following command we see what type of action we have to support.

ros2 action type /navigate_to_pose

The output of the command will be the following action type.

nav2_msgs/action/NavigateToPose

Thence, our next step is to see what are the fields that we need to support in our action client. The long way to do so is with the command we are already aware of.

ros2 interface show nav2_msgs/action/NavigateToPose

A more condensed version of the action can be obtained with the following commands.

cd $(ros2 pkg prefix nav2_msgs --share)
cat action/NavigateToPose.action

The output of this command will be the following.

#goal definition
geometry_msgs/PoseStamped pose
string behavior_tree
---
#result definition

# Error codes
# Note: The expected priority order of the errors should match the message order
uint16 NONE=0

uint16 error_code
string error_msg
---
#feedback definition
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining

So what?#

After going through this investigative process looking through topics and actions, we reached a conclusion of what must be done in a more concrete sense.

Create a publisher to send the initial pose.
- The message type is geometry_msgs/msg/PoseWithCovarianceStamped.
Create an action client to send navigation goals.
- The action type is nav2_msgs/action/NavigateToPose.

That’s it!

Create the package#

We’ll start by creating a suitable ROS2 package. We already now, from the previous investigation, what package we need to depend on.

cd ~/ros2_tutorial_workspace/src
ros2 pkg create python_package_that_uses_nav2 \
--build-type ament_python \
--dependencies rclpy geometry_msgs nav2_msgs

Files#

In this section, we will be creating or modifying the following highlighted files.

python_package_that_uses_nav2/
|-- package.xml
|-- python_package_that_uses_nav2
|   |-- __init__.py
|   |-- nav2_initial_pose_publisher_node.py
|   `-- nav2_navigate_to_pose_action_client_node.py
|-- resource
|   `-- python_package_that_uses_nav2
|-- setup.cfg
|-- setup.py
`-- test
    |-- test_copyright.py
    |-- test_flake8.py
    `-- test_pep257.py

Sending the initial pose to `nav2`#

Given that this node is a simple publisher, we can jump straight to it.

nav2_initial_pose_publisher_node.py

import time

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

class Nav2InitialPosePublisherNode(Node):
    """A ROS2 Node that publishes the initial pose for nav2."""

    def __init__(self):
        super().__init__('nav2_initial_pose_publisher_node')

        self._topic = '/initialpose'

        self.publisher = self.create_publisher(
            msg_type=PoseWithCovarianceStamped,
            topic=self._topic,
            qos_profile=1)

        publisher_count = 0
        while publisher_count < 2:
            publisher_count = self.count_publishers(self._topic)
            print(f"Waiting for publisher to be connected to {self._topic}.")
            print(f"Publisher count is {publisher_count}.")
            time.sleep(1)

    def send_initial_pose_with_covariance(self):
        """Method to create the PoseWithCovarianceStamped."""

        print(f"Publishing pose to topic: {self._topic}.")

        pwcs = PoseWithCovarianceStamped()
        pwcs.header.stamp = self.get_clock().now().to_msg()
        pwcs.header.frame_id = 'map'

        pwcs.pose.pose.position.x = -2.1
        pwcs.pose.pose.position.y = -0.3
        pwcs.pose.pose.position.z = 0.0

        pwcs.pose.pose.orientation.w = 1.0
        pwcs.pose.pose.orientation.x = 0.0
        pwcs.pose.pose.orientation.y = 0.0
        pwcs.pose.pose.orientation.z = 0.0

        pwcs.pose.covariance[0] = 0.25
        pwcs.pose.covariance[7] = 0.25
        pwcs.pose.covariance[35] = 0.06853891909122467

        self.publisher.publish(pwcs)

def main(args=None):
    """
    The main function.
    :param args: Not used directly by the user, but used by ROS2 to configure
    certain aspects of the Node.
    """
    try:
        rclpy.init(args=args)
        node = Nav2InitialPosePublisherNode()
        node.send_initial_pose_with_covariance()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(e)

if __name__ == '__main__':
    main()

There might be two minor novelties in this example. The first one is similar to what we did in tf2 when guaranteeing that the publisher was connected before attempting to publish for the first time. In this case, because in this demo rviz2 is already connected to that topic, we check that there are two subscribers connected before proceeding with the initialisation.

        publisher_count = 0
        while publisher_count < 2:
            publisher_count = self.count_publishers(self._topic)
            print(f"Waiting for publisher to be connected to {self._topic}.")
            print(f"Publisher count is {publisher_count}.")
            time.sleep(1)

The second part that might be unfamiliar is the covariance used in the message. The covariance matrix holds the variance in the diagonal and off the diagonal any pair-wise variances between different states. All of this to say that the covariance will be used to say how confident we are in a given field of the pose and if their variation is correlated. In this case, we choose the same, or similar, values that were sent by rviz2. This is enough for our purposes. The covariance is highlighted below.

    def send_initial_pose_with_covariance(self):
        """Method to create the PoseWithCovarianceStamped."""

        print(f"Publishing pose to topic: {self._topic}.")

        pwcs = PoseWithCovarianceStamped()
        pwcs.header.stamp = self.get_clock().now().to_msg()
        pwcs.header.frame_id = 'map'

        pwcs.pose.pose.position.x = -2.1
        pwcs.pose.pose.position.y = -0.3
        pwcs.pose.pose.position.z = 0.0

        pwcs.pose.pose.orientation.w = 1.0
        pwcs.pose.pose.orientation.x = 0.0
        pwcs.pose.pose.orientation.y = 0.0
        pwcs.pose.pose.orientation.z = 0.0

        pwcs.pose.covariance[0] = 0.25
        pwcs.pose.covariance[7] = 0.25
        pwcs.pose.covariance[35] = 0.06853891909122467

        self.publisher.publish(pwcs)

It is important not to be distracted by the nested pose. The first one os the PoseWithCovariance, part of the PoseWithCovarianceStamped. The second one is the Pose part of the PoseWithCovariance.

That’s it. This node is relatively simple: a publisher that publishes a single message. There will be other ways to achieve this. The one shown herein is the most standard one according to what has been shown in the tutorial.

Handling `nav2` pose navigation actions#

I’m conflicted whether this one is even easier. I suppose it’s more complex, being an action client. However, it follows the same formula as we did in the action tutorial.

nav2_navigate_to_pose_action_client_node.py

import rclpy
from rclpy.action import ActionClient
from rclpy.action.client import ClientGoalHandle
from rclpy.node import Node
from rclpy.task import Future

from geometry_msgs.msg import Pose, PoseStamped
from nav2_msgs.action import NavigateToPose

class Nav2NavigateToPoseActionClient(Node):
    """A ROS2 Node with an Action Client for Nav2NavigateToPoseActionClient."""

    def __init__(self):
        super().__init__('nav2_navigate_to_pose_action_client')

        self.action_client = ActionClient(self, NavigateToPose, '/navigate_to_pose')

        self.send_goal_future = None # This will be used in `send_goal`
        self.get_result_future = None # This will be used in 'goal_response_callback'

    def send_goal_async(self, desired_pose: Pose, behaviour_tree: str) -> None:
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.pose = desired_pose
        goal_msg.behavior_tree = behaviour_tree

        while not self.action_client.wait_for_server(timeout_sec=1.0):
            self.get_logger().info(f'action {self.action_client} not available, waiting...')

        self.get_logger().info(f'Sending goal: {goal_msg}.')

        self.send_goal_future = self.action_client.send_goal_async(goal_msg, feedback_callback=self.action_feedback_callback)
        self.send_goal_future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future: Future) -> None:
        goal: ClientGoalHandle = future.result()

        if not goal.accepted:
            self.get_logger().info('Goal was rejected by the server.')
            return
        self.get_logger().info('Goal was accepted by the server.')

        self.get_result_future = goal.get_result_async()
        self.get_result_future.add_done_callback(self.action_result_callback)

    def action_result_callback(self, future: Future) -> None:
        result: NavigateToPose.Result = future.result()
        self.get_logger().info(f'Final position was: {result.result}.')

    def action_feedback_callback(self, feedback_msg: NavigateToPose.Feedback) -> None:
        feedback = feedback_msg.feedback
        self.get_logger().info(f'Received feedback distance: {feedback.distance_remaining}.')


def main(args=None):
    """
    The main function.
    :param args: Not used directly by the user, but used by ROS2 to configure certain aspects of the Node.
    """
    try:
        rclpy.init(args=args)

        node = Nav2NavigateToPoseActionClient()

        desired_pose = Pose()
        desired_pose.position.x = 1.0
        desired_pose.position.y = -1.0
        desired_pose.orientation.w = 1.0

        node.send_goal_async(desired_pose, "")

        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(e)


if __name__ == '__main__':
    main()

We can highlight two possible pinch points. Starting by the main() function, we instantiate the node and create a desired pose, highlighted below. Indeed, we are going to use a stamped pose. However, it is simpler to grab the stamp from a node, so we do that in a method. We send an empty behaviour tree, because otherwise would be out of the scope of this tutorial.

        node = Nav2NavigateToPoseActionClient()

        desired_pose = Pose()
        desired_pose.position.x = 1.0
        desired_pose.position.y = -1.0
        desired_pose.orientation.w = 1.0

        node.send_goal_async(desired_pose, "")

Then, in our implementation we populate the fields of the goal with the pose we just created and the stamp obtained from the node. We receive an empty behaviour tree and that is also added to the goal.

    def send_goal_async(self, desired_pose: Pose, behaviour_tree: str) -> None:
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.pose = desired_pose
        goal_msg.behavior_tree = behaviour_tree

Besides the slightly different action type, the process to make an action client is mostly unchanged.

Adjusting the `setup.py`#

setup.py

from setuptools import find_packages, setup

package_name = 'python_package_that_uses_nav2'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='murilo.marinho@manchester.ac.uk',
    description='TODO: Package description',
    license='TODO: License declaration',
    extras_require={
        'test': [
            'pytest',
        ],
    },
    entry_points={
        'console_scripts': [
            'nav2_initial_pose_publisher_node = python_package_that_uses_nav2.nav2_initial_pose_publisher_node:main',
            'nav2_navigate_to_pose_action_client_node = python_package_that_uses_nav2.nav2_navigate_to_pose_action_client_node:main'
        ],
    },
)

Build and source#

Before we proceed, let us build and source once.

cd ~/ros2_tutorial_workspace
colcon build
source install/setup.bash

Note

For additional explanation and troubleshooting tips, see Always source after you build.

Warning

colcon will not work properly if your terminal has an active venv.

Testing#

The first step is to run the same demo as before, with tb3_simulation_launch.py. We can do that with the command below.

ros2 launch nav2_bringup \
tb3_simulation_launch.py \
use_sim_time:=True \
headless:=False \
sigterm_timeout:=120

Then, in another terminal, we can run the following command. This one will be rather tolerant of what it’s started because it will wait for the correct number of publishers before publishing.

ros2 run python_package_that_uses_nav2 nav2_initial_pose_publisher_node

The following command should only be attempted after the initial pose was set. The navigation stack will not accept goals unless an initial estimate is given first.

ros2 run python_package_that_uses_nav2 nav2_navigate_to_pose_action_client_node

The qualitative behaviour can be seen in the following video.

Exercises

Other possible interactions with these interfaces could be as follows.

What would change if the same node was supposed to both send the initial pose and, after that is done, send the goal?
What would have to be modified if you had three goals, goal a, goal b, and goal c. Suppose that we initially send goal a. If the goal is reached, then go to goal b. If not reached, go to goal c. This is a very basic state machine. An embryo, but somewhat the motivation for behaviour tress.