Nav2 trả lời câu hỏi "đi tới điểm X thế nào". Nhà máy cần nhiều hơn: nhận lệnh vận chuyển, đi lấy hàng, nâng, mang tới nơi giao, hạ, báo hoàn thành, tự đi sạc khi pin yếu, và xử lý tình huống bất thường. Đó là việc của mission manager – lớp ứng dụng nằm trên Nav2.

Sau bài này bạn sẽ:
  • Mô hình hóa nghiệp vụ AGV bằng máy trạng thái.
  • Quản lý trạm (station) bằng file cấu hình thay vì tọa độ rải trong code.
  • Viết mission manager bằng Python với Nav2 Simple Commander.

1. Máy trạng thái

Hình 1. Máy trạng thái của mission manager: chu trình lấy–giao hàng, đi sạc khi pin yếu và trạng thái lỗi.
Hình 1. Máy trạng thái của mission manager: chu trình lấy–giao hàng, đi sạc khi pin yếu và trạng thái lỗi.

Máy trạng thái hữu hạn (FSM) làm cho hành vi của xe rõ ràng và kiểm thử được: tại mỗi thời điểm xe ở đúng một trạng thái, chỉ những sự kiện định trước mới chuyển trạng thái. Khi có sự cố, nhìn trạng thái hiện tại và sự kiện cuối là biết chuyện gì xảy ra. Với logic phức tạp hơn có thể dùng Behavior Tree (giống Nav2) hoặc thư viện như SMACH/FlexBE, nhưng nguyên tắc giống nhau.

2. Định nghĩa trạm bằng YAML

# agv_mission/config/stations.yaml
stations:
  A3:      {x: 12.40, y: 3.10, yaw: 1.5708, type: pick}
  LINE2:   {x: 25.80, y: -4.20, yaw: 0.0, type: drop}
  DOCK1:   {x: 1.20, y: -0.50, yaw: 3.1416, type: charge}
  PARK1:   {x: 2.50, y: 1.50, yaw: 0.0, type: park}
battery:
  go_charge_below: 30.0
  resume_above: 90.0

Khi bố trí kho thay đổi, chỉ cần sửa file này (hoặc lấy từ cơ sở dữ liệu của WMS), không phải build lại.

3. Mission manager bằng Python

Hình 2. Trình tự một nhiệm vụ giữa MES, mission manager, Nav2 và cơ cấu nâng.
Hình 2. Trình tự một nhiệm vụ giữa MES, mission manager, Nav2 và cơ cấu nâng.
import math
import yaml
import rclpy
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import BatteryState
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult


class MissionManager:
    def __init__(self, cfg_file):
        self.nav = BasicNavigator()
        cfg = yaml.safe_load(open(cfg_file))
        self.st = cfg['stations']
        self.bat_cfg = cfg['battery']
        self.battery = 100.0
        self.state = 'IDLE'
        self.jobs = []                       # hàng đợi (pick, drop) – nhận từ MES/WMS
        self.nav.create_subscription(BatteryState, 'battery_state', self.on_bat, 10)

    def on_bat(self, msg):
        self.battery = msg.percentage * 100.0

    def pose(self, name):
        s = self.st[name]
        p = PoseStamped()
        p.header.frame_id = 'map'
        p.header.stamp = self.nav.get_clock().now().to_msg()
        p.pose.position.x, p.pose.position.y = s['x'], s['y']
        p.pose.orientation.z = math.sin(s['yaw'] / 2)
        p.pose.orientation.w = math.cos(s['yaw'] / 2)
        return p

    def go(self, name, timeout_s=300):
        self.nav.goToPose(self.pose(name))
        while not self.nav.isTaskComplete():
            rclpy.spin_once(self.nav, timeout_sec=0.1)
            fb = self.nav.getFeedback()
            if fb and fb.navigation_time.sec > timeout_s:
                self.nav.cancelTask()
        return self.nav.getResult() == TaskResult.SUCCEEDED

    def lift(self, up):
        self.nav.get_logger().info('Nang hang' if up else 'Ha hang')
        return True                          # gọi action/service của bộ điều khiển nâng

    def step(self):
        if self.state == 'IDLE':
            if self.battery < self.bat_cfg['go_charge_below']:
                self.state = 'GO_CHARGE'
            elif self.jobs:
                self.job = self.jobs.pop(0)
                self.state = 'GO_PICK'
        elif self.state == 'GO_PICK':
            self.state = 'PICK' if self.go(self.job[0]) else 'FAULT'
        elif self.state == 'PICK':
            self.state = 'GO_DROP' if self.lift(True) else 'FAULT'
        elif self.state == 'GO_DROP':
            self.state = 'DROP' if self.go(self.job[1]) else 'FAULT'
        elif self.state == 'DROP':
            self.state = 'IDLE' if self.lift(False) else 'FAULT'
        elif self.state == 'GO_CHARGE':
            self.state = 'CHARGING' if self.go('DOCK1') else 'FAULT'
        elif self.state == 'CHARGING':
            if self.battery > self.bat_cfg['resume_above']:
                self.state = 'IDLE'
        self.nav.get_logger().info(f'Trang thai: {self.state}', throttle_duration_sec=2.0)


def main():
    rclpy.init()
    mm = MissionManager('stations.yaml')
    mm.nav.waitUntilNav2Active()
    mm.jobs.append(('A3', 'LINE2'))
    while rclpy.ok() and mm.state != 'FAULT':
        mm.step()
        rclpy.spin_once(mm.nav, timeout_sec=0.1)
    mm.nav.get_logger().error('Dung o FAULT - cho nguoi van hanh xu ly')


if __name__ == '__main__':
    main()

Để gọn, ví dụ chạy tuần tự trong một vòng lặp. Bản thực tế nên: đưa trạng thái và job ra topic (TRANSIENT_LOCAL) cho dashboard, nhận job qua action server riêng (có hủy), lưu trạng thái ra đĩa để khởi động lại không mất job, và tách phần docking (bài #19) thành action riêng.

4. Xử lý tình huống bất thường

Tình huốngHành vi đề xuất
Nav2 thất bại (bị chặn, không lập được đường)Thử lại 1–2 lần sau khi chờ; vẫn lỗi → FAULT, báo vị trí và lý do
Pin yếu giữa nhiệm vụHoàn thành chặng hiện tại nếu đủ năng lượng (bài #20), nếu không về sạc và trả job về hàng đợi
Không phát hiện hàng khi nângKhông đi tiếp, báo MES "vị trí trống"
E-stop / protective stopTạm dừng FSM, không tính là lỗi điều hướng; tiếp tục khi người vận hành xác nhận
Mất kết nối MESHoàn thành job đang chạy, không nhận job mới, về điểm đỗ
Đi qua nhiều điểm: khi tuyến có các điểm trung gian bắt buộc (cửa, giao lộ), dùng goThroughPoses (action NavigateThroughPoses) để xe đi liền mạch không dừng ở từng điểm; followWaypoints thì dừng tại từng điểm và có thể chạy tác vụ (chụp ảnh, chờ) ở mỗi điểm.

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

  • Tọa độ trạm lấy từ bản đồ cũ: sau khi tạo lại bản đồ, gốc tọa độ thay đổi – mọi trạm phải đo lại.
  • Không có timeout: nhiệm vụ treo mãi khi Nav2 không trả kết quả.
  • Logic dàn trải trong nhiều callback: khó biết xe đang làm gì; gom về một FSM rõ ràng.
  • Không phân biệt "dừng an toàn" và "lỗi": người dùng bị báo lỗi liên tục mỗi khi có người đi qua.

Tài liệu tham khảo