Autonomous Navigation Introduction

Driving a robot around is fun to begin with, but if you had to drive your robot vacuum cleaner around then the novelty would quickly would wear off. Autonomous navigation is the answer. An agent (a human or an algorithm) supplies a goal position, and we expect the robot to do the rest. When a human supplies goal positions (and makes the decision where these are) this is normally called semi-autonomous navigation.

In previous weeks we have gradually built up a simulation of a robot which can generate a map (or use an existing map) and localise itself in an environment. These were key stepping stones to autonomous navigation.

For autonomous navigation we need the following capabilities of a mobile robot:

• Locomotion given an input velocity (Kinematics, Actuation, and Control)

• Sensing to avoid obstacles and produce a map (e.g. lidar)

• Where is the robot (Localisation/SLAM and Transformations)

• Start and Goal positions (in the same global frame)

• Draw a path from the start to the goal (Path Planning)

• Drive the robot whilst attempting to Keep the robot on that path (Control and Navigation)

We can sense obstacles (with a simulated lidar), generate a map of the environment to provide a global frame and a reference of obstacles, move a robot based on a desired command velocity, know where the robot is in the global frame. All that is left is Path Planning and Navigation.

Everything a mobile robot will need is covered by the existing ROS Nav2 Navigation Stack.

Behaviour Trees and the Navigation Stack

The Nav2 Navigation stack relies on behaviour trees. If you are familiar with state machines, behaviour trees are a somewhat similar concept. These will be covered in more detail in Week 11, so these week you can treat it like a black box.

As a very high-level overview, you can consider the behaviour tree as a solution for handling the transitions between different “modes” in navigation. For example, these modes may include what happens if the robot gets stuck (recovery behaviours).

Build a Package for This Tutorial

We will begin making use of an UPDATED VERSION of the gz_example_description_package. Please replace any existing versions you have of this package. You can download a copy here: gz_example_robot_description.zip

We will utilise a new ROS2 package called navigation_demos for these activities, which will contain two additional directories launch and config.

Make a package the usual way:

cd ~/MY_ROS_WS/src/
ros2 pkg create navigation_demos --build-type ament_python
cd navigation_demos
mkdir launch
mkdir config

Your setup.py file should have some extra lines added to include the launch and config directories:

from setuptools import find_packages, setup
import os
from glob import glob

package_name = 'navigation_demos'

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']),
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
        (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.yaml'))),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Andy West',
    maintainer_email='andrew.west@manchester.ac.uk',
    description='TODO: Package description',
    license='MIT',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

A launch file will be built up gradually, whilst we add more config files to our navigation system. In the launch directory of the package, create a launch file called nav_demo.launch.py. You can copy the code below to get you started.

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch_ros.actions import SetParameter, Node
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution


def generate_launch_description():
    ld = LaunchDescription()

    # Parameters, Nodes and Launch files go here

    # Declare package directory
    pkg_nav_demos = get_package_share_directory('navigation_demos')
    # Necessary fixes
    remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]


    # LOAD PARAMETERS FROM YAML FILES
    config_bt_nav     = PathJoinSubstitution([pkg_nav_demos, 'config', 'bt_nav.yaml'])

    # Include Gazebo Simulation
    launch_gazebo = IncludeLaunchDescription(
    PythonLaunchDescriptionSource([get_package_share_directory('gz_example_robot_description'), '/launch', '/sim_robot.launch.py']),
    launch_arguments={}.items(),
    )

    # Include SLAM Toolbox standard launch file
    launch_slamtoolbox = IncludeLaunchDescription(
    PythonLaunchDescriptionSource([get_package_share_directory('slam_toolbox'), '/launch', '/online_async_launch.py']),
    launch_arguments={}.items(),
    )

    # Behaviour Tree Navigator
    node_bt_nav = Node(
        package='nav2_bt_navigator',
        executable='bt_navigator',
        name='bt_navigator',
        output='screen',
        parameters=[config_bt_nav],
        remappings=remappings,
    )

    # Behaviour Tree Server
    node_behaviour = Node(
        package='nav2_behaviors',
        executable='behavior_server',
        name='behaviour_server',
        output='screen',
        parameters=[config_bt_nav],
        remappings=remappings,
    )


    # Add actions to LaunchDescription
    ld.add_action(SetParameter(name='use_sim_time', value=True))
    ld.add_action(launch_gazebo)
    ld.add_action(launch_slamtoolbox)
    ld.add_action(node_bt_nav)
    ld.add_action(node_behaviour)

    return ld

Also add the bt_nav.yaml to the config directory. Download the file bt_nav.yaml.

Then check everything builds as per usual.

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

Now we can start the tutorial.