Nav2 là bộ điều hướng chuẩn của ROS 2: nhận một đích trên bản đồ, tự tìm đường, bám đường tránh vật cản, và tự phục hồi khi bị kẹt. Nav2 có rất nhiều tham số, nhưng nếu hiểu kiến trúc thì bạn biết chính xác cần chỉnh ở đâu khi xe hoạt động chưa như ý.

Sau bài này bạn sẽ:
  • Hiểu vai trò từng server trong Nav2 và luồng dữ liệu giữa chúng.
  • Đọc được Behavior Tree mặc định và biết cách tùy biến.
  • Khởi động Nav2 với bản đồ của bạn và điều khiển bằng Python.

1. Kiến trúc

Hình 1. Kiến trúc Nav2: BT Navigator nhận goal và gọi các server (planner, smoother, controller, behavior) qua action.
Hình 1. Kiến trúc Nav2: BT Navigator nhận goal và gọi các server (planner, smoother, controller, behavior) qua action.
ServerNhiệm vụPlugin phổ biến
planner_serverTìm đường toàn cục trên global costmapNavFn, Smac 2D, Smac Hybrid-A*, Smac Lattice, Theta*
controller_serverBám đường, tránh vật cản cục bộ, xuất vận tốcDWB, Regulated Pure Pursuit, MPPI
smoother_serverLàm mượt đường của plannerSimple Smoother, Constrained Smoother
behavior_serverHành vi phục hồiSpin, BackUp, DriveOnHeading, Wait
bt_navigatorĐiều phối tất cả bằng Behavior Treenavigate_to_pose, navigate_through_poses
velocity_smootherGiới hạn gia tốc lệnh cuối cùng—
lifecycle_managerBật/tắt các server theo đúng thứ tự—

Các server là lifecycle node: khởi động ở trạng thái unconfigured, được lifecycle_manager chuyển qua configure (đọc tham số, cấp phát) rồi activate (bắt đầu làm việc). Nếu một server lỗi khi configure, toàn bộ Nav2 không active – lỗi "Nav2 không nhận goal" thường bắt nguồn từ đây; đọc log đầu tiên của lifecycle_manager.

2. Behavior Tree

Hình 2. Behavior Tree mặc định rút gọn: lập đường lại định kỳ khi bám đường; thất bại thì chạy hành vi phục hồi và thử lại.
Hình 2. Behavior Tree mặc định rút gọn: lập đường lại định kỳ khi bám đường; thất bại thì chạy hành vi phục hồi và thử lại.

Thay vì viết cứng logic "lập đường → bám đường → nếu lỗi thì…", Nav2 mô tả bằng Behavior Tree (BT) trong file XML. Mỗi lần "tick", cây được duyệt từ gốc: node điều khiển (Sequence, Fallback, Recovery…) quyết định chạy con nào; node lá gọi action tới các server. Muốn đổi hành vi – ví dụ không xoay tại chỗ khi phục hồi vì xe chở hàng cao – chỉ cần sửa XML, không sửa C++.

<root BTCPP_format="4" main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="3" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
        </RateController>
        <FollowPath path="{path}" controller_id="FollowPath"/>
      </PipelineSequence>
      <Sequence name="RecoveryNoSpin">
        <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        <Wait wait_duration="5"/>
        <BackUp backup_dist="0.30" backup_speed="0.05"/>
      </Sequence>
    </RecoveryNode>
  </BehaviorTree>
</root>

Ví dụ trên dùng cú pháp BehaviorTree.CPP v4 (Nav2 từ bản Iron); trên Humble Nav2 dùng BT.CPP v3 nên thẻ gốc là <root main_tree_to_execute="MainTree"> và tên tham số của một số node hơi khác – luôn xuất phát từ file XML mẫu đi kèm đúng phiên bản Nav2 bạn cài (thư mục nav2_bt_navigator/behavior_trees).

3. Cấu hình plugin

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

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    controller_plugins: ["FollowPath"]
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"]
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    general_goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.10
      yaw_goal_tolerance: 0.10
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.6
      lookahead_dist: 0.8

Tên plugin phải khớp chính xác phiên bản: ví dụ trên Humble planner viết nav2_navfn_planner/NavfnPlanner, controller viết với ::. Sai một ký tự là server không configure được.

4. Khởi động và điều khiển

ros2 launch nav2_bringup bringup_launch.py \
  map:=$HOME/maps/kho_a.yaml \
  params_file:=$HOME/agv_ws/src/agv_navigation/config/nav2_params.yaml \
  use_sim_time:=false
ros2 launch nav2_bringup rviz_launch.py

Ngoài nút "Nav2 Goal" trên RViz, gói nav2_simple_commander cho phép viết kịch bản bằng Python rất gọn:

import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult


def pose(nav, x, y, z=0.0, w=1.0):
    p = PoseStamped()
    p.header.frame_id = 'map'
    p.header.stamp = nav.get_clock().now().to_msg()
    p.pose.position.x, p.pose.position.y = x, y
    p.pose.orientation.z, p.pose.orientation.w = z, w
    return p


def main():
    rclpy.init()
    nav = BasicNavigator()
    nav.waitUntilNav2Active()                     # chờ AMCL + các server active
    nav.goToPose(pose(nav, 6.5, 2.0))
    while not nav.isTaskComplete():
        fb = nav.getFeedback()
        if fb and fb.navigation_time.sec > 120:
            nav.cancelTask()                       # quá 2 phút thì hủy
    result = nav.getResult()
    print('Thanh cong' if result == TaskResult.SUCCEEDED else f'Ket qua: {result}')
    rclpy.shutdown()


if __name__ == '__main__':
    main()

5. Lỗi thường gặp

  • Goal bị từ chối ngay: Nav2 chưa active hoặc thiếu TF map → base_link.
  • "Failed to create plan": đích nằm trong vùng cost cao/vật cản, hoặc robot đang nằm trong vùng lethal do footprint khai báo quá lớn.
  • Xe xoay tại chỗ liên tục gần đích: yaw_goal_tolerance quá chặt so với khả năng điều khiển.
  • Progress checker hủy nhiệm vụ khi xe chờ người đi qua: tăng movement_time_allowance phù hợp quy trình nhà máy.

Tài liệu tham khảo