Navigation

This tutorial will cover various methods of navigating with the TurtleBot 4 and Nav2.

SLAM vs Localization

There are two localization methods we can use to figure out where the robot is on the map: SLAM or Localization. SLAM allows us to generate the map as we navigate, while localization requires that a map already exists.

SLAM

SLAM is useful for generating a new map, or navigating in unknown or dynamic environments. It updates the map as it detects and changes, but cannot see areas of the environment that it has not discovered yet.

Localization

Localization uses an existing map along with live odometry and laserscan data to figure out the position of the robot on the given map. It does not update the map if any changes have been made to the environment, but we can still avoid new obstacles when navigating. Because the map doesn't change, we can get more repeatable navigation results.

For this tutorial, we will be using localization to navigate on a map generated with SLAM.

Nav2

The TurtleBot 4 uses the Nav2 stack for navigation.

Launching navigation

• galactic
• humble

• For this tutorial we can launch navigation with Nav Bringup.

  On a physical TurtleBot 4, call:

  ros2 launch turtlebot4_navigation nav_bringup.launch.py slam:=off localization:=true map:=office.yaml
  

  Replace office.yaml with your own map.

  If you are using the simulator, call:

  ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav2:=true slam:=off localization:=true
  

  This will launch the simulation in the default depot world and will use the existing depot.yaml file for the map. If you are using a different world you will need to create a map for it and pass that in as a launch argument.

  For example:

  ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav2:=true slam:=off localization:=true world:=classroom map:=classroom.yaml
  

• For this tutorial we can launch navigation with the turtlebot4_navigation package.

  Open a terminal and launch localization:

  ros2 launch turtlebot4_navigation localization.launch.py map:=office.yaml
  

  Then, in another terminal, launch nav2:

  ros2 launch turtlebot4_navigation nav2.launch.py
  

Interacting with Nav2

In a new terminal, launch Rviz so that you can view the map and interact with navigation:

ros2 launch turtlebot4_viz view_robot.launch.py

At the top of the Rviz window is the toolbar. You will notice that there are three navigation tools available to you.

2D Pose Estimate

The 2D Pose Estimate tool is used in localization to set the approximate initial pose of the robot on the map. This is required for the Nav2 stack to know where to start localizing from. Click on the tool, and then click and drag the arrow on the map to approximate the position and orientation of the robot.

Publish Point

The Publish Point tool allows you to click on a point on the map, and have the coordinates of that point published to the /clicked_point topic.

Open a new terminal and call:

ros2 topic echo /clicked_point

Then, select the Publish Point tool and click on a point on the map. You should see the coordinates published in your terminal.

Nav2 Goal

The Nav2 Goal tool allows you to set a goal pose for the robot. The Nav2 stack will then plan a path to the goal pose and attempt to drive the robot there. Make sure to set the initial pose of the robot before you set a goal pose.

TurtleBot 4 Navigator

The TurtleBot 4 Navigator is a Python node that adds on to the Nav2 Simple Commander. It includes TurtleBot 4 specific features such as docking and undocking, as well as easy to use methods for navigating.

The following examples can be installed with sudo apt install ros-$ROS_DISTRO-turtlebot4-tutorials and are available at https://github.com/turtlebot/turtlebot4_tutorials. For each example, the robot starts on a dock at the origin of the map.

Navigate to Pose

This example demonstrates the same behaviour as Nav2 Goal. The Nav2 stack is given a pose on the map with which it calculates a path. The robot then attempts to drive along the path. This example is demonstrated in the depot world of the TurtleBot 4 simulation.

To run this example, start the Ignition simulation:

ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav:=true slam:=off localization:=true

Once the simulation has started, open another terminal and run:

ros2 run turtlebot4_python_tutorials nav_to_pose

Code breakdown

The source code for this example is available here.

Lets take a look at the main function.

def main():
    rclpy.init()

    navigator = TurtleBot4Navigator()

    # Start on dock
    if not navigator.getDockedStatus():
        navigator.info('Docking before intialising pose')
        navigator.dock()

    # Set initial pose
    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
    navigator.setInitialPose(initial_pose)

    # Wait for Nav2
    navigator.waitUntilNav2Active()

    # Set goal poses
    goal_pose = navigator.getPoseStamped([13.0, 5.0], TurtleBot4Directions.EAST)

    # Undock
    navigator.undock()

    # Go to each goal pose
    navigator.startToPose(goal_pose)

    rclpy.shutdown()

Initialise the node

We start by initialising rclpy and creating the TurtleBot4Navigator object. This will initialise any ROS 2 publishers, subscribers and action clients that we need.

rclpy.init()

navigator = TurtleBot4Navigator()

Dock the robot

Next, we check if the robot is docked. If it is not, we send an action goal to dock the robot. By docking the robot we guarantee that it is at the [0.0, 0.0] coordinates on the map.

if not navigator.getDockedStatus():
    navigator.info('Docking before intialising pose')
    navigator.dock()

Set the initial pose

Now that we know the robot is docked, we can set the initial pose to [0.0, 0.0], facing "North".

initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
navigator.setInitialPose(initial_pose)

The TurtleBot 4 Navigator uses cardinal directions to set the orientation of the robot relative to the map. You can use actual integers or floating points if you need a more precise direction.

class TurtleBot4Directions(IntEnum):
    NORTH = 0
    NORTH_WEST = 45
    WEST = 90
    SOUTH_WEST = 135
    SOUTH = 180
    SOUTH_EAST = 225
    EAST = 270
    NORTH_EAST = 315

Wait for Nav2

Once the initial position has been set, the Nav2 stack will place the robot at that position on the map and begin localizing. We want to wait for Nav2 to be ready before we start sending navigation goals.

navigator.waitUntilNav2Active()

Set the goal pose

Now we can create a geometry_msgs/PoseStamped message. The getPoseStamped method makes it easy for us. All we have to do is pass in a list describing the x and y position that we want to drive to on the map, and the direction that we want the robot to be facing when it reaches that point.

goal_pose = navigator.getPoseStamped([13.0, 5.0], TurtleBot4Directions.EAST)

Undock the robot and go to the goal pose

We are ready to drive to the goal pose. We start by undocking the robot so that it does not attempt to drive through the dock, and then send the goal pose. As the robot drives to the goal pose, we will be receiving feedback from the action. This feedback includes the estimated time of arrival.

navigator.undock()

navigator.startToPose(goal_pose)

Once the robot has reached the goal, we call rclpy.shutdown() to gracefully destroy the rclpy context.

Watch navigation progress in Rviz

You can visualise the navigation process in Rviz by calling:

ros2 launch turtlebot4_viz view_robot.launch.py

Navigate Through Poses

This example demonstrates the Navigate Through Poses behaviour tree. The Nav2 stack is given a set of poses on the map and creates a path that goes through each pose in order until the last pose is reached. The robot then attempts to drive along the path. This example is demonstrated in the depot world of the TurtleBot 4 simulation.

To run this example, start the Ignition simulation:

ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav:=true slam:=off localization:=true

Once the simulation has started, open another terminal and run:

ros2 run turtlebot4_python_tutorials nav_through_poses

Code breakdown

The source code for this example is available here.

Lets take a look at the main function.

def main():
    rclpy.init()

    navigator = TurtleBot4Navigator()

    # Start on dock
    if not navigator.getDockedStatus():
        navigator.info('Docking before intialising pose')
        navigator.dock()

    # Set initial pose
    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
    navigator.setInitialPose(initial_pose)

    # Wait for Nav2
    navigator.waitUntilNav2Active()

    # Set goal poses
    goal_pose = []
    goal_pose.append(navigator.getPoseStamped([0.0, -1.0], TurtleBot4Directions.NORTH))
    goal_pose.append(navigator.getPoseStamped([1.7, -1.0], TurtleBot4Directions.EAST))
    goal_pose.append(navigator.getPoseStamped([1.6, -3.5], TurtleBot4Directions.NORTH))
    goal_pose.append(navigator.getPoseStamped([6.75, -3.46], TurtleBot4Directions.NORTH_WEST))
    goal_pose.append(navigator.getPoseStamped([7.4, -1.0], TurtleBot4Directions.SOUTH))
    goal_pose.append(navigator.getPoseStamped([-1.0, -1.0], TurtleBot4Directions.WEST))

    # Undock
    navigator.undock()

    # Navigate through poses
    navigator.startThroughPoses(goal_pose)

    # Finished navigating, dock
    navigator.dock()

    rclpy.shutdown()

This example starts the same as navigate to pose. We initialse the node, make sure the robot is docked, and set the initial pose. Then we wait for Nav2 to become active.

Set goal poses

The next step is to create a list of PoseStamped messages which represent the poses that the robot needs to drive through.

goal_pose = []
goal_pose.append(navigator.getPoseStamped([0.0, -1.0], TurtleBot4Directions.NORTH))
goal_pose.append(navigator.getPoseStamped([1.7, -1.0], TurtleBot4Directions.EAST))
goal_pose.append(navigator.getPoseStamped([1.6, -3.5], TurtleBot4Directions.NORTH))
goal_pose.append(navigator.getPoseStamped([6.75, -3.46], TurtleBot4Directions.NORTH_WEST))
goal_pose.append(navigator.getPoseStamped([7.4, -1.0], TurtleBot4Directions.SOUTH))
goal_pose.append(navigator.getPoseStamped([-1.0, -1.0], TurtleBot4Directions.WEST))

Navigate through the poses

Now we can undock the robot and begin navigating through each point. Once the robot has reached the final pose, it will then return to the dock.

navigator.undock()

navigator.startThroughPoses(goal_pose)

navigator.dock()

Watch navigation progress in Rviz

You can visualise the navigation process in Rviz by calling:

ros2 launch turtlebot4_viz view_robot.launch.py

Follow Waypoints

This example demonstrates how to follow waypoints. The Nav2 stack is given a set of waypoints on the map and creates a path that goes through each waypoint in order until the last waypoint is reached. The robot then attempts to drive along the path. The difference between this example and Navigating Through Poses is that when following waypoints the robot will plan to reach each waypoint individually, rather than planning to reach the last pose by driving through the other poses. This example is demonstrated in the depot world of the TurtleBot 4 simulation.

To run this example, start the Ignition simulation:

ros2 launch turtlebot4_ignition_bringup ignition.launch.py nav:=true slam:=off localization:=true

Once the simulation has started, open another terminal and run:

ros2 run turtlebot4_python_tutorials follow_waypoints

Code breakdown

The source code for this example is available here.

Lets take a look at the main function.

def main():
    rclpy.init()

    navigator = TurtleBot4Navigator()

    # Start on dock
    if not navigator.getDockedStatus():
        navigator.info('Docking before intialising pose')
        navigator.dock()

    # Set initial pose
    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
    navigator.setInitialPose(initial_pose)

    # Wait for Nav2
    navigator.waitUntilNav2Active()

    # Set goal poses
    goal_pose = []
    goal_pose.append(navigator.getPoseStamped([-3.3, 5.9], TurtleBot4Directions.NORTH))
    goal_pose.append(navigator.getPoseStamped([2.1, 6.3], TurtleBot4Directions.EAST))
    goal_pose.append(navigator.getPoseStamped([2.0, 1.0], TurtleBot4Directions.SOUTH))
    goal_pose.append(navigator.getPoseStamped([-1.0, 0.0], TurtleBot4Directions.NORTH))

    # Undock
    navigator.undock()

    # Follow Waypoints
    navigator.startFollowWaypoints(goal_pose)

    # Finished navigating, dock
    navigator.dock()

    rclpy.shutdown()

This example is very similar to Navigate Through Poses. The difference is that we are using different poses as our waypoints, and that we use the startFollowWaypoints method to perform our navigation behaviour.

Watch navigation progress in Rviz

You can visualise the navigation process in Rviz by calling:

ros2 launch turtlebot4_viz view_robot.launch.py

Create Path

This example demonstrates how to create a navigation path in Rviz during runtime. It uses the 2D Pose Estimate tool to pass the TurtleBot 4 Navigator a set of poses. Then we use the Follow Waypoints behaviour to follow those poses. This example was run on a physical TurtleBot 4.

To run this example, start Navigation on your PC or on the Raspberry Pi using a map of your environment.

Once the navigation has started, open another terminal and run:

ros2 run turtlebot4_python_tutorials create_path

On your PC you will need to start Rviz:

ros2 launch turtlebot4_viz view_robot.launch.py

Code breakdown

The source code for this example is available here.

Lets take a look at the main function.

def main():
    rclpy.init()

    navigator = TurtleBot4Navigator()

    # Set goal poses
    goal_pose = navigator.createPath()

    if len(goal_pose) == 0:
        navigator.error('No poses were given, exiting.')
        exit(0)

    # Start on dock
    if not navigator.getDockedStatus():
        navigator.info('Docking before intialising pose')
        navigator.dock()

    # Set initial pose
    initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
    navigator.clearAllCostmaps()
    navigator.setInitialPose(initial_pose)

    # Wait for Nav2
    navigator.waitUntilNav2Active()

    # Undock
    navigator.undock()

    # Navigate through poses
    navigator.startFollowWaypoints(goal_pose)

    # Finished navigating, dock
    navigator.dock()

    rclpy.shutdown()

This example begins the same as the others by initialising the TurtleBot 4 Navigator.

Create your path

After initialisation, the user is prompted to create their path by using the 2D Pose Estimate tool. You must set at least one pose. Once all of the poses have been set, the robot will begin navigating.

goal_pose = navigator.createPath()

if len(goal_pose) == 0:
    navigator.error('No poses were given, exiting.')
    exit(0)

Set initial pose and clear costmaps

Next we set the initial pose and clear all costmaps. We clear costmaps because the 2D Pose Estimate tool is subscribed to by the Nav2 stack, and every time we use it Nav2 assumes that the robot is in that position, when it is not. Clearing the costmaps will get rid of any false costmaps that may have spawned when creating the path.

if not navigator.getDockedStatus():
    navigator.info('Docking before intialising pose')
    navigator.dock()

initial_pose = navigator.getPoseStamped([0.0, 0.0], TurtleBot4Directions.NORTH)
navigator.clearAllCostmaps()
navigator.setInitialPose(initial_pose)

navigator.waitUntilNav2Active()

We also wait for Nav2 to be active before continuing.

Follow the path

Now we can undock and follow the created path. In this example we use the Follow Waypoints behaviour, but this can easily be replaced with Navigate Through Poses.

navigator.undock()

navigator.startFollowWaypoints(goal_pose)

navigator.dock()

We finish the example by docking the robot. This assumes that the last pose in the created path is near the dock. If it is not, you can remove this action.

Creating a path with Rviz

Running this example will look something like this: