ROS 2 SLAM & Navigation Guide 2026slam_toolbox · RTAB-Map · Nav2

sudo apt update
sudo apt install -y \
  ros-jazzy-slam-toolbox \
  ros-jazzy-navigation2 \
  ros-jazzy-nav2-bringup \
  ros-jazzy-robot-localization \
  ros-jazzy-twist-mux

# Verify
ros2 pkg list | grep slam_toolbox
ros2 pkg list | grep nav2

# online_async: processes scans asynchronously (never blocks odometry)
ros2 launch slam_toolbox online_async_launch.py \
  slam_params_file:=./config/mapper_params_online_async.yaml \
  use_sim_time:=false

# online_sync: processes every scan before returning (safer for fast scanners)
# ros2 launch slam_toolbox online_sync_launch.py

slam_toolbox:
  ros__parameters:
    # Solver — use ceres for best loop closure accuracy
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI

    # Scan matcher
    odom_frame: odom
    map_frame: map
    base_frame: base_footprint

    # Scan topic — must publish sensor_msgs/LaserScan
    scan_topic: /scan

    # Mapping settings
    resolution: 0.05          # 5 cm cells — increase for larger environments
    max_laser_range: 20.0     # metres
    minimum_travel_distance: 0.5   # trigger scan only after 0.5 m movement
    minimum_travel_heading: 0.5    # or after 0.5 rad rotation

    # Loop closure
    link_match_minimum_response_fine: 0.45
    link_scan_maximum_distance: 1.5
    do_loop_closing: true
    loop_search_maximum_distance: 3.0
    loop_match_minimum_chain_size: 10

    # Save on exit
    map_file_name: /tmp/my_map

# Binary install (recommended)
sudo apt install ros-jazzy-rtabmap-ros

# Docker (pre-built with all optional deps including GTSAM, g2o, OpenCV CUDA)
docker pull introlab3it/rtabmap:jazzy

ros2 launch rtabmap_launch rtabmap.launch.py \
  rtabmap_args:="--delete_db_on_start" \
  frame_id:=base_footprint \
  subscribe_scan:=true \
  scan_topic:=/scan \
  odom_topic:=/odom \
  qos:=1 \
  rviz:=true

# Start RealSense node first
ros2 launch realsense2_camera rs_launch.py \
  align_depth.enable:=true \
  pointcloud.enable:=true

# Launch RTAB-Map
ros2 launch rtabmap_launch rtabmap.launch.py \
  rtabmap_args:="--delete_db_on_start" \
  rgb_topic:=/camera/camera/color/image_raw \
  depth_topic:=/camera/camera/aligned_depth_to_color/image_raw \
  camera_info_topic:=/camera/camera/color/camera_info \
  frame_id:=base_footprint \
  approx_sync:=false \
  rviz:=true

# Save the current map (while SLAM is running)
ros2 run nav2_map_server map_saver_cli \
  -f /home/user/maps/my_map \
  --ros-args -p save_map_timeout:=5.0

# Output: my_map.yaml + my_map.pgm

# Load saved map for navigation (localization only, SLAM off)
ros2 launch nav2_bringup localization_launch.py \
  map:=/home/user/maps/my_map.yaml \
  use_sim_time:=false

# SLAM Toolbox serialized map (recommended for re-localization)
ros2 service call /slam_toolbox/save_map slam_toolbox/srv/SaveMap \
  "{name: {data: '/home/user/maps/my_map'}}"

# Nav2 bringup (assumes map already loaded or SLAM running)
ros2 launch nav2_bringup navigation_launch.py \
  use_sim_time:=false \
  params_file:=./config/nav2_params.yaml

# OR combine SLAM + Nav2 in one launch
ros2 launch nav2_bringup slam_launch.py \
  use_sim_time:=false \
  params_file:=./config/nav2_params.yaml

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10          # ms per BT tick
    default_server_timeout: 20
    # Use the default BT XML for point-to-point nav
    default_nav_to_pose_bt_xml: >
      /opt/ros/jazzy/share/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false          # Dijkstra by default; set true for A*
      allow_unknown: true

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.5
      lookahead_dist: 0.6
      min_lookahead_dist: 0.3
      max_lookahead_dist: 0.9
      use_velocity_scaled_lookahead_dist: true
      min_approach_linear_velocity: 0.05
      approach_velocity_scaling_dist: 1.0
      use_collision_detection: true
      max_allowed_time_to_collision_up_to_goal: 1.0

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      footprint: "[[0.25,0.25],[0.25,-0.25],[-0.25,-0.25],[-0.25,0.25]]"

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      resolution: 0.05
      track_unknown_space: true

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from nav2_msgs.action import NavigateToPose
from geometry_msgs.msg import PoseStamped
from builtin_interfaces.msg import Duration


class Nav2Client(Node):
    def __init__(self):
        super().__init__("nav2_client")
        self._client = ActionClient(self, NavigateToPose, "navigate_to_pose")

    def send_goal(self, x: float, y: float, yaw: float = 0.0):
        self._client.wait_for_server()

        goal = NavigateToPose.Goal()
        goal.pose = PoseStamped()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x = x
        goal.pose.pose.position.y = y
        # Simplified: set orientation via quaternion (yaw=0 → facing +X)
        import math
        goal.pose.pose.orientation.z = math.sin(yaw / 2)
        goal.pose.pose.orientation.w = math.cos(yaw / 2)

        self.get_logger().info(f"Navigating to ({x}, {y})")
        future = self._client.send_goal_async(
            goal,
            feedback_callback=self._feedback_callback
        )
        future.add_done_callback(self._goal_response_callback)

    def _goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error("Goal rejected!")
            return
        self.get_logger().info("Goal accepted")
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self._result_callback)

    def _result_callback(self, future):
        result = future.result().result
        self.get_logger().info("Navigation complete!")

    def _feedback_callback(self, feedback_msg):
        f = feedback_msg.feedback
        self.get_logger().info(
            f"Distance remaining: {f.distance_remaining:.2f} m"
        )


def main():
    rclpy.init()
    node = Nav2Client()
    node.send_goal(x=2.0, y=1.5, yaw=0.0)
    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == "__main__":
    main()