ROS 2 từ A đến Z (Phần 3): Nav2 — Robot tự hành đầu tiên

Nav2 — Navigation Stack cho robot tự hành

Nếu bạn đang xây dựng AMR (Autonomous Mobile Robot), Nav2 là stack quan trọng nhất bạn cần nắm. Nav2 cung cấp toàn bộ pipeline từ lập bản đồ SLAM, định vị AMCL, lập đường đi, tránh vật cản, cho đến điều khiển tốc độ bánh xe. Trong bài này, chúng ta sẽ cấu hình Nav2 từ đầu, chạy simulation trong Gazebo, và gửi navigation goal để robot di chuyển tự động.

Bài này yêu cầu bạn đã nắm vững Phần 1: Cài đặt và Node đầu tiên và Phần 2: Topics, Services và Actions. Nếu bạn muốn hiểu lý thuyết SLAM trước, hãy đọc bài SLAM và Navigation cho robot tự hành.

Cài đặt Nav2 và dependencies

# Nav2 full stack
sudo apt install -y \
  ros-humble-navigation2 \
  ros-humble-nav2-bringup \
  ros-humble-slam-toolbox \
  ros-humble-turtlebot3-gazebo \
  ros-humble-turtlebot3-navigation2

# Gazebo simulation
sudo apt install -y ros-humble-gazebo-ros-pkgs

# Set TurtleBot3 model (dùng Waffle Pi — có LiDAR + camera)
echo "export TURTLEBOT3_MODEL=waffle_pi" >> ~/.bashrc
echo "export GAZEBO_MODEL_PATH=\$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models" >> ~/.bashrc
source ~/.bashrc

Kiến trúc Nav2

Nav2 là một hệ thống modular với nhiều node chạy đồng thời. Hiểu kiến trúc sẽ giúp bạn biết cần cấu hình gì:

                    ┌─────────────────┐
                    │   BT Navigator   │  ← Behavior Tree điều phối
                    └────────┬────────┘
                             │
              ┌──────────────┼──────────────┐
              │              │              │
    ┌─────────▼──────┐ ┌────▼────┐ ┌───────▼───────┐
    │    Planner     │ │Controller│ │   Recovery    │
    │  (NavFn/Smac)  │ │ (DWB)   │ │  (Spin/Wait)  │
    └─────────┬──────┘ └────┬────┘ └───────────────┘
              │              │
    ┌─────────▼──────────────▼──────┐
    │        Costmap 2D             │
    │  (Global + Local costmaps)    │
    └─────────┬─────────────────────┘
              │
    ┌─────────▼──────────────────────┐
    │   Sensor input (LiDAR, camera) │
    │   + TF transforms + Map        │
    └────────────────────────────────┘

Các thành phần chính:

Component	Vai trò	Default plugin
BT Navigator	Điều phối hành vi bằng Behavior Tree	NavigateToPose
Planner	Tính global path từ A đến B	NavfnPlanner
Controller	Bám theo path, tránh vật cản cục bộ	DWBLocalPlanner
Costmap	Bản đồ chi phí cho planner + controller	Costmap2D
Recovery	Xử lý khi bị kẹt (spin, backup, wait)	Spin, BackUp, Wait
AMCL	Định vị robot trên bản đồ đã có	AMCL particle filter

Bước 1: Chạy Gazebo Simulation

Trước khi chạy trên robot thật, luôn test trong simulation:

# Terminal 1: Launch Gazebo world
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

Gazebo sẽ mở với TurtleBot3 trong một thế giới có tường và vật cản. Kiểm tra các topic:

# Terminal 2: Xem topics
ros2 topic list | grep -E "(scan|odom|cmd_vel)"
# /scan           ← LiDAR data
# /odom           ← Odometry
# /cmd_vel        ← Velocity commands

# Test LiDAR
ros2 topic echo /scan --once

Bước 2: Lập bản đồ với SLAM Toolbox

SLAM (Simultaneous Localization and Mapping) cho phép robot vừa di chuyển vừa xây dựng bản đồ.

# Terminal 2: Launch SLAM Toolbox
ros2 launch slam_toolbox online_async_launch.py \
  slam_params_file:=/opt/ros/humble/share/slam_toolbox/config/mapper_params_online_async.yaml \
  use_sim_time:=true

Cấu hình SLAM Toolbox

Tạo file cấu hình tuỳ chỉnh:

# ~/ros2_ws/src/my_first_robot/config/slam_params.yaml
slam_toolbox:
  ros__parameters:
    # Solver params
    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

    # Matcher params (quan trọng cho accuracy)
    resolution: 0.05                # 5cm/pixel — cân bằng chi tiết và bộ nhớ
    max_laser_range: 12.0           # Range tối đa của LiDAR
    minimum_travel_distance: 0.3    # Di chuyển ít nhất 30cm mới update map
    minimum_travel_heading: 0.3     # Quay ít nhất 0.3 rad mới update

    # Map params
    map_update_interval: 3.0        # Update map mỗi 3 giây
    max_laser_range: 12.0
    min_laser_range: 0.1

    # Loop closure
    do_loop_closing: true
    loop_match_minimum_chain_size: 10

    # TF frames
    odom_frame: odom
    map_frame: map
    base_frame: base_footprint
    scan_topic: /scan

    use_sim_time: true

Lái robot để quét bản đồ

# Terminal 3: Điều khiển robot bằng bàn phím
ros2 run turtlebot3_teleop teleop_keyboard

Lái robot đi khắp phòng. Tips cho bản đồ tốt:

Di chuyển chậm (giảm tốc độ linear)
Đi theo tường trước, sau đó vào giữa phòng
Quay lại nơi đã đi để SLAM làm loop closure
Tránh quay nhanh — odometry sẽ drift

Visualize bản đồ trong RViz

# Terminal 4: Mở RViz
ros2 run rviz2 rviz2

Trong RViz, thêm các display:

Map — topic /map
LaserScan — topic /scan
TF — xem frame tree
RobotModel — xem vị trí robot

Lưu bản đồ

Khi bản đồ đủ tốt:

# Lưu bản đồ (tạo 2 file: .pgm và .yaml)
ros2 run nav2_map_server map_saver_cli -f ~/ros2_ws/maps/my_warehouse

# Kết quả:
# ~/ros2_ws/maps/my_warehouse.pgm    ← Ảnh bản đồ
# ~/ros2_ws/maps/my_warehouse.yaml   ← Metadata

Kiểm tra file yaml:

# ~/ros2_ws/maps/my_warehouse.yaml
image: my_warehouse.pgm
mode: trinary
resolution: 0.05          # 5cm/pixel
origin: [-5.0, -5.0, 0.0] # Góc dưới-trái của map
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

Bước 3: Cấu hình Nav2

File cấu hình Nav2 chính

Đây là file quan trọng nhất — cấu hình toàn bộ navigation stack:

# ~/ros2_ws/src/my_first_robot/config/nav2_params.yaml
amcl:
  ros__parameters:
    use_sim_time: true
    alpha1: 0.2           # Rotation noise from rotation
    alpha2: 0.2           # Rotation noise from translation
    alpha3: 0.2           # Translation noise from translation
    alpha4: 0.2           # Translation noise from rotation
    base_frame_id: "base_footprint"
    global_frame_id: "map"
    odom_frame_id: "odom"
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    max_particles: 2000
    min_particles: 500
    update_min_a: 0.2     # Min rotation to trigger update
    update_min_d: 0.25    # Min translation to trigger update
    laser_model_type: "likelihood_field"
    laser_max_range: 12.0
    scan_topic: /scan

bt_navigator:
  ros__parameters:
    use_sim_time: true
    global_frame: map
    robot_base_frame: base_footprint
    odom_topic: /odom
    # Behavior Tree mặc định — navigate to pose
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_rate_controller_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node

controller_server:
  ros__parameters:
    use_sim_time: true
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_theta_velocity_threshold: 0.001
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: true
      min_vel_x: 0.0
      min_vel_y: 0.0
      max_vel_x: 0.26       # Tốc độ tối đa (m/s) — giảm nếu robot chậm
      max_vel_y: 0.0
      max_vel_theta: 1.0    # Tốc độ quay tối đa (rad/s)
      min_speed_xy: 0.0
      max_speed_xy: 0.26
      min_speed_theta: 0.0
      acc_lim_x: 2.5
      acc_lim_y: 0.0
      acc_lim_theta: 3.2
      decel_lim_x: -2.5
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      xy_goal_tolerance: 0.25
      trans_stopped_velocity: 0.25
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]

planner_server:
  ros__parameters:
    use_sim_time: true
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false       # A* hoặc Dijkstra
      allow_unknown: true

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               # 3m x 3m local window
      height: 3
      resolution: 0.05
      robot_radius: 0.22     # Bán kính robot (m) — đo chính xác!
      plugins: ["voxel_layer", "inflation_layer"]
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          raytrace_max_range: 3.0
          obstacle_max_range: 2.5
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55   # Giữ khoảng cách an toàn

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: true
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_footprint
      robot_radius: 0.22
      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
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55

recoveries_server:
  ros__parameters:
    use_sim_time: true
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"

Giải thích các tham số quan trọng

robot_radius: Phải đo chính xác! Quá nhỏ thì robot va vào tường, quá lớn thì không đi qua cửa hẹp
inflation_radius: Vùng đệm an toàn quanh vật cản. Thường = robot_radius x 2
max_vel_x: Giới hạn tốc độ. Bắt đầu chậm (0.1-0.2) rồi tăng dần khi robot đã ổn định
xy_goal_tolerance: Sai số cho phép khi đến đích. 0.25m là OK cho AMR, robot arm cần nhỏ hơn

Bước 4: Launch Navigation

# Terminal 1: Gazebo
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

# Terminal 2: Nav2 với bản đồ đã lưu
ros2 launch nav2_bringup bringup_launch.py \
  use_sim_time:=true \
  map:=$HOME/ros2_ws/maps/my_warehouse.yaml \
  params_file:=$HOME/ros2_ws/src/my_first_robot/config/nav2_params.yaml

# Terminal 3: RViz với Nav2 config
ros2 launch nav2_bringup rviz_launch.py

Set initial pose trong RViz

Khi mới launch, AMCL chưa biết robot ở đâu. Trong RViz:

Click "2D Pose Estimate" trên toolbar
Click vào vị trí robot trên bản đồ, kéo để chỉ hướng
Đợi particle cloud hội tụ (vài giây)

Gửi Navigation Goal

Cách 1: RViz GUI

Click "Nav2 Goal" trên toolbar
Click vào điểm đích trên bản đồ
Robot sẽ tự lập đường đi và di chuyển

Cách 2: Python script

# ~/ros2_ws/src/my_first_robot/my_first_robot/send_nav_goal.py
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
from rclpy.duration import Duration


def main():
    rclpy.init()
    navigator = BasicNavigator()

    # Set initial pose
    initial_pose = PoseStamped()
    initial_pose.header.frame_id = 'map'
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    initial_pose.pose.position.x = 0.0
    initial_pose.pose.position.y = 0.0
    initial_pose.pose.orientation.w = 1.0
    navigator.setInitialPose(initial_pose)

    # Đợi Nav2 sẵn sàng
    navigator.waitUntilNav2Active()

    # Tạo goal
    goal_pose = PoseStamped()
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose.pose.position.x = 2.0
    goal_pose.pose.position.y = -1.5
    goal_pose.pose.orientation.w = 1.0

    # Gửi goal
    navigator.goToPose(goal_pose)

    # Theo dõi tiến trình
    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        if feedback:
            distance = feedback.distance_remaining
            print(f'Distance remaining: {distance:.2f}m')

        if Duration(seconds=60) < navigator.get_clock().now() - goal_pose.header.stamp:
            navigator.cancelTask()
            print('Navigation timeout!')
            break

    result = navigator.getResult()
    print(f'Navigation result: {result}')

    rclpy.shutdown()


if __name__ == '__main__':
    main()

Waypoint following — Đi qua nhiều điểm

# Gửi nhiều waypoints tuần tự
waypoints = []

for pt in [(1.0, 0.0), (2.0, -1.0), (3.0, 0.5), (0.0, 0.0)]:
    pose = PoseStamped()
    pose.header.frame_id = 'map'
    pose.header.stamp = navigator.get_clock().now().to_msg()
    pose.pose.position.x = pt[0]
    pose.pose.position.y = pt[1]
    pose.pose.orientation.w = 1.0
    waypoints.append(pose)

# Robot sẽ đi qua từng điểm
navigator.followWaypoints(waypoints)

while not navigator.isTaskComplete():
    feedback = navigator.getFeedback()
    if feedback:
        current_wp = feedback.current_waypoint
        print(f'Heading to waypoint {current_wp + 1}/{len(waypoints)}')

Behavior Trees — Não bộ của Nav2

Nav2 sử dụng Behavior Trees (BT) thay vì state machine để điều phối hành vi. BT linh hoạt hơn, dễ debug và modify:

<!-- Simplified Nav2 default BT -->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6">
      <PipelineSequence>
        <!-- 1. Tính global path -->
        <RateController hz="1.0">
          <ComputePathToPose goal="{goal}" path="{path}"
                             planner_id="GridBased"/>
        </RateController>
        <!-- 2. Bám theo path -->
        <FollowPath path="{path}" controller_id="FollowPath"/>
      </PipelineSequence>

      <!-- 3. Recovery nếu bị kẹt -->
      <SequenceStar>
        <ClearEntireCostmap name="ClearGlobalCostmap"
                            service_name="/global_costmap/clear_entirely_global_costmap"/>
        <ClearEntireCostmap name="ClearLocalCostmap"
                            service_name="/local_costmap/clear_entirely_local_costmap"/>
        <Spin spin_dist="1.57"/>
        <Wait wait_duration="5"/>
        <BackUp backup_dist="0.3" backup_speed="0.05"/>
      </SequenceStar>
    </RecoveryNode>
  </BehaviorTree>
</root>

Luồng hoạt động:

Tính global path (mỗi giây replan)
Controller bám theo path
Nếu bị kẹt -> clear costmap -> spin -> wait -> backup
Retry tối đa 6 lần

Troubleshooting Nav2 thường gặp

Robot không di chuyển

# Kiểm tra TF tree
ros2 run tf2_tools view_frames
# Phải có: map → odom → base_footprint → base_link

# Kiểm tra costmap
ros2 topic echo /local_costmap/costmap --once
# Nếu trống → LiDAR topic sai hoặc QoS mismatch

Robot va vào vật cản

Tăng inflation_radius (0.55 → 0.8)
Giảm max_vel_x (0.26 → 0.15)
Kiểm tra robot_radius có đúng không

AMCL không hội tụ

Initial pose sai quá xa → set lại chính xác hơn
Tăng max_particles (2000 → 5000)
Bản đồ đã cũ, environment thay đổi → quét lại SLAM

Tổng kết

Trong bài này, bạn đã:

Hiểu kiến trúc Nav2 stack: planner, controller, costmap, recovery
Lập bản đồ SLAM với slam_toolbox
Cấu hình Nav2 params chi tiết
Gửi navigation goal từ RViz và Python code
Sử dụng waypoint following cho patrol robot

Đây là nền tảng cho bất kỳ AMR nào — từ robot giao hàng trong nhà kho đến robot vệ sinh. Trong Phần 4, chúng ta sẽ đi vào tầng thấp nhất: kết nối ROS 2 với phần cứng thực qua ros2_control và hardware interface.