TurtleBot 4 Navigator

Note

TurtleBot 4 Navigator requires at least version 1.0.11 of Nav2 Simple Commander

Install Tutorial Package

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.

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 Gazebo 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
    
  • To run this example, start the Ignition Gazebo simulation:

    ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py nav2:=true slam:=false localization:=true rviz:=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.

    Let's take a look at the main function.

    def main():
        rclpy.init()
    
        navigator = TurtleBot4Navigator()
    
        # Start on dock
        if not navigator.getDockedStatus():
            navigator.info('Docking before initialising 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()
    
  • The source code for this example is available here.

    Let's take a look at the main function.

    def main():
        rclpy.init()
    
        navigator = TurtleBot4Navigator()
    
        # Start on dock
        if not navigator.getDockedStatus():
            navigator.info('Docking before initialising 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, 9.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 initialising 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
Note

These cardinal directions are relative to the map, not the actual magnetic north pole. Driving north is equivalent to driving upwards on the map, west is driving left, and so on.

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

This call will block until Nav2 is ready. Make sure you have launched Nav2.

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)
    
  • 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, 9.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. If Rviz wasn't already launched, launch it by calling:

ros2 launch turtlebot4_viz view_robot.launch.py
  • Nav to pose
    Navigate to a pose
  • Navigate to a pose
  • 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 Gazebo 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
    
  • 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 warehouse world of the TurtleBot 4 simulation.

    To run this example, start the Ignition Gazebo simulation:

    ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py nav2:=true slam:=false localization:=true rviz:=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.

    Let's take a look at the main function.

    def main():
        rclpy.init()
    
        navigator = TurtleBot4Navigator()
    
        # Start on dock
        if not navigator.getDockedStatus():
            navigator.info('Docking before initialising 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()
    
  • The source code for this example is available here.

    Let's take a look at the main function.

    def main():
        rclpy.init()
    
        navigator = TurtleBot4Navigator()
    
        # Start on dock
        if not navigator.getDockedStatus():
            navigator.info('Docking before initialising 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.0, -0.0], TurtleBot4Directions.EAST))
        goal_pose.append(navigator.getPoseStamped([-3.0, -3.0], TurtleBot4Directions.NORTH))
        goal_pose.append(navigator.getPoseStamped([3.0, -3.0], TurtleBot4Directions.NORTH_WEST))
        goal_pose.append(navigator.getPoseStamped([9.0, -1.0], TurtleBot4Directions.WEST))
        goal_pose.append(navigator.getPoseStamped([9.0, 1.0], TurtleBot4Directions.SOUTH))
        goal_pose.append(navigator.getPoseStamped([-1.0, 1.0], TurtleBot4Directions.EAST))
    
        # 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 initialise 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))
    
  • goal_pose = []
    goal_pose.append(navigator.getPoseStamped([-3.0, -0.0], TurtleBot4Directions.EAST))
    goal_pose.append(navigator.getPoseStamped([-3.0, -3.0], TurtleBot4Directions.NORTH))
    goal_pose.append(navigator.getPoseStamped([3.0, -3.0], TurtleBot4Directions.NORTH_WEST))
    goal_pose.append(navigator.getPoseStamped([9.0, -1.0], TurtleBot4Directions.WEST))
    goal_pose.append(navigator.getPoseStamped([9.0, 1.0], TurtleBot4Directions.SOUTH))
    goal_pose.append(navigator.getPoseStamped([-1.0, 1.0], TurtleBot4Directions.EAST))
    

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. If Rviz wasn't already launched, launch it by calling:

ros2 launch turtlebot4_viz view_robot.launch.py
  • Nav through pose
    Navigate through a set of poses
  • Navigate through a set of poses

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 Gazebo 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
    
  • 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 Gazebo simulation:

    ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py nav2:=true slam:=false localization:=true rviz:=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.

  • The source code for this example is available here.

Let's take a look at the main function.

def main():
    rclpy.init()

    navigator = TurtleBot4Navigator()

    # Start on dock
    if not navigator.getDockedStatus():
        navigator.info('Docking before initialising 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. If Rviz wasn't already launched, launch it by calling:

ros2 launch turtlebot4_viz view_robot.launch.py
  • Follow Waypoints
    Follow a set of Waypoints
  • Follow a set of Waypoints

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.

  • The source code for this example is available here.

Let's 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 initialising 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 initialising 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:

  • Create Path
    Creating a path and following it
  • Creating a path and following it
Note

As the path is created, you will see the robot being placed at the position you click on. This is normal and gets cleared up when the initial pose is set by the TurtleBot 4 Navigator.

Mail Delivery

  • This tutorial is only available in Humble

  • This example demonstrates how to create an interactive delivery route. The user is able to send the robot to different pre-defined locations on demand using the terminal interface. It then uses the Navigate to Pose behaviour to navigate to the pose.

    To run this example, start the Ignition simulation:

    ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py nav2:=true slam:=false localization:=true rviz:=true
    

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

    ros2 run turtlebot4_python_tutorials mail_delivery
    

Code breakdown

  • This tutorial is only available in Humble

  • The source code for this example is available here.

    Let's take a look at the main function.

    def main(args=None):
        rclpy.init(args=args)
    
        navigator = TurtleBot4Navigator()
    
        # Start on dock
        if not navigator.getDockedStatus():
            navigator.info('Docking before initialising 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()
    
        # Undock
        navigator.undock()
    
        # Prepare goal pose options
        goal_options = [
            {'name': 'Home',
             'pose': navigator.getPoseStamped([-1.0, 1.0], TurtleBot4Directions.EAST)},
    
            {'name': 'Position 1',
             'pose': navigator.getPoseStamped([10.0, 6.0], TurtleBot4Directions.EAST)},
    
            {'name': 'Position 2',
             'pose': navigator.getPoseStamped([-9.0, 9.0], TurtleBot4Directions.NORTH)},
    
            {'name': 'Position 3',
             'pose': navigator.getPoseStamped([-12.0, 2.0], TurtleBot4Directions.NORTH_WEST)},
    
            {'name': 'Position 4',
             'pose': navigator.getPoseStamped([3.0, -7.0], TurtleBot4Directions.WEST)},
    
            {'name': 'Exit',
             'pose': None}
        ]
    
        navigator.info('Welcome to the mail delivery service.')
    
        while True:
            # Create a list of the goals for display
            options_str = 'Please enter the number corresponding to the desired robot goal position:\n'
            for i in range(len(goal_options)):
                options_str += f'    {i}. {goal_options[i]["name"]}\n'
    
            # Prompt the user for the goal location
            raw_input = input(f'{options_str}Selection: ')
    
            selected_index = 0
    
            # Verify that the value input is a number
            try:
                selected_index = int(raw_input)
            except ValueError:
                navigator.error(f'Invalid goal selection: {raw_input}')
                continue
    
            # Verify that the user input is within a valid range
            if (selected_index < 0) or (selected_index >= len(goal_options)):
                navigator.error(f'Goal selection out of bounds: {selected_index}')
    
            # Check for exit
            elif goal_options[selected_index]['name'] == 'Exit':
                break
    
            else:
                # Navigate to requested position
                navigator.startToPose(goal_options[selected_index]['pose'])
    
        rclpy.shutdown()
    

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

    Prepare Goal Poses

    The next step is to create a list of PoseStamped messages which represent all possible poses that the robot can be sent to.

    goal_options = [
        {'name': 'Home',
            'pose': navigator.getPoseStamped([-1.0, 1.0], TurtleBot4Directions.EAST)},
    
        {'name': 'Position 1',
            'pose': navigator.getPoseStamped([10.0, 6.0], TurtleBot4Directions.EAST)},
    
        {'name': 'Position 2',
            'pose': navigator.getPoseStamped([-9.0, 9.0], TurtleBot4Directions.NORTH)},
    
        {'name': 'Position 3',
            'pose': navigator.getPoseStamped([-12.0, 2.0], TurtleBot4Directions.NORTH_WEST)},
    
        {'name': 'Position 4',
            'pose': navigator.getPoseStamped([3.0, -7.0], TurtleBot4Directions.WEST)},
    
        {'name': 'Exit',
            'pose': None}
    ]
    

    Select the Goal Pose

    The remainder of the program is repeated in a loop.

    A list of goals is compiled and displayed on the terminal as a prompt and the program will wait for input from the user.

    # Create a list of the goals for display
    options_str = 'Please enter the number corresponding to the desired robot goal position:\n'
    for i in range(len(goal_options)):
        options_str += f'    {i}. {goal_options[i]["name"]}\n'
    
    # Prompt the user for the goal location
    raw_input = input(f'{options_str}Selection: ')
    

    The terminal will display:

    Mail Delivery Terminal
    Mail Delivery Terminal Prompt

    The user will then enter a number and press enter.

    User Input Validation

    The user input is verified to be an integer which is within the allowable range. If it is not a number or if the number does not correspond to a valid option then the loop will restart, prompting the user for a new input.

    selected_index = 0
    
    # Verify that the value input is a number
    try:
        selected_index = int(raw_input)
    except ValueError:
        navigator.error(f'Invalid goal selection: {raw_input}')
        continue
    
    # Verify that the user input is within a valid range
    if (selected_index < 0) or (selected_index >= len(goal_options)):
        navigator.error(f'Goal selection out of bounds: {selected_index}')
    

    Check for exit

    If the user selected to exit the program then the loop is terminated and the program will exit.

    # Check for exit
    elif goal_options[selected_index]['name'] == 'Exit':
        break
    

    Watch Navigation Progress in RViz

    Mail Delivery (3x speed)

    Finally, navigate to the selected position.

    else:
        # Navigate to requested position
        navigator.startToPose(goal_options[selected_index]['pose'])
    

    The loop then repeats, prompting the user for the next goal position.

Patrol Loop

  • This tutorial is only available in Humble

  • This example demonstrates how to create an infinite patrol loop with auto-charging. The robot will continuously drive through a set of poses until the charge is low and then it will go charge. Once the robot is sufficiently charged, it will start driving through the set of poses again. It uses the Navigate to Pose behaviour to navigate to the pose.

    To run this example, start the Ignition simulation:

    ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py nav2:=true slam:=false localization:=true rviz:=true
    

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

    ros2 run turtlebot4_python_tutorials patrol_loop
    

Code breakdown

  • This tutorial is only available in Humble

  • The source code for this example is available here.

    Battery Monitoring Node

    Let's take a look at the battery monitoring node class.

    class BatteryMonitor(Node):
    
        def __init__(self, lock):
            super().__init__('battery_monitor')
    
            self.lock = lock
    
            # Subscribe to the /battery_state topic
            self.battery_state_subscriber = self.create_subscription(
                BatteryState,
                'battery_state',
                self.battery_state_callback,
                qos_profile_sensor_data)
    
        # Callbacks
        def battery_state_callback(self, batt_msg: BatteryState):
            with self.lock:
                self.battery_percent = batt_msg.percentage
    
        def thread_function(self):
            executor = SingleThreadedExecutor()
            executor.add_node(self)
            executor.spin()
    

    This class serves to monitor the Create3® battery charge percentage and to make it available for decision making.

    Subscription

    The class contains a subscriber that listens on the battery_state topic for the latest battery state published by the Create3® and calls the battery_state_callback function whenever new information has been published.

    # Subscribe to the /battery_state topic
    self.battery_state_subscriber = self.create_subscription(
        BatteryState,
        'battery_state',
        self.battery_state_callback,
        qos_profile_sensor_data)
    
    Battery State Callback

    Whenever a battery_state message is received, the battery charge percentage is saved in a member variable.

    A Global Interpreter Lock (GIL) is used to ensure that the main function and battery monitoring functions do not access or modify that member variable at the same time. The process will queue until the lock is available.

    # Callbacks
    def battery_state_callback(self, batt_msg: BatteryState):
        with self.lock:
            self.battery_percent = batt_msg.percentage
    
    Prepare Threading

    In order to listen for new messages and run callbacks, a node must be actively spinning. To simultaneously spin the node and run the navigation code, multi-threading is used. Here the battery node is prepared to spin in a separate thread which is created in the main function (see below).

    def thread_function(self):
        executor = SingleThreadedExecutor()
        executor.add_node(self)
        executor.spin()
    
    

    Main Function

    Let's take a look at the main function.

    def main(args=None):
        rclpy.init(args=args)
    
        lock = Lock()
        battery_monitor = BatteryMonitor(lock)
    
        navigator = TurtleBot4Navigator()
        battery_percent = None
        position_index = 0
    
        thread = Thread(target=battery_monitor.thread_function, daemon=True)
        thread.start()
    
        # Start on dock
        if not navigator.getDockedStatus():
            navigator.info('Docking before initialising 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()
    
        # Undock
        navigator.undock()
    
        # Prepare goal poses
        goal_pose = []
        goal_pose.append(navigator.getPoseStamped([-5.0, 1.0], TurtleBot4Directions.EAST))
        goal_pose.append(navigator.getPoseStamped([-5.0, -23.0], TurtleBot4Directions.NORTH))
        goal_pose.append(navigator.getPoseStamped([9.0, -23.0], TurtleBot4Directions.NORTH_WEST))
        goal_pose.append(navigator.getPoseStamped([10.0, 2.0], TurtleBot4Directions.WEST))
    
        while True:
            with lock:
                battery_percent = battery_monitor.battery_percent
    
            if (battery_percent is not None):
                navigator.info(f'Battery is at {(battery_percent*100):.2f}% charge')
    
                # Check battery charge level
                if (battery_percent < BATTERY_CRITICAL):
                    navigator.error('Battery critically low. Charge or power down')
                    break
                elif (battery_percent < BATTERY_LOW):
                    # Go near the dock
                    navigator.info('Docking for charge')
                    navigator.startToPose(navigator.getPoseStamped([-1.0, 1.0],
                                          TurtleBot4Directions.EAST))
                    navigator.dock()
    
                    if not navigator.getDockedStatus():
                        navigator.error('Robot failed to dock')
                        break
    
                    # Wait until charged
                    navigator.info('Charging...')
                    battery_percent_prev = 0
                    while (battery_percent < BATTERY_HIGH):
                        sleep(15)
                        battery_percent_prev = floor(battery_percent*100)/100
                        with lock:
                            battery_percent = battery_monitor.battery_percent
    
                        # Print charge level every time it increases a percent
                        if battery_percent > (battery_percent_prev + 0.01):
                            navigator.info(f'Battery is at {(battery_percent*100):.2f}% charge')
    
                    # Undock
                    navigator.undock()
                    position_index = 0
    
                else:
                    # Navigate to next position
                    navigator.startToPose(goal_pose[position_index])
    
                    position_index = position_index + 1
                    if position_index >= len(goal_pose):
                        position_index = 0
    
        battery_monitor.destroy_node()
        rclpy.shutdown()
    

    This example starts with many of the same steps as navigate to pose. We initialise the node, make sure the robot is docked, and set the initial pose. Then we wait for Nav2 to become active. The additional steps are discussed below.

    Multi-threading

    A Global Interpreter Lock (GIL) is used to ensure that the main function and battery monitoring functions do not access or modify that member variable at the same time.

    The lock is created as well as the battery monitor node being created. The lock is passed into the battery monitor node to ensure that both processes are using the same lock.

    lock = Lock()
    battery_monitor = BatteryMonitor(lock)
    

    A thread is created to run the battery monitor thread function that was created earlier. This thread is started.

    thread = Thread(target=battery_monitor.thread_function, daemon=True)
    thread.start()
    
    Prepare Goal Pose Loop

    The robot poses that make up the robot's patrol loop are assembled into a list.

    # Prepare goal poses
    goal_pose = []
    goal_pose.append(navigator.getPoseStamped([-5.0, 1.0], TurtleBot4Directions.EAST))
    goal_pose.append(navigator.getPoseStamped([-5.0, -23.0], TurtleBot4Directions.NORTH))
    goal_pose.append(navigator.getPoseStamped([9.0, -23.0], TurtleBot4Directions.NORTH_WEST))
    goal_pose.append(navigator.getPoseStamped([10.0, 2.0], TurtleBot4Directions.WEST))
    
    Operating Loop

    The remainder of the code continues until it is interrupted.

    The battery percentage is updated when it is granted access by the lock.

    with lock:
        battery_percent = battery_monitor.battery_percent
    

    Initially when the processes are not ready, the battery percent may be None. The program continues to check until the battery percent is not None.

    if (battery_percent is not None):
    
    

    Based on the battery charge level, the program chooses an action:

    If the battery is at critical levels, it breaks the loop, ending the program.

    navigator.info(f'Battery is at {(battery_percent*100):.2f}% charge')
    
    # Check battery charge level
    if (battery_percent < BATTERY_CRITICAL):
        navigator.error('Battery critically low. Charge or power down')
        break
    

    If the battery is low then it navigates near the charger, docks and then waits for full charge. While charging it continues to periodically use the lock to update the battery percentage. Once full, it undocks and prepares to start the patrol loop again from the first position.

    elif (battery_percent < BATTERY_LOW):
        # Go near the dock
        navigator.info('Docking for charge')
        navigator.startToPose(navigator.getPoseStamped([-1.0, 1.0],
                                TurtleBot4Directions.EAST))
        navigator.dock()
    
        if not navigator.getDockedStatus():
            navigator.error('Robot failed to dock')
            break
    
        # Wait until charged
        navigator.info('Charging...')
        battery_percent_prev = 0
        while (battery_percent < BATTERY_HIGH):
            sleep(15)
            battery_percent_prev = floor(battery_percent*100)/100
            with lock:
                battery_percent = battery_monitor.battery_percent
    
            # Print charge level every time it increases a percent
            if battery_percent > (battery_percent_prev + 0.01):
                navigator.info(f'Battery is at {(battery_percent*100):.2f}% charge')
    
        # Undock
        navigator.undock()
        position_index = 0
    

    Finally, if the battery was not critical or low then the robot navigates to the next position in the loop. This position is tracked by the ‘position_index' variable

    else:
        # Navigate to next position
        navigator.startToPose(goal_pose[position_index])
    
        position_index = position_index + 1
        if position_index >= len(goal_pose):
            position_index = 0
    

    Watch Navigation Progress in RViz

    Patrol Loop (9x speed)