Skip to main content

Getting Started​

These are my notes from the ROS2 Navigation course available on The Construct. As this is a robotics note, you will need physical hardware components to follow along. Before each section though, I will list what is required before starting.

Requirements​

Before starting, we need a robot with LIDAR and Odometry. We also need to use SLAM for localizing. Available ones are cartographer and SLAM-Toolbox. This note uses cartographer as the learning curve is lower. It is notable that cartographer is no longer maintained, so the better software to use is SLAM-Toolbox.

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in both 2D and 3D across multiple platforms and sensor configurations. Cartographer_ros is a ROS wrapper of cartographer so that we can integrate cartographer with ROS. This allows us to use prebuilt packages and only have to configure them.

cartographer_2d

Map​

A map is a representation of the environment that the robot is functioning in. It uses the map to localize and plan trajectories. Maps are implemented in ROS2 via an occupancy grid where in each cell, specific values represent obstacles.

Configuring Cartographer​

We use a launch file because we can start multiple nodes and can also set a node-specific parameter while launching a node. These parameters can then be loaded from a YAML file or specified in the launch file.

To launch cartographer, we need to launch two nodes. Below are the files you need within their respective directories.

<package>/launch

import os
from ament_index_python.packages import get_package_share_directory

cartographer_config_dir = os.path.join(get_package_share_directory('cartographer_slam'), 'config')

Node(
package='cartographer_ros',
executable='cartographer_node',
name='cartographer_node',
output='screen',
parameters=[{'use_sim_time': True}], # boolean indicating node synchronization time with simulation time
arguments=['-configuration_directory', cartographer_config_dir,
'-configuration_basename', configuration_basename] # configuration_directory specifies where the configuration files are # configuration_basename specifies the configuration file name
),
Node(
package='cartographer_ros',
executable='cartographer_occupancy_grid_node',
output='screen',
name='occupancy_grid_node',
parameters=[{'use_sim_time': True}],
arguments=['-resolution', '0.05', '-publish_period_sec', '1.0'] # resolution specifies number of meters per grid in the map # publish_period_sec specifies frequency of publishing to /map topic
)

Compilation​

Compile the package and launch the launch file from the CLI. Once we've done that, run RVIZ2 on another terminal while our cartographer node is running.

To display our mapping, add the map display and set the topic to /map. If the mapping isn't showing, then the Quality of Service (QoS) parameters of the /map may be wrong. We can add a couple more displays.

  • TF: Allows us to see the frames of the robot
  • LaserScan: Allows us to see the laser coliding with the objects on the map
note

These are all specific to the hardware configurations and the topic may be dependent on your system.

Alternative Configurations​

Cartographer works for many robots and sensors but must be configured properly to work optimally. By default, cartographer automatically subscribes to /scan, /odom, /imu topics. All configurations can be given from the Lua configuration file. This means that in the launch files, we have to remap those three if we want to publish data to them.

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='cartographer_ros',
            executable='cartographer_node',
            name='cartographer',
            remappings=[
                ('/scan', '/my_laser_scan'),
                ('/odom', '/my_odom'),
                ('/imu', '/my_imu')
            ]
        )
    ])

A list of configuration parameters and their details for cartographer

CategoryParameterDescriptionDefault
Framesmap_frame

ROS frame ID for publishing sub-maps, the parent frame of poses.

map
tracking_frame

Frame ID tracked by SLAM. IMU position if used, commonly base_link or base_footprint.

base_link
published_frame

Child frame for publishing poses. If odom provided, it publishes pose of odom in map_frame.

odom or base_link
odom_frame

Frame for publishing local SLAM result if provide_odom_frame is true.

odom
provide_odom_frame

Publishes local non-loop-closed pose as odom_frame in map_frame if enabled.

true
use_odometry

Subscribes to nav_msgs/Odometry on odom topic and includes odometry in SLAM.

true
use_nav_sat

Subscribes to sensor_msgs/NavSatFix on fix topic and includes navigation data in SLAM.

true
Laser Parametersnum_laser_scansNumber of laser scan topics to subscribe to.1
num_multi_echo_laser_scans

Number of multi-echo laser scan topics to subscribe to.

0
num_subdivisions_per_laser_scan

Number of point clouds to split each laser scan for unwarping during motion.

1
num_point_cloudsNumber of point cloud topics to subscribe to.1
Filter Parameterslookup_transform_timeout_sec

Timeout in seconds for looking up transforms using tf2.

0.2
submap_publish_period_secInterval in seconds to publish submap poses.0.3
pose_publish_period_sec

Interval in seconds to publish poses (e.g., 5e-3 for 200 Hz).

5e-3
trajectory_publish_period_sec

Interval in seconds to publish trajectory markers (e.g., 30e-3 for 30 ms).

30e-3
odometry_sampling_ratioSampling ratio for odometry messages.1.0
fixed_frame_sampling_ratioSampling ratio for fixed frame messages.1.0
imu_sampling_ratioSampling ratio for IMU messages.1.0
landmarks_sampling_ratioSampling ratio for landmark messages.1.0
Trajectory BuilderTRAJECTORY_BUILDER_2D.min_range

Minimum distance measured to be considered for mapping.

0.2
TRAJECTORY_BUILDER_2D.max_range

Maximum distance measured to be considered for mapping.

30.0
TRAJECTORY_BUILDER_2D.missing_data_ray_lengthDistance to treat as a lost laser ray.25.0
TRAJECTORY_BUILDER_2D.use_imu_dataWhether or not to use IMU data.true

If we want to save the map that we mapped out, we can run an executable named map_saver from nav2_map_server. It's important that we call this within the directory that we want to save the map in. The cartography has to be running for this to work.

ros2 run nav2_map_server map_saver_cli -f <name>

This'll generate two files of type .pgm and .yaml. The .pgm is an image file of the map as an occupancy grid image. The .yaml contains details about the resolution of the map.

image: turtlebot_area.pgm
mode: trinary
resolution: 0.05 # Resolution of the map (meters/pixel)
origin: [-3.8, -1.84, 0] # Coordinates of the lower-left pixel on the map (x,y,z). The third value indicates rotation and if there is none, value is 0
negate: 0 # Inverts the color of the map.
occupied_thresh: 0.65 # Pixels with a value greater than this value will be considered an occupied zone
free_thresh: 0.25 # Pixels with a value smaller than this will be considered a completely free zone

Using Saved Map​

If we want to use the saved map, we have to launch map_server and nav2_lifecycle_manager. We can create a custom launch file for this, but we must specify some parameters.

from launch import LaunchDescription
from launch_ros.actions import Node
frmo ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    map_file = os.path.join(get_package_share_directory('map_server'), 'config', 'turtlebot_area.yaml')
    return LaunchDescription([
        Node(
            package="nav2_map_server",
            executable="map_server",
            name="map_server",
            output="screen",
            parameters=[{'use_sim_time': True}, # Boolean indicating if map_server must synchronize with its time with the simulation
                        {'yaml_filename':map_file] # The complete path to the map yaml file
            ),
        Node(
            package="nav2_lifecycle_manager",
            executable="lifecycle_manager",
            name="lifecycle_manager_mapper",
            output="screen",
            parameters=[{'use_sim_time': True},
                        {'autostart': True}, # lifecycle manager autostart when launched
                        {'node_names':  ['map_server']}] # Nodes that the lifecycle manager takes care of
            )
        ])

We can try using our saved map by creating a ROS2 package called map_server, placing our saved map files inside of config, and creating our launch file. Note that we can just put our launch file in any package and just create a new launch file with our configs.

info

You must specify your map display durability as Transient Local. Volatile means use incoming data being published as the map source, while transient local allows us to check if we had any cached messages that were published but not received by our subscriber as well as all incoming data.

Navigation nodes are managed nodes meaning that they can be easily controlled to be restarted, paused, or running. They can do so by being at these states:

  • Unconfigured
  • Inactive
  • Active
  • Finalized

The diagram is taken from the course and depicts how nodes transition from one state to another. Seems to be like a state machine. Node Transition

Managed nodes start at the unconfigured state and is active usually when running. To transition to active, nodes need an external agent that moves them to this state. Several nodes in Nav2, such as map_server, amcl, planner_server, and controller_server, are lifecycle enabled meaning they are managed nodes.

These nodes have the required overrides of the lifecycle functions:

  • on_configure()
  • on_activate()
  • on_deactivate()
  • on_cleanup()
  • on_shutdown()
  • on_error()

Lifecycle Manager​

In Nav2, the external agent that helps transition nodes to an active state is called the nav2_lifecycle_manager. The lifecycle manager changes the state of the managed nodes to achieve a controlled startup, shutdown, reset, pause, or resume of the navigation stack.

Referenced from Steve Macenski

Figure

Nav2 uses a wrapper of LifecycleNode, the nav2_util LifecycleNode and it hides many abstractions of LifecycleNodes for typical applications. It includes a bond connection for the lifecycle manager to ensure that after a node transitions up, it also remains active. When a node crashes, it lets the lifecycle manager know and transitions down the system to prevent a critical failure.

The nav2_lifecycle_manager provides a ROS service from which other ROS nodes can invoke the startup, shutdown, reset, pause, or resume functions. Based on these requests, the nav2_lifecycle_manager calls the necessary lifecycle services in the managed nodes. As shown previously in the nav2_lifecycle_manager node within the launch file, we specify which nodes need management via node_names. The ordering matters as that is the order in which they have to be moved to initialized (first to last) and the order in which they are paused and stopped (last to first).

It's important that the manager has the autostart parameter which allows the nav2_lifecycle_manager to do it's default behavior.

You interact with the nav2_lifecycle_manager via a service named:

/<lifecycle_manager_node_name>/manage_nodes

ros2 service call /lifecycle_manager/manage_nodes nav2_msgs/srv/ManageLifecycleNodes "{command: 0}" # Start the nodes
ros2 service call /lifecycle_manager/manage_nodes nav2_msgs/srv/ManageLifecycleNodes "{command: 1}" # Pause the nodes
ros2 service call /lifecycle_manager/manage_nodes nav2_msgs/srv/ManageLifecycleNodes "{command: 2}" # Resume the nodes
ros2 service call /lifecycle_manager/manage_nodes nav2_msgs/srv/ManageLifecycleNodes "{command: 3}" # Reset the nodes
ros2 service call /lifecycle_manager/manage_nodes nav2_msgs/srv/ManageLifecycleNodes "{command: 4}" # Shutdown the nodes

This is because the service /manage_nodes uses the message nav2_msgs/srv/ManageLifecycleNodes which is as follows:

uint8 STARTUP = 0
uint8 PAUSE = 1
uint8 RESUME = 2
uint8 RESET = 3
uint8 SHUTDOWN = 4

uint8 command
---
bool success

Localization​

Localization is knowing the current position with respect to the environment. This means knowing the position and orientation. This is otherwise known as the pose.

ROS has a very robust algorithm for localization. It uses AMCL which is the Adaptive Monte-Carlo Licalization and uses a probabilistic localization system for a robot moving in 2D. It implements the adaptive or KLD sampling Monte Carlo localization approach which uses a particle filter to track a robot's pose against a known map.

A ROS Robot is localized when somebody publishes a transform between the /map frame and the /odom frame

That means that the /odom frame of the robot knows its relative position to the /map frame. Therefore, the robot knows its position on the map since /base_link frame is directly connected to the /odom frame.

When everything is correct, the AMCL publishes that transform.

ROS2 Localization​

In order to localize, we need three nodes:

  • map_server: provides the map to the localization algorithm
  • localization: the algorithm
  • life cycle manager: state manager

Let's create an ament_cmake package and the launch file for all three nodes.

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
import os

def generate_launch_description():
nav2_yaml = os.path.join(get_package_share_directory('localization'), 'config', 'amcl_config.yaml')
map_file = os.path.join(get_package_share_directory('map_server'), 'config', 'turtlebot_area.yaml')
print(map_file)
return LaunchDescription([
Node(
package="nav2_map_server",
executable="map_server",
name="map_server",
output="screen",
parameters=[{'use_sim_time': True}, {'yaml_filename': map_file}] ## This is the saved map we got from our cartography.
),
Node(
package="nav2_amcl",
executable="amcl",
name="amcl",
output="screen",
parameters=[nav2_yaml]
),
Node(
package="nav2_lifecycle_manager",
Launch Fil executable="lifecycle_manager",
name="lifecycle_manager",
output="screen",
parameters=[{'use_sim_time': True},
{'autostart': True},
{'node_names': ['map_server', 'amcl']}]
)
])

If all is placed correctly, we should be able to receive our old map. From there onwards, you want to configure your RVIZ settings with these below:

  • map: Environment
  • TF: Frames of the robot
  • Robot Model: Robot structure (uses URDF model)
  • Laser Scan: Estimate of robot position relative to actual map
  • Pose with Covariance: Status of the localization
  • Particle Cloud: See the localization particle distribution
note

Particle cloud uses the reliability policy of best effort and not volatile. If you want to use these configs again, you can save them.

We can use the 2D Pose Estimate to tell our AMCL system to start localizing the robot. You want to put it at a rough estimate of where the robot is so the algorithm can estimate roughly from the initial state. This'll start publishing the transform between map and odom topics respectively. When we say transform, it means relationship. The map and odom data are independent from oen another, but there is a mathematical relationship between them using AMCL, and can be used to "localize" our robot's position from these data. That's because we have laser scan (environment scanning) and odometry (wheel/position).

If we start moving our robot, our estimation and localization starts to fit better.

info

For 2D Localization, we only need (x,y,θ)(x,y, \theta) and are the variables used for setting the robot in the map frame. We can compute (x,y,θ)(x,y,\theta) by using the AMCL. The Adaptive Monte Carlo Localization algorithm uses particles to localize the robot. Each particle have their own coordinates, orientation value, and a weight. These weights are the likelihood of the particle being a good estimate of the robot's actual pose based on our sensor data. That means that if our weight is higher for a given particle, that particle is more likely to represent the robot's true pose. These particles are then re-sampled whenever the robot moves and with new sensor data, the higher likelihood particles remain and will converge over many iterations to the robot's true pose.

To get a good localization of our robot, we need to configure out AMCL node from our amcl_config.yaml. Below are the parameters and their respective categories.

AMCL Parameters and Descriptions
ParameterTypeDefaultDescription
min_particlesint500Minimum allowed number of particles.
max_particlesint2000Maximum allowed number of particles.
update_min_ddouble0.25 metersTranslational movement required before performing a filter update.
update_min_adouble0.2 radiansRotational movement required before performing a filter update.
resample_intervalint1Number of filter updates required before re-sampling.
transform_tolerancedouble1.0 secondsTime to post-date the published transform, indicating validity into the future.
recovery_alpha_slowdouble0.0 (disabled)

Exponential decay rate for the slow average weight filter; helps decide recovery with random poses. Suggested: 0.001.

recovery_alpha_fastdouble0.0 (disabled)

Exponential decay rate for the fast average weight filter; helps decide recovery with random poses. Suggested: 0.1.

set_initial_poseboolfalse

Causes AMCL to set the initial pose from initial_pose* parameters instead of waiting for the initial_pose message.

initial_posePose2D0X, Y, Z, and yaw coordinates of the robot base frame's initial pose in the global frame.
always_reset_initial_poseboolfalseRequires an initial pose on reset. Uses the last known pose if false.
save_pose_ratedouble0.5 Hz

Maximum rate at which to save the last estimated pose and covariance to the parameter server. Use -1.0 to disable.

Laser Model Parameters
ParameterTypeDefaultDescription
laser_min_rangedouble-1.0Minimum scan range to be considered. Use laser's minimum range if set to -1.0.
laser_max_rangedouble100Maximum scan range to be considered. Use laser's maximum range if set to -1.0.
max_beamsint60Number of evenly-spaced beams in each scan used when updating the filter.
z_hitdouble0.5Mixture weight for z_hit part of the model.
z_shortdouble0.05Mixture weight for the z_short part of the model.
z_maxdouble0.05Mixture weight for the z_max part of the model.
z_randdouble0.5Mixture weight for the z_rand part of the model.
sigma_hitdouble0.2 metersStandard deviation for Gaussian model used in z_hit part.
lambda_shortdouble0.1Exponential decay parameter for the z_short part of the model.
laser_likelihood_max_distdouble2.0 metersMaximum distance for obstacle inflation in the likelihood_field model.
laser_model_typestring"likelihood_field"

Model to use: beam, likelihood_field, or likelihood_field_prob (includes beam skipping).

Odometry Model Parameters
ParameterTypeDefaultDescription
robot_model_typestring"differential"Type of motion model: differential or omnidirectional.
alpha1double0.2Noise in odometry's rotation estimate from rotational motion.
alpha2double0.2Noise in odometry's rotation estimate from translational motion.
alpha3double0.2Noise in odometry's translation estimate from translational motion.
alpha4double0.2Noise in odometry's translation estimate from rotational motion.
alpha5double0.2Translation-related noise parameter (only for omni model).
odom_frame_idstring"odom"Frame to use for odometry.
base_frame_idstring"base_footprint"Frame to use for the robot base.
global_frame_idstring"map"Name of the coordinate frame published by the localization system.
tf_broadcastbooltrueSet to false to prevent AMCL from publishing the transform between global and odometry frames.

Auto Localization​

Via YAML​

If you don't want to localize via GUI rviz, you can do so by modifying the configuration settings for amcl. When you start your localization server, you're given the initial_pose, to which you can set those within the amcl_config:

set_initial_pose: true
initial_pose:
  x: -4.44264
  y: 2.32243
  yaw: 0.328028

Rebuild the package, and you can start the server and not have to localize the robot.

Via CLI​

ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {pose: {position: {x: 0.2, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}}"

Global Localization​

Global localization is when neither the person or robot knows where it is on the map. In order to localize itself, it does global localization:

  1. Distribute all the particles of the filter across the map. (Calls the service /reinitialize_global_localization
  2. Move the robot around until it detects where the it is located. If the environment is simple, can just rotate itself.

Modify the amcl_config to increase particles:

max_particles: 80000
min_particles: 2000

Call the service and start moving around to localize.

ros2 service call /reinitialize_global_localization std_srvs/srv/Empty

Path Planning​

Path planning is planning a route from Point A to Point B, avoiding obstacles along the way.

In ROS2, to do path planning, we have to launch several nodes. Before we start any path planning, we need to have two nodes which provides the base map and localization:

Mapping Environment​
  1. nav2_map_server: provides the base map as a topic and service
  2. nav2_amcl: responsible for localization via Adaptive Monte Carlo Localizer

To do path planning, we need these nodes:

Path Planning​
  1. planner
  2. controller
  3. manager_of_recovery_behaviors
  4. behavior_tree_navigator
  5. nav2_lifecycle_manager

The planner is responsible for computing a path from Point A to Point B. It can compute a path as soon as the robot receives a 2D_Goal_Pose and also has access to a global environment representation and sensor data buffered into it ('Global Costmap`).

Currently on Galactic, there is only one Planenr algorithm available in ROS2, the Nav2Fn_Planner.

In order to launch the planner, we need these in the launch file:

package='nav2_planner', these in the ```config``` directory.
<Tabs>
executable='planner_server',
name='planner_server',
output='screen',
parameters=[nav2_yaml] # contains all of the configuration parameters
planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

Demo​

We'll start by first creating a new package using our favorite CMake build system. We'll then create two directories: config and launch.

We then create these and place them in their respective directories.

from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():

    planner_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'planner.yaml')
    controller_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'controller.yaml')
    recovery_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'recovery.yaml')
    bt_navigator_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'bt_navigator.yaml')

    return LaunchDescription([
        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[planner_yaml]
            ),
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen', these in the ```config``` directory.

<Tabs>                               these in the ```config``` directory.    
<Tabs>                                                                                                                                                                
            parameters=[controller_yaml],
            ),
        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_yaml],
            ),
        Node(
            package='nav2_behaviors',
            executable='behavior_server',
            name='behavior_server',
            output='screen',
            parameters=[recovery_yaml],
            ),
        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_pathplanner',
            output='screen',
            parameters=[{'autostart': True},
                        {'node_names': ['planner_server', 'controller_server', 'behavior_server', 'bt_navigator']}],
            )
        ])

info

We have to be running in parallel our localization server, the one responsible for localizing our robot with the map and the one providing the base map environment.

RVIZ2 Settings​

On top of our settings provided for localization, we add the Path display and change LineStyle to BillBoards. If everything is set, we can get something cool like below!

We can send a goal via the CLI through the server interface /navigate_to_pose or goal_pose.

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "pose: {header: {frame_id: map}, pose: {position: {x: 1.52, y: 1.92, z: 0.0}, orientation:{x: 0.0, y: 0.0, z: 0, w: 1.0000000}}}"

Obstacle Avoidance​

The Nav2 stack avoids dynamic and static obstacles by utilizing what's known as costmaps.

Costmaps​

A Costmap is a 2D representation of robot-sensed obstacles on a grid map. Each grid cells contain information about the obstacles detected by the sensors. The cost of the cell can be unknown, free, occupied, or inflated. Different colors indicate how likely it is to colide against an obstacle. Using this information, the Nav2 stack can compute their tasks safely and efficiently.

There are two kinds of costmaps: global and local.

  1. global: generated from obstacles on a static map and is used by the planner to generate long term path.
  2. local: created from new obstacles during runtime(sensors) and is used by the controller to generate short-term path

Global Costmaps​

To add a global costmap, we can add these parameters to the planner.yaml file.

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.15
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.35
      always_send_full_costmap: True

When we run our path_planner_server with these new parameters, we see an added map on top of our base map. When we add a new obstacle, the global costmap is not updated. The differences can be seen below. This means that our planner will not take into consideration the new dynamic obstacle that spawned in and will crash.

note

The different tones of gray are to illustrate the different regions the robot can use. Dark tones of grey means the robot cannot use that area to path plan as it is too close to an obstacle

Global Costmap Obstacle Layers​

Global costmaps are generated as the superposition of different obstacle layers. This means that each layer adds a set of obstacles to the global costmap based on how the layer computes the obstacles.

Available Costmap layers:

  • Static Layer: Adds as obstacles to the global costmap any black dot that exists in the static map
  • Inflation Layer: Adds an inflation to any obstacle in the global costmap, as a safe distance to maintain
  • Obstacle Layer: Adds to the global costmap any object detected by a 2D sensor
  • Voxel Layer: Adds to the global costmap 3D obstacles from PointCloud data.

We can specify which layers or all in our parameter list via plugins.

plugins: ["static_layer", "obstacle_layer", "inflation_layer", "voxel_layer"] # Order matters here, inflation is applied after obstacle, etc.

Adding these layers will allow us to account for dynamic obstacles.

General Parameters​

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.15
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

Local Costmap​

Let's figure out how to add a local costmap. It's no different than a global costmap but goes inside of controller.yaml instead of planner.yaml. This is because a local costmap is a dynamic version of the global costmap using sensors.

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 1
      height: 1
      resolution: 0.05
      robot_radius: 0.15
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.35
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
info

The robot's shape can influence what extent the obstacles in the Costmaps will be inflated to. This is then taken into consideration when computing a path to said point. We can specify our robot's shape within our costmap parameters in both local and global.

When we use round robots, we use the parameter robot_radius. For more complex robots that aren't round, we have to use footprint parameter instead that specifies the coordinates for each point of the robot. The footprint parameter is the project of the robot on the ground. For example, if a square robot has 65 cm per side, we would specify:

footprint: "[[0.325, 0.325], [0.325, -0.325], [-0.325, -0.325], [-0.325, 0.325]]"

We also have to change the controller.yaml configuration:

  • BaseObstacle if using robot_radius
  • ObstacleFootprint if using footprint

Remember that we can only specify one parameter: footprint or robot_radius.

Demo​

Path Planner Package​

Currently, we have two packages: localization and path_planner. One does the localization and mapping while the other does the path planning. Ideally, for a full navigaiton system, we want a single package with one nav2_lifecycle_manager managing all navigation nodes.

How you wanna structure it is up to you, and having separate packages can be good for debugging, but ideally, in real navigation systems for deployment, you want one package for ease of use with one single launch file.

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    controller_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'controller.yaml')
    bt_navigator_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'bt_navigator.yaml')
    planner_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'planner_server.yaml')
    recovery_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'recovery.yaml')
    amcl_yaml = os.path.join(get_package_share_directory('localization_server'), 'config', 'amcl_config.yaml')
    map_file = os.path.join(get_package_share_directory('map_server'), 'config', 'turtlebot_area.yaml')

    return LaunchDescription([
        Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[{'use_sim_time': True},
                        {'yaml_filename':map_file}]
        ),

        Node(
            package='nav2_amcl',
            executable='amcl',
            name='amcl',
            output='screen',
            parameters=[amcl_yaml]
        ),
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[controller_yaml]),

        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[planner_yaml]),

        Node(
            package='nav2_behaviors',
            executable='behavior_server',
            name='recoveries_server',
            parameters=[recovery_yaml],
            output='screen'),

        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_yaml]),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager',
            output='screen',
            parameters=[{'autostart': True},
                        {'node_names': ['map_server',
                                        'amcl',
                                        'controller_server',
                                        'planner_server',
                                        'recoveries_server',
                                        'bt_navigator']}])
    ])

Multi-robot Navigation​

In the scenario you have multiple robots, each robot will have the same /cmd_vel topic name, same base_footprint frame name and even the same control node name. This is complicated because the navigation stack won't know which robot it's talking to. We can bypass this by using namespaces which are labels we assign to each robot. This means that each robot may have the same name but will have a unique namespace for it. namespaces are configured at:

  1. Simulated Robots: At the URDF description file and the Gazebo plugins
  2. Real Robots: At the bring-up routine provided by the vendor

If these are not configured properly, the robots will have difficulty booting into namespaces mode. For two robots, we could have the following:

/clock
/parameter_events
/performance_metrics
/rosout
/tb3_0/cmd_vel
/tb3_0/imu
/tb3_0/joint_states
/tb3_0/odom
/tb3_0/robot_description
/tb3_0/scan
/tb3_1/cmd_vel
/tb3_1/imu
/tb3_1/joint_states
/tb3_1/odom
/tb3_1/robot_description
/tb3_1/scan
/tf
/tf_static

Configuration​

To have a multiagent system that uses the same nav stack, we need for each robot:

  • localization
  • controller_server
  • planner_server
  • bt_navigator

The only thing that'll be used by all is the map_server as all robots will use the same map.

Mapping​

It is possible to have multiple robots collaborate to create a single map between them but that is covered later on. This tutorial goes over:

  1. One robot from the fleet in charge of making the robot
  2. A single map_server is launched using that map
  3. All the navigation systems for each robot will use the same name by asking for the same map_server

Multirobot cartographer​

There isn't much difference when trying to do multirobot navigation. For mapping, al lwe do is add and modify a few lines of code. For this, we use our cartographer config files and simply add these few lines of code below.

tracking_frame = "tb3_0/base_footprint",
published_frame = "tb3_0/odom",
odom_frame = "tb3_0/odom"

Configuring RVIZ2​

  • Load the RVIZ configuration of the localization unit: .rviz
  • Modify the topic of the LaserScan to point to the tb3_0/scan and indicate the Color Transformer as FlatColor.
  • Add another LaserScan and make it tb3_1/scan with Color Transformer as the same as previous but different color.
  • Modify the RobotModel display as below:
    • In the Description Source, select Topic and indicate /tb3_0/robot_description.
    • In the TF prefix, indicate the namespace of the robot, which for tb3_0 is tb3_0.
  • Do the same for tb3_1.
  • Add the Panel of Tool Properties and modify it such that the topics for 2D Pose Estimate. (You do this to localize each robot individually)
    • Should always be /namespace/topic_name.
  • You should relaunch the simulation because the robots will drift if given a long time of idling in the simulation. Then localize by doing the 2D Pose Estimate.

After, we should get something like this:

Path Planning​

To path plan for multiple robots, we need n path planning node systems. This means for our two robot, we need:

  • two controller_server nodes
  • two planner_server nodes
  • two recoveries_server nodes
  • two bt_navigator nodes
  • Add a namespace to the launches of the nodes.

Example below for one single node, the planner_server. We need to do this for the 4 nodes, for each robot, meaning we should have 8 nodes in total for the path planning system.

Node(
    namespace='tb3_0',
    package='nav2_planner',
    executable='planner_server',
    name='planner_server',
    output='screen',
    parameters=[planner_yaml_tb1_0]
),
info

The order is localization -> path_planning -> rviz2 for launches.

Configuring RVIZ2​

  • Add a Path display configured for the /tb3_0/plan topic and select the Line Style as Billboards with Red as it's color.
  • Add another Path for the /tb3_1/plan and choose a different color for it.
  • Add a Map display and configure the topic for the global costmap: /tb3_0/global_costmap/costmap
  • Add another Map for /tb3_1/global_costmap/costmap.
  • Add another Map for the local costmap for again each of the topics: /tb_3_0/local_costmap/costmap and /tb3_1/local_costmap/costmap.
  • All the cost_maps change the color scheme from map to cost_map to see the nice colors.
  • To make the robot move, use the Tool Properties and change the 2D Goal Pose to be /tb3_0/goal_pose and then use the 2D Goal Pose. For the other robot, use the other namespace /tb3_1/goal_pose.

Single Launch​

To make this simpler, we can create several launch files to concentrate everything into a single launch. Inside of our path_planner_server, we can create the launch multi_main.launch.xml. Inside the cartographer_slam, we also create the launch file start_rviz.launch.py which'll launch RVIZ2 without terminal commands. Both of these files should be inside of the launch directory.

<?xml version='1.0' ?>

<launch>
    <include file="$(find-pkg-share localization)/launch/multi_localization.launch.py"/>
    <include file="$(find-pkg-share path_planner_server)/launch/multi_pathplanner.launch.py"/>

    <include file="$(find-pkg-share cartographer_slam)/launch/start_rviz.launch.py">
        <arg name="use_sim_time" value="true"/>
        <arg name="rviz_config_dir" value="$(find-pkg-share cartographer_slam)/rviz/tb3_1_and_2.rviz" />
    </include>

</launch>

Then launch via:

ros2 launch path_planner_server multi_main.launch.xml
Last updated on