Waypoint Follower: đi qua nhiều điểm liên tiếp, có hoặc không dừng lại

Mọi bài trước trong chuyên mục Nav2 đều nói về đi tới một điểm đích (NavigateToPose). Robot giao hàng thực tế thường cần ghé qua nhiều điểm liên tiếp trong một nhiệm vụ — lấy hàng ở điểm A, giao ở điểm B, quay về trạm sạc ở điểm C. Đây là vai trò của Waypoint Follower.

Khái niệm chính

Hai cách tiếp cận cho bài toán nhiều điểm:

Cách 1 — waypoint_follower gọi NavigateToPose lặp lại:
    đi tới điểm 1 (dừng hẳn) → đi tới điểm 2 (dừng hẳn) → ...
    mỗi điểm là một action riêng, hoàn thành xong mới bắt đầu điểm tiếp

Cách 2 — NavigateThroughPoses (action riêng của Nav2):
    lập kế hoạch đường đi liên tục QUA tất cả các điểm cùng lúc,
    không nhất thiết dừng hẳn tại mỗi điểm trung gian

Cách 1 phù hợp khi mỗi điểm cần một hành động thực sự dừng lại (lấy hàng, chờ xác nhận — giống mô hình dừng ở trạm của Atlas A2 trong phần showcase của trang này). Cách 2 phù hợp khi các điểm chỉ là "điểm phải đi qua" trên đường tới đích cuối, không cần dừng hẳn — đường đi mượt hơn, không có các lần dừng-khởi động không cần thiết.

Nguyên lý hoạt động

waypoint_follower:
  ros__parameters:
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      waypoint_pause_duration: 3000   # dừng 3 giây tại mỗi điểm

Plugin waypoint_task_executor cho phép gắn một hành động cụ thể tại mỗi điểm dừng — mặc định là dừng chờ một khoảng thời gian (WaitAtWaypoint), nhưng có thể tuỳ biến thành hành động nghiệp vụ thực tế (gửi tín hiệu "đã tới nơi" cho ứng dụng bên ngoài, chờ xác nhận thủ công — giống chế độ confirm đã nhắc trong nội dung dự án Atlas A2).

Về kiểu route: Sequential — đi hết danh sách điểm rồi dừng; Loop — đi hết danh sách rồi quay lại điểm đầu, lặp vô hạn; Confirm — dừng chờ xác nhận thủ công tại mỗi điểm trước khi đi tiếp. Ba kiểu route này không phải tính năng có sẵn "cắm là chạy" của nav2_waypoint_follower thuần — thường cần thêm một lớp logic ứng dụng (application layer, đã nhắc ở bài Application Layer trong AMR) quản lý danh sách waypoint và gọi waypoint_follower/NavigateThroughPoses theo đúng kiểu route mong muốn.

Tóm lại: NavigateToPose lặp lại qua waypoint_follower = nhiều nhiệm vụ độc lập nối tiếp nhau, mỗi nhiệm vụ hoàn thành trọn vẹn (dừng hẳn) trước khi bắt đầu nhiệm vụ sau. NavigateThroughPoses = một nhiệm vụ duy nhất với nhiều điểm mốc trung gian, đường đi liên tục không dừng.

Kiến thức kỹ thuật

Công thức

Thời gian hoàn thành tổng thể khác nhau đáng kể giữa 2 cách tiếp cận — mở rộng trực tiếp công thức ước lượng thời gian nhiệm vụ đã học ở bài Application Layer trong AMR:

Cách 1 (dừng hẳn mỗi điểm):
Thời_gian_tổng = Σ(Thời_gian_di_chuyển_i) + Σ(Thời_gian_dừng_khởi_động_i) + Σ(Thời_gian_thao_tác_i)

Cách 2 (liên tục qua các điểm):
Thời_gian_tổng = Thời_gian_di_chuyển_liên_tục_toàn_tuyến (không có dừng-khởi động giữa chừng)

VD: 3 điểm, mỗi lần dừng-khởi động tốn ~3 giây (giảm/tăng tốc)
Cách 1 tốn thêm ~3×3=9 giây so với Cách 2 chỉ vì dừng-khởi động,
chưa kể thời gian thao tác thực tế tại mỗi điểm (nếu có)

Thông số

	Cách 1 (waypoint_follower)	Cách 2 (NavigateThroughPoses)
Dừng tại mỗi điểm	Có, dừng hẳn	Không, đi liên tục
Phù hợp	Cần thao tác thực tế tại mỗi điểm	Điểm chỉ là mốc trung gian
Đường đi	Nhiều đoạn riêng biệt	Một đường liên tục mượt hơn

Ứng dụng thực tế

Huỷ giữa chừng theo đúng cơ chế huỷ action đã học ở bài ros2 action:

ros2 action send_goal /follow_waypoints nav2_msgs/action/FollowWaypoints "{...}" --feedback
# Ctrl+C để huỷ, giống cơ chế đã học ở bài ros2 action

FollowWaypoints là một action có feedback (điểm hiện tại đang đi tới, số điểm còn lại) và có thể huỷ giữa chừng bất kỳ lúc nào — quan trọng cho ứng dụng thực tế khi nhân viên kho cần can thiệp gọi robot quay lại giữa nhiệm vụ đang chạy, không cần đợi hoàn thành hết danh sách waypoint hiện tại.

Ví dụ thực tế

Kịch bản: robot giao hàng cần lấy hàng ở kệ A (dừng thao tác lấy hàng thật),
đi qua 2 điểm trung gian tránh khu vực đông người, rồi giao ở quầy B

Thiết kế đúng: kết hợp cả 2 cách
- Điểm lấy hàng (kệ A) và điểm giao hàng (quầy B): dùng waypoint_task_executor
  với hành động thực tế (chờ xác nhận lấy/giao hàng xong)
- 2 điểm trung gian tránh khu vực đông người: chỉ là điểm mốc đường đi,
  không cần dừng, nên nằm trong đoạn NavigateThroughPoses giữa kệ A và quầy B

Kết quả: robot dừng đúng 2 lần (lấy hàng, giao hàng), di chuyển mượt
qua các điểm trung gian mà không dừng-khởi động không cần thiết

Lỗi thường gặp

• Dùng Cách 1 (dừng hẳn mỗi điểm) cho toàn bộ tuyến khi chỉ 1-2 điểm cần thao tác thực tế — như ví dụ trên, tốn thêm thời gian dừng-khởi động không cần thiết ở các điểm chỉ là mốc trung gian.
• Dùng Cách 2 (liên tục) cho điểm cần thao tác thực tế (lấy/giao hàng) — robot không dừng đủ lâu để hoàn thành thao tác, cần waypoint_task_executor với hành động cụ thể tại đúng những điểm đó.
• Không xử lý huỷ giữa chừng đúng cách trong ứng dụng thực tế — bỏ qua cơ chế feedback/cancel của action FollowWaypoints, khiến người vận hành không thể can thiệp linh hoạt khi cần.
• Tự viết logic quản lý route Loop/Confirm mà không tận dụng feedback có sẵn của action — action FollowWaypoints đã cung cấp đủ thông tin (điểm hiện tại, số điểm còn lại) để xây logic ứng dụng phía trên, không cần tự đoán trạng thái robot bằng cách khác.

Kết luận

Waypoint Follower mở rộng Nav2 từ điều hướng 1 điểm sang nhiều điểm liên tiếp, với 2 cách tiếp cận phục vụ 2 nhu cầu khác nhau — dừng hẳn để thao tác thực tế, hay đi liên tục qua các điểm chỉ là mốc trung gian. Kết hợp cả hai đúng chỗ, cùng với tầng ứng dụng quản lý route Sequential/Loop/Confirm, là cách xây dựng nhiệm vụ giao hàng thực tế hoàn chỉnh. Đây là bài cuối của chuyên mục Nav2 — toàn bộ framework từ Planner, Controller, Costmap, Behavior Tree, Recovery, tới Waypoint Follower đã đầy đủ. Chuyên mục Tuning Nav2 tiếp theo đi vào cách tinh chỉnh từng tham số đã gặp xuyên suốt các bài trên cho phù hợp với robot và môi trường vận hành cụ thể.