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ư ý.
- 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

| Server | Nhiệm vụ | Plugin phổ biến |
|---|---|---|
| planner_server | Tìm đường toàn cục trên global costmap | NavFn, Smac 2D, Smac Hybrid-A*, Smac Lattice, Theta* |
| controller_server | Bám đường, tránh vật cản cục bộ, xuất vận tốc | DWB, Regulated Pure Pursuit, MPPI |
| smoother_server | Làm mượt đường của planner | Simple Smoother, Constrained Smoother |
| behavior_server | Hành vi phục hồi | Spin, BackUp, DriveOnHeading, Wait |
| bt_navigator | Điều phối tất cả bằng Behavior Tree | navigate_to_pose, navigate_through_poses |
| velocity_smoother | Giới hạn gia tốc lệnh cuối cùng | — |
| lifecycle_manager | Bậ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

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_tolerancequá 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_allowancephù hợp quy trình nhà máy.