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

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

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ống | Hà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âng | Không đi tiếp, báo MES "vị trí trống" |
| E-stop / protective stop | Tạ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 MES | Hoàn thành job đang chạy, không nhận job mới, về điểm đỗ |
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.