ROS 2 Nav2: Navigation hoàn chỉnh cho AMR

Nav2 là gì?

Nav2 (Navigation2) là navigation stack chính thức của ROS 2 -- bộ công cụ hoàn chỉnh để robot tự động di chuyển từ điểm A đến điểm B trong môi trường đã biết hoặc chưa biết. Nav2 thay thế navigation stack cũ của ROS 1 với kiến trúc hoàn toàn mới, linh hoạt và mạnh mẽ hơn.

Nếu bạn đang xây dựng AMR (Autonomous Mobile Robot) cho nhà máy, kho hàng, hay bất kỳ ứng dụng nào, Nav2 là starting point tốt nhất. Nó cung cấp tất cả những gì bạn cần: SLAM, localization, path planning, obstacle avoidance, và recovery behaviors.

Trong bài này, mình sẽ hướng dẫn bạn setup Nav2 từ đầu -- từ xây bản đồ đến navigate tự động.

Kiến trúc Nav2 Stack

Nav2 được thiết kế theo kiến trúc modular -- mỗi component là một ROS 2 node riêng biệt, giao tiếp qua topics, services, và actions. Bạn có thể thay thế bất kỳ component nào mà không ảnh hưởng đến phần còn lại.

Các component chính

                    ┌─────────────────────┐
                    │   BT Navigator      │  ← Behavior Tree orchestration
                    └────────┬────────────┘
                             │
              ┌──────────────┼──────────────┐
              │              │              │
    ┌─────────▼──────┐ ┌────▼─────┐ ┌─────▼────────┐
    │   Planner      │ │ Controller│ │  Recovery    │
    │   Server       │ │  Server  │ │  Server      │
    └─────────┬──────┘ └────┬─────┘ └─────┬────────┘
              │              │              │
    ┌─────────▼──────┐ ┌────▼─────┐ ┌─────▼────────┐
    │  NavFn/Smac/   │ │ DWB/MPPI/│ │ Spin/Backup/ │
    │  Theta*        │ │ RPP      │ │ Wait         │
    └────────────────┘ └──────────┘ └──────────────┘
              │              │
    ┌─────────▼──────────────▼─────┐
    │     Costmap 2D               │  ← Global + Local costmaps
    │  (static + obstacle layers)  │
    └──────────────────────────────┘

BT Navigator: điều phối toàn bộ quá trình navigation bằng Behavior Tree
Planner Server: tính toán global path từ vị trí hiện tại đến goal
Controller Server: theo dõi global path và tránh vật cản local
Recovery Server: xử lý khi robot bị kẹt (quay, lùi, chờ)
Costmap 2D: bản đồ chi phí cho path planning và obstacle avoidance

Bước 1: Xây bản đồ với slam_toolbox

Trước khi navigate, robot cần bản đồ. slam_toolbox là SLAM package chính thức của Nav2.

Cài đặt

# ROS 2 Humble
sudo apt install ros-humble-slam-toolbox
sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup

Chạy SLAM

# Terminal 1: Launch robot (hoặc simulation)
ros2 launch my_robot robot.launch.py

# Terminal 2: Launch slam_toolbox
ros2 launch slam_toolbox online_async_launch.py \
  use_sim_time:=true

# Terminal 3: Launch RViz2 để xem bản đồ
ros2 launch nav2_bringup rviz_launch.py

Điều khiển robot đi quanh môi trường (bằng teleop hoặc joystick). slam_toolbox sẽ tự động xây dựng occupancy grid map.

Lưu bản đồ

# Lưu bản đồ khi đã scan xong
ros2 run nav2_map_server map_saver_cli -f my_warehouse
# Tạo 2 file: my_warehouse.pgm (image) và my_warehouse.yaml (metadata)

my_warehouse.yaml:

image: my_warehouse.pgm
resolution: 0.05          # 5cm per pixel
origin: [-10.0, -10.0, 0] # Map origin (x, y, yaw)
occupied_thresh: 0.65
free_thresh: 0.25
negate: 0

Bước 2: Localization với AMCL

Khi đã có bản đồ, robot cần biết mình đang ở đâu. AMCL (Adaptive Monte Carlo Localization) sử dụng particle filter để estimate vị trí robot trên bản đồ.

# amcl config
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"
    max_particles: 2000
    min_particles: 500
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    tf_broadcast: true
    set_initial_pose: true
    initial_pose:
      x: 0.0
      y: 0.0
      yaw: 0.0

Bước 3: Path Planning -- NavFn vs Smac vs Theta*

Nav2 cung cấp nhiều planner plugins. Mỗi planner có đặc điểm riêng.

NavFn (Navigation Function)

Thuật toán: Dijkstra hoặc A*
Output: grid-based path
Ưu điểm: nhanh, đơn giản, reliable
Nhược điểm: path không smooth, chỉ 8-connected directions

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      tolerance: 0.5
      use_astar: true
      allow_unknown: true

Smac Planner (State Lattice)

Thuật toán: A* trên state lattice (x, y, theta)
Variants: Smac 2D, Smac Hybrid-A*, Smac Lattice
Ưu điểm: kinematically feasible paths, smooth, hỗ trợ non-holonomic robots
Tốt nhất cho: Ackermann steering, differential drive cần smooth paths

planner_server:
  ros__parameters:
    planner_plugins: ["SmacPlanner"]
    SmacPlanner:
      plugin: "nav2_smac_planner::SmacPlannerHybrid"
      tolerance: 0.25
      motion_model_for_search: "DUBIN"  # DUBIN, REEDS_SHEPP, STATE_LATTICE
      angle_quantization_bins: 72
      minimum_turning_radius: 0.40

Theta* (Any-Angle)

Thuật toán: Theta* -- A* với line-of-sight optimization
Ưu điểm: paths ngắn hơn NavFn, any-angle (không bị giới hạn 8 hướng)
Nhược điểm: không xét kinematics

Bảng so sánh Planners

Planner	Path Quality	Tốc độ	Kinematic Feasible	Use Case
NavFn (A*)	Trung bình	Nhanh nhất	Không	Prototype nhanh
Smac 2D	Tốt	Nhanh	Không	Omnidirectional
Smac Hybrid-A*	Rất tốt	Trung bình	Có	Diff-drive, Ackermann
Smac Lattice	Tốt nhất	Chậm nhất	Có (full)	Non-holonomic
Theta*	Tốt	Nhanh	Không	Any-angle paths

Bước 4: Controller -- Theo dõi path và tránh vật cản

Controller (trước đây gọi là local planner) chịu trách nhiệm theo dõi global path và tránh vật cản động.

DWB (Dynamic Window Based)

Kế thừa từ DWA (Dynamic Window Approach) của ROS 1. Sample các velocity commands và chọn command tốt nhất dựa trên nhiều critics.

MPPI (Model Predictive Path Integral)

Controller mới, mạnh nhất trong Nav2. Sử dụng sampling-based MPC -- generate hàng nghìn trajectory candidates trên GPU/CPU và chọn trajectory tối ưu.

controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      vx_max: 0.5
      vy_max: 0.0
      wz_max: 1.9
      critics:
        - "GoalCritic"
        - "GoalAngleCritic"
        - "ObstaclesCritic"
        - "PathFollowCritic"
        - "PathAngleCritic"
        - "PreferForwardCritic"

RPP (Regulated Pure Pursuit)

Đơn giản và hiệu quả -- theo dõi path bằng pure pursuit với regulated speed. Tốt cho robot di chuyển nhanh trong hành lang rộng.

Bước 5: Behavior Trees -- Bộ não của Nav2

Nav2 sử dụng Behavior Trees (BT) thay vì state machines để điều phối navigation. BT linh hoạt hơn, dễ debug, và có thể customize.

BT mặc định: NavigateToPose

<root BTCPP_format="4">
  <BehaviorTree ID="NavigateWithReplanning">
    <PipelineSequence>
      <!-- Compute path -->
      <RateController hz="1.0">
        <ComputePathToPose goal="{goal}" path="{path}"
                           planner_id="GridBased"/>
      </RateController>
      
      <!-- Follow path -->
      <ReactiveSequence>
        <PathExpiringTimer seconds="5.0" path="{path}"/>
        <FollowPath path="{path}" controller_id="FollowPath"/>
      </ReactiveSequence>
    </PipelineSequence>
  </BehaviorTree>
</root>

Custom BT: thêm recovery behaviors

<root BTCPP_format="4">
  <BehaviorTree ID="NavigateWithRecovery">
    <RecoveryNode number_of_retries="3">
      <PipelineSequence>
        <ComputePathToPose goal="{goal}" path="{path}"/>
        <FollowPath path="{path}"/>
      </PipelineSequence>
      
      <!-- Recovery actions khi thất bại -->
      <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"/>  <!-- Quay 90 độ -->
        <Wait wait_duration="3"/>
        <BackUp backup_dist="0.3" backup_speed="0.1"/>
      </SequenceStar>
    </RecoveryNode>
  </BehaviorTree>
</root>

Bước 6: Launch config hoàn chỉnh

# nav2_launch.py
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    nav2_dir = get_package_share_directory('nav2_bringup')
    
    return LaunchDescription([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(nav2_dir, 'launch', 'bringup_launch.py')
            ),
            launch_arguments={
                'map': '/path/to/my_warehouse.yaml',
                'params_file': '/path/to/nav2_params.yaml',
                'use_sim_time': 'true',
                'autostart': 'true',
            }.items(),
        ),
    ])

Send navigation goal từ code

# Python client để gửi goal
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator

navigator = BasicNavigator()
navigator.waitUntilNav2Active()

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

# Navigate!
navigator.goToPose(goal)

while not navigator.isTaskComplete():
    feedback = navigator.getFeedback()
    print(f"Distance remaining: {feedback.distance_remaining:.2f}m")

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

Tips và Best Practices

1. Costmap tuning

Costmap là yếu tố quan trọng nhất ảnh hưởng đến navigation quality. Một số tips:

inflation_radius: đặt bằng khoảng cách an toàn tối thiểu từ robot đến tường (thường 0.3-0.5m)
cost_scaling_factor: tăng để robot đi xa tường hơn, giảm để robot đi sát tường hơn
observation_sources: chỉ định rõ sensor nào dùng cho costmap (LiDAR, depth camera)

2. Recovery behavior

Luôn cấu hình recovery behaviors. Robot sẽ bị kẹt -- câu hỏi là khi nào, không phải có hay không.

3. Simulation trước, real robot sau

Test mọi config trong Gazebo trước khi deploy lên robot thật. Tham khảo Simulation cho Robotics để chọn simulator phù hợp.

4. Monitor và logging

# Xem trạng thái Nav2
ros2 topic echo /bt_navigator/transition_event

# Xem costmap
ros2 topic echo /global_costmap/costmap

# Record rosbag cho debugging
ros2 bag record -a -o nav2_debug

Tiếp theo trong series

Đây là Part 2 của series Navigation hiện đại. Trong các bài tiếp theo:

Part 1: SLAM từ A đến Z: LiDAR, Visual và cách robot định vị -- Nền tảng SLAM cho navigation
Part 3: Learning-based Navigation: GNM, ViNT và NoMaD -- Foundation models cho robot navigation
Part 4: Vision-Language Navigation: Robot đi theo chỉ dẫn -- VLN task và LLM-based planning
Part 5: Outdoor Navigation và Multi-Robot coordination -- GPS-denied nav, MAPF