Debug SLAM-to-Nav2 Integration

The problem

Your SLAM works. Now Nav2 should drive the TurtleBot 3 to a goal on the live map. You have a hand-written bring-up: SLAM Toolbox, the controller, planner, behaviour and navigator servers, and a lifecycle manager.

Goals are sent but the robot does not move, and the logs complain about different things at different moments. There are four faults; they interact.

Expected behaviour. All Nav2 servers are active, the global costmap is in the map frame and shows the SLAM map, a goal 1 m ahead is accepted and finishes with status SUCCEEDED, and the robot ends within 0.3 m of it.

Observable failure. Goals are accepted but nothing moves, or they are rejected; ros2 lifecycle get shows servers that are not active; the costmap logs say it cannot transform; /cmd_vel has no publisher.

This is a real ROS 2 project, not a browser lab. You run it on your own machine with ROS 2, a simulator and the tools of Module 7. The website does not run ROS 2 and does not grade it; you grade yourself with the criteria below and the check.sh script.

Time: about 90 to 120 minutes · Level: Advanced · Theory: M7-T7 SLAM and Nav2 Integration

Prerequisites and setup

You need a computer with ROS 2 Humble on Ubuntu 22.04 (a virtual machine or container with a display is fine). Jazzy works for the SLAM and Nav2 parts; the simulator launch differs, and the project notes say where. Install once:

sudo apt update
sudo apt install ros-humble-turtlebot3-gazebo ros-humble-turtlebot3-teleop ros-humble-slam-toolbox \
  ros-humble-navigation2 ros-humble-nav2-bringup ros-humble-tf2-tools ros-humble-tf2-ros ros-humble-rviz2
source /opt/ros/humble/setup.bash
export TURTLEBOT3_MODEL=burger

Get the project files: ros2_labs/l5-debug-slam-to-nav2-integration in the site’s repository (clone it, or copy the files listed below from this page). The folder ros2_labs/common holds scripts shared by several labs.

1. Terminal 1: export TURTLEBOT3_MODEL=burger, then ros2 launch launch/lab5.launch.py.
2. Terminal 2: teleop; drive a metre or two and turn so that the SLAM map is not empty. Stop the teleop before sending goals.
3. Terminal 3: debugging. Start RViz and add Map, the global and local costmap displays, and the Path.

Verification status. The project files follow the documented Humble interfaces but have not been executed by the site’s automated tests (they cannot run ROS 2). If a command or parameter is rejected by your version, the error names it: treat that as part of the exercise and tell us.

The project files

The launch file starts Nav2 servers one by one (not with the packaged bring-up) so that you can see every connection. The parameter file is reduced and uses Humble plugin names.

launch/lab5.launch.py

"""Lab M7-L5: TurtleBot 3 world, SLAM Toolbox (live map) and a hand-written Nav2 bring-up. Navigation does not work; find out why.

Run:   ros2 launch launch/lab5.launch.py
Then:  ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: {header: {frame_id: map}, pose: {position: {x: 1.0, y: 0.0}, orientation: {w: 1.0}}}}"
(drive the robot a metre or two with turtlebot3_teleop first, so that the map is not empty)
"""
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node


def generate_launch_description():
    here = os.path.dirname(os.path.realpath(__file__))
    slam = os.path.join(here, '..', 'config', 'slam_params.yaml')
    nav = os.path.join(here, '..', 'config', 'nav2_params.yaml')
    tb3 = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch', 'turtlebot3_world.launch.py')
    remaps = [('cmd_vel', 'cmd_vel_nav')]
    return LaunchDescription([
        IncludeLaunchDescription(PythonLaunchDescriptionSource(tb3)),
        Node(package='slam_toolbox', executable='async_slam_toolbox_node', name='slam_toolbox', output='screen', parameters=[slam]),
        Node(package='nav2_controller', executable='controller_server', name='controller_server', output='screen',
             parameters=[nav], remappings=remaps),
        Node(package='nav2_planner', executable='planner_server', name='planner_server', output='screen', parameters=[nav]),
        Node(package='nav2_behaviors', executable='behavior_server', name='behavior_server', output='screen', parameters=[nav]),
        Node(package='nav2_bt_navigator', executable='bt_navigator', name='bt_navigator', output='screen', parameters=[nav]),
        Node(package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_navigation', output='screen',
             parameters=[{'use_sim_time': True, 'autostart': True,
                          'node_names': ['controller_server', 'planner_server', 'bt_navigator']}]),
    ])

config/nav2_params.yaml

# Nav2 parameters for lab M7-L5 (reduced; written for the Humble plugin names). It contains mistakes on purpose.
bt_navigator:
  ros__parameters:
    use_sim_time: false
    global_frame: map
    robot_base_frame: base_footprint
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20

controller_server:
  ros__parameters:
    use_sim_time: true
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"]
    controller_plugins: ["FollowPath"]
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    general_goal_checker:
      stateful: true
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.15
      yaw_goal_tolerance: 0.25
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.2
      lookahead_dist: 0.5
      min_lookahead_dist: 0.3
      max_lookahead_dist: 0.9
      lookahead_time: 1.5
      rotate_to_heading_angular_vel: 1.0
      transform_tolerance: 0.1
      use_velocity_scaled_lookahead_dist: false
      use_rotate_to_heading: true
      allow_reversing: false

planner_server:
  ros__parameters:
    use_sim_time: true
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

behavior_server:
  ros__parameters:
    use_sim_time: true
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
    wait:
      plugin: "nav2_behaviors/Wait"
    global_frame: odom
    robot_base_frame: base_footprint
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: true
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_footprint
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.12
      plugins: ["obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.35
      always_send_full_costmap: true

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: true
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: odom
      robot_base_frame: base_footprint
      robot_radius: 0.12
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true
        subscribe_to_updates: true
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.35
      always_send_full_costmap: true

config/slam_params.yaml

# SLAM Toolbox parameters for lab M7-L5 (correct as given).
slam_toolbox:
  ros__parameters:
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None
    use_sim_time: true
    odom_frame: odom
    map_frame: map
    base_frame: base_footprint
    scan_topic: /scan
    mode: mapping
    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02
    map_update_interval: 2.0
    resolution: 0.05
    max_laser_range: 3.5
    minimum_time_interval: 0.3
    transform_timeout: 0.2
    tf_buffer_duration: 30.
    stack_size_to_use: 40000000
    use_scan_matching: true
    minimum_travel_distance: 0.2
    minimum_travel_heading: 0.2
    do_loop_closing: true

What to do

Follow the loop of this track: Problem → Experiment → Diagnose → Fix → Evaluate → Understand.

1. Experiment. Send the goal command from the header of the launch file. Describe exactly what happens and read each server’s log.
2. Diagnose the lifecycle. For each server run ros2 lifecycle get /<name>. Which ones are not active? Read the launch file: which servers does the lifecycle manager know about?
3. Diagnose time. ros2 param get /bt_navigator use_sim_time and the same for the other servers. Why would one node on wall time break a navigation goal (M7-T3)?
4. Diagnose frames. ros2 param get /global_costmap/global_costmap global_frame. In which frame should the global costmap live, and the local one (M7-T7)?
5. Diagnose the output. ros2 topic info /cmd_vel -v and ros2 topic info /cmd_vel_nav -v. Who publishes where, and who listens?
6. Fix, one change at a time, restarting after each, and record which symptom each removed. Then evaluate with ./check.sh and write the explanation of each fault in your own words.

Expected output

Typical output when things are right. Exact numbers, timestamps and wording differ between machines and versions.

Lifecycle states

ros2 lifecycle get /controller_server
active [3]
ros2 lifecycle get /planner_server
active [3]

A goal that succeeds

ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "..."
Goal accepted with ID: ...
Result:
  result: {}
Goal finished with status: SUCCEEDED

Velocity commands reach the base

ros2 topic info /cmd_vel -v
Publisher count: 1
Node name: controller_server
...
Subscription count: 1
Node name: turtlebot3_diff_drive

check.sh when everything is fixed

PASS  lifecycle: controller_server is active
...
PASS  the global costmap is in the map frame
PASS  the controller output reaches the base (/cmd_vel has a publisher)
PASS  a goal 1 m ahead is reached (NavigateToPose succeeds)
PASS  the robot ended within 0.3 m of the goal

9 passed, 0 failed

Troubleshooting

Evaluation criteria

You are done when every criterion passes. check.sh automates most of them (run it from the project folder, with the launch file running and ROS 2 sourced). The remaining ones you judge yourself.

cd ros2_labs/l5-debug-slam-to-nav2-integration
./check.sh

Worked explanation

Try the lab before opening this.

Why it happens

Navigation sits on top of everything in this module: the data must connect (topics and remaps), the frames must agree (REP 105), the clocks must agree (simulated time everywhere), and the lifecycle must be complete (every server configured and activated). SLAM supplies the map and map → odom; Nav2 plans in map and controls in odom (M7-T7). Each fault here is outside the SLAM algorithm, which is why a debugging habit that starts with the wiring (M7-T1) finds them.

What this project does not cover

• The reduced parameter file omits the smoother, waypoint follower and docking; the packaged bring-up has more nodes and a different lifecycle list.
• Plugin names follow Humble; Jazzy renamed some (for example nav2_navfn_planner::NavfnPlanner).
• The simulated world is small and static; dynamic obstacles and narrow doors need tuning of the inflation (M7-T7).
• The project files have not been run by the site’s tests.

What’s next

Other projects in this module

• M7-L1: Debug a ROS 2 SLAM Pipeline
• M7-L2: Fix a TF Configuration Error
• M7-L3: Diagnose Sensor Timestamp Problems
• M7-L4: Configure Mapping and Localization
• M7-L5: Debug SLAM-to-Nav2 Integration: this project.