任务执行

2026-05-28 10:05:40 +08:00
parent fcf5649d8c
commit 6ec778dc6d
12 changed files with 2681 additions and 0 deletions
@@ -0,0 +1,650 @@
+# -*- coding: utf-8 -*-
+"""
+任务执行器 V3 — M×N 网格蛇形路径拍摄
+
+工作流：
+1. 根据 grid 生成蛇形路径（奇数行左→右，偶数行右→左）
+2. 逐台机器：
+   - 正面：导航 → 扫码(多姿态重试) → 查机型 → 按姿态拍照
+   - 背面：导航 → 按姿态拍照
+3. 回到 (0,0)
+"""
+import os
+import time
+import json
+import logging
+import threading
+import subprocess
+import math
+from typing import Optional, Dict, List
+from enum import Enum
+
+import requests
+import cv2
+import numpy as np
+
+logger = logging.getLogger(__name__)
+
+ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
+from config import ARM_CAMERA_CONFIG
+ARM_CAMERA_SNAPSHOT = ARM_CAMERA_CONFIG["snapshot_url"]
+PHOTOS_DIR = "/home/elephant/photos"
+
+# 二维码扫描重试参数
+QR_SCAN_TIMEOUT = 5  # 单次扫描超时
+QR_POSE_WAIT = 1.5   # 调整姿态后等待时间
+MANUAL_QR_TIMEOUT = 300  # 5分钟超时
+
+
+class MissionStatus(str, Enum):
+    IDLE = "idle"
+    RUNNING = "running"
+    PAUSED = "paused"
+    COMPLETED = "completed"
+    WAITING_QR = "waiting_qr"
+
+
+class MissionExecutorV3:
+    """任务执行器 V3 — M×N 网格蛇形路径"""
+
+    _instance = None  # 单例，供外部停止用
+
+    def __init__(self, config: dict):
+        self.config = config
+        self.status = MissionStatus.IDLE
+        MissionExecutorV3._instance = self
+
+        # 实时状态报告
+        self.report = {
+            "status": "idle",
+            "step": "",
+            "progress": 0,
+            "total": 0,
+            "log": [],
+            "error": None,
+        }
+
+        # 线程同步
+        self._stop = threading.Event()
+        self._pause = threading.Event()
+        self._pause.set()  # 初始不暂停
+        self._qr_event = threading.Event()
+        self._qr_value: Optional[str] = None
+
+        # 设备
+        from .arm_client import ArmClient
+        self.arm_client = ArmClient(
+            config["arm"]["host"],
+            config["arm"]["port"]
+        )
+
+        from .agv_controller_ros2 import AGVController
+        self.agv = AGVController(
+            device=config.get("device", "/dev/agvpro_controller"),
+            baudrate=config.get("baudrate", 1000000)
+        )
+
+    # ==================== 连接 ====================
+
+    def connect_all(self) -> Dict[str, bool]:
+        results = {}
+        results["agv"] = self.agv.connect()
+        results["arm"] = self.arm_client.connect()
+        return results
+
+    def disconnect_all(self):
+        if self.arm_client:
+            self.arm_client.close()
+        self.agv.disconnect()
+
+    # ==================== 主任务流程 ====================
+
+    def execute_mission(
+        self,
+        mission_config: dict,
+        machines: list,
+        qr_configs: list,
+        models: list,
+    ) -> dict:
+        """
+        执行完整拍摄任务。
+
+        Args:
+            mission_config: {rows, cols, grid, positions}
+            machines: [{id, row, col, front: {coords}, back: {coords}}]
+            qr_configs: [{id, name, joint_angles: [a1..a6]}]
+            models: [{id, name, poses: [{name, arm_angles}], poses_back: [...]}]
+
+        Returns:
+            执行报告 dict
+        """
+        self.status = MissionStatus.RUNNING
+        self.report = {"status": "running", "step": "初始化", "progress": 0, "total": 0, "log": [], "error": None}
+        self._stop.clear()
+        self._pause.set()
+
+        start_time = time.time()
+
+        try:
+            rows = int(mission_config.get("rows", 1))
+            cols = int(mission_config.get("cols", 1))
+            grid = mission_config.get("grid", [])
+            positions = mission_config.get("positions", [])
+
+            # 1. 生成蛇形路径
+            path = self._build_snake_path(rows, cols, grid)
+            if not path:
+                self._log("❌ 网格中没有机器，任务终止")
+                self.report["error"] = "No machines in grid"
+                return self._finish(0)
+
+            self.report["total"] = len(path)
+            self._log(f"📍 蛇形路径生成: {len(path)} 台机器")
+
+            # 初始化任务列表
+            self.report["tasks"] = [{
+                "row": r, "col": c,
+                "machine_id": f"m_{r}_{c}",
+                "label": f"{r+1}-{c+1}",
+                "status": "pending",
+                "step": "等待",
+                "qr_value": None,
+                "photos_front": 0,
+                "photos_back": 0,
+            } for (r, c) in path]
+
+            # 初始化任务列表
+            self.report["tasks"] = [{
+                "row": r, "col": c,
+                "machine_id": f"m_{r}_{c}",
+                "label": f"{r+1}-{c+1}",
+                "status": "pending",
+                "step": "等待",
+                "qr_value": None,
+                "photos_front": 0,
+                "photos_back": 0,
+            } for (r, c) in path]
+
+            # 2. 逐台执行
+            for idx, (r, c) in enumerate(path):
+                if self._stop.is_set():
+                    self._log("⏹️ 任务已停止")
+                    break
+
+                self._wait_pause()
+
+                # 更新任务状态 → 正面开始
+                task = self._get_task(r, c)
+                if task:
+                    task["status"] = "active"
+                    task["step"] = "正面扫码"
+
+                machine_id = f"m_{r}_{c}"
+                machine = next((m for m in machines if m.get("id") == machine_id), None)
+                if not machine:
+                    self._log(f"⚠️ 机器 {r+1}-{c+1} 不存在，跳过")
+                    continue
+
+                rl, cl = r + 1, c + 1  # 显示用的 1-based
+
+                # --- 正面 ---
+                self._log(f"📍 机器 {rl}-{cl} 进入正面点位")
+                self._step(f"机器 {rl}-{cl} 正面")
+
+                # 导航到正面点位
+                front_pt = self._find_point(positions, r, c, "front")
+                if front_pt and self._has_coords(front_pt):
+                    if not self._navigate(front_pt, "正面"):
+                        self._log(f"⚠️ 导航失败，尝试继续")
+                else:
+                    self._log(f"⚠️ 无正面点位坐标")
+
+                # 扫描二维码
+                qr_value = self._scan_qr_with_poses(qr_configs)
+                if self._stop.is_set():
+                    break
+
+                # 查机型 + 更新任务步骤
+                model_name = self._lookup_model(qr_value)
+                self._log(f"  🏷️ 机型: {model_name}")
+                if task and qr_value:
+                    task["qr_value"] = qr_value
+                if task:
+                    task["step"] = "正面拍照"
+
+                # 正面拍照
+                model = self._find_model(models, model_name)
+                if model:
+                    self._shoot(model, "front", rl, cl, qr_value or "unknown")
+                else:
+                    self._log(f"  ⚠️ 未找到机型 {model_name}")
+
+                self._progress(idx, 1)
+
+                # --- 背面 ---
+                if task:
+                    task["step"] = "背面拍照"
+                self._log(f"📍 机器 {rl}-{cl} 进入背面点位")
+                self._step(f"机器 {rl}-{cl} 背面")
+
+                # 导航到背面点位
+                back_pt = self._find_point(positions, r + 1, c, "back")
+                if back_pt and self._has_coords(back_pt):
+                    if not self._navigate(back_pt, "背面"):
+                        self._log(f"⚠️ 导航失败，尝试继续")
+                else:
+                    self._log(f"⚠️ 无背面点位坐标")
+
+                # 背面拍照
+                if model:
+                    self._shoot(model, "back", rl, cl, qr_value or "unknown")
+
+                # 标记任务完成
+                if task:
+                    task["status"] = "completed"
+                    task["step"] = "完成"
+                self._progress(idx, 2)
+
+            # 3. 回到出发点
+            if not self._stop.is_set():
+                self._step("返回出发点")
+                self._log("→ 返回 (0, 0)")
+                self._nav2_go_to_point(0, 0, 0, timeout_sec=60)
+
+            elapsed = time.time() - start_time
+            return self._finish(elapsed)
+
+        except Exception as e:
+            self._log(f"❌ 任务异常: {e}")
+            logger.exception("execute_mission 崩溃")
+            self.report["error"] = str(e)
+            self.status = MissionStatus.IDLE
+            self.report["status"] = "idle"
+            return self.report
+
+    def _finish(self, elapsed: float) -> dict:
+        if self._stop.is_set():
+            self._step("已停止")
+        else:
+            self._step("完成")
+            self._log(f"✅ 任务完成 ({elapsed:.0f}s)")
+        self.report["progress"] = 100
+        self.status = MissionStatus.IDLE
+        self.report["status"] = "idle"
+        return self.report
+
+    # ==================== 蛇形路径 ====================
+
+    def _build_snake_path(self, rows: int, cols: int, grid: list) -> list:
+        """奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左"""
+        path = []
+        for r in range(rows):
+            if r % 2 == 0:
+                for c in range(cols):
+                    if self._has_machine(grid, r, c):
+                        path.append((r, c))
+            else:
+                for c in range(cols - 1, -1, -1):
+                    if self._has_machine(grid, r, c):
+                        path.append((r, c))
+        return path
+
+    @staticmethod
+    def _has_machine(grid: list, r: int, c: int) -> bool:
+        if not grid or r >= len(grid):
+            return False
+        row = grid[r]
+        if isinstance(row, list):
+            return c < len(row) and bool(row[c])
+        return False
+
+    # ==================== 点位查找 ====================
+
+    @staticmethod
+    def _find_point(positions: list, row: int, col: int, side: str) -> Optional[dict]:
+        for p in positions:
+            if p.get("row") == row and p.get("col") == col and p.get("side") == side:
+                return p
+        return None
+
+    @staticmethod
+    def _has_coords(point: dict) -> bool:
+        coords = point.get("coords", [])
+        return len(coords) >= 2 and (coords[0] != 0 or coords[1] != 0)
+
+    # ==================== 导航 ====================
+
+    def _navigate(self, point: dict, label: str) -> bool:
+        coords = point["coords"]
+        x, y = float(coords[0]), float(coords[1])
+        yaw = float(coords[2]) if len(coords) >= 3 else 0.0
+        self._log(f"  🧭 导航到{label}点位 ({x:.2f}, {y:.2f}, yaw={math.degrees(yaw):.0f}°)")
+        return self._nav2_go_to_point(x, y, yaw)
+
+    # ==================== 二维码扫描 ====================
+
+    def _scan_qr_with_poses(self, qr_configs: list) -> Optional[str]:
+        """用二维码配置中的姿态依次尝试"""
+        if not qr_configs:
+            self._log(f"  ⚠️ 无二维码配置")
+            return self._request_manual_qr()
+
+        self._log(f"  🔍 尝试 {len(qr_configs)} 个二维码姿态...")
+        for i, qc in enumerate(qr_configs):
+            if self._stop.is_set():
+                return None
+            self._wait_pause()
+
+            angles = qc.get("joint_angles", [])
+            if not angles or len(angles) < 6:
+                continue
+
+            name = qc.get("name", f"姿态{i+1}")
+            self._log(f"  [{i+1}/{len(qr_configs)}] {name}")
+
+            # 调整机械臂
+            if self.arm_client:
+                self.arm_client.set_angles(angles, speed=500)
+                time.sleep(QR_POSE_WAIT)
+
+            # 扫码
+            qr = self._decode_qr_from_arm()
+            if qr:
+                self._log(f"  ✅ 识别成功: {qr}")
+                return qr
+
+            time.sleep(0.3)
+
+        self._log(f"  ⚠️ 全部 {len(qr_configs)} 个姿态均未识别到二维码")
+        return self._request_manual_qr()
+
+    def _decode_qr_from_arm(self) -> Optional[str]:
+        """从机械臂摄像头取一帧，识别二维码"""
+        for attempt in range(3):
+            try:
+                resp = requests.get(ARM_CAMERA_SNAPSHOT, timeout=QR_SCAN_TIMEOUT)
+                if resp.status_code != 200 or not resp.content:
+                    continue
+
+                arr = np.frombuffer(resp.content, dtype=np.uint8)
+                frame = cv2.imdecode(arr, cv2.IMREAD_COLOR)
+                if frame is None:
+                    continue
+
+                detector = cv2.QRCodeDetector()
+                data, bbox, _ = detector.detectAndDecode(frame)
+                if data and len(data) > 0:
+                    return data
+            except Exception:
+                pass
+            time.sleep(0.5)
+        return None
+
+    def _request_manual_qr(self) -> Optional[str]:
+        """暂停任务，等待手动输入"""
+        self.status = MissionStatus.WAITING_QR
+        self.report["status"] = "waiting_qr"
+        self.report["step"] = "等待手动输入二维码"
+        self._log("  ⌨️  弹窗等待手动输入二维码...")
+
+        self._qr_event.clear()
+        if self._qr_event.wait(timeout=MANUAL_QR_TIMEOUT):
+            self.status = MissionStatus.RUNNING
+            self.report["status"] = "running"
+            self._log(f"  ✏️ 手动输入: {self._qr_value}")
+            return self._qr_value
+        else:
+            self.status = MissionStatus.RUNNING
+            self.report["status"] = "running"
+            self._log(f"  ⚠️ 等待超时（{MANUAL_QR_TIMEOUT}s），跳过")
+            return None
+
+    def set_manual_qr(self, value: str):
+        self._qr_value = value.strip()
+        self._qr_event.set()
+
+    # ==================== 机型查询 ====================
+
+    def _lookup_model(self, qr_value: Optional[str]) -> str:
+        """TODO: 后续通过 HTTP 接口查询机型"""
+        return "机器1"
+
+    @staticmethod
+    def _find_model(models: list, name: str) -> Optional[dict]:
+        """在机型列表中找到匹配的机型"""
+        for m in models:
+            if m.get("name") == name or m.get("id") == name:
+                return m
+        # fallback: 第一个机型
+        return models[0] if models else None
+
+    # ==================== 姿态拍照 ====================
+
+    def _shoot(self, model: dict, side: str, row: int, col: int, qr_value: str):
+        """按机型配置的所有姿态依次拍照"""
+        # 更新任务照片计数
+        task = self._get_task(row - 1, col - 1)
+        side_label = "正面" if side == "front" else "背面"
+        poses = model.get("poses", []) if side == "front" else model.get("poses_back", [])
+        if not poses:
+            self._log(f"  ⚠️ 机型无{side_label}姿态配置")
+            return
+
+        self._log(f"  📷 {side_label}拍照 ({len(poses)} 个姿态)")
+        for pi, pose in enumerate(poses):
+            if self._stop.is_set():
+                break
+            self._wait_pause()
+
+            angles = pose.get("arm_angles", [])
+            if not angles or len(angles) < 6:
+                self._log(f"  跳过 {pose.get('name', f'姿态{pi+1}')}: 无效角度")
+                continue
+
+            name = pose.get("name", f"{side_label}-{pi+1}")
+            self._log(f"  🎯 {name}")
+
+            # 调整机械臂
+            if self.arm_client:
+                self.arm_client.set_angles(angles, speed=500)
+                time.sleep(QR_POSE_WAIT)
+
+            # 拍照
+            path = self._capture_arm_photo(row, col, side, pi + 1, qr_value)
+            if path:
+                self._log(f"  💾 {os.path.basename(path)}")
+
+    def _capture_arm_photo(self, row: int, col: int, side: str,
+                           pose_idx: int, qr_value: str) -> Optional[str]:
+        """从机械臂摄像头拍照存本地"""
+        try:
+            resp = requests.get(ARM_CAMERA_SNAPSHOT, timeout=10)
+            if resp.status_code != 200 or not resp.content:
+                logger.error("arm snapshot 请求失败")
+                return None
+
+            os.makedirs(PHOTOS_DIR, exist_ok=True)
+            ts = time.strftime("%Y%m%d_%H%M%S")
+            fname = f"{ts}_r{row}c{col}_{side}_p{pose_idx}_{qr_value[:20]}.jpg"
+            fpath = os.path.join(PHOTOS_DIR, fname)
+            with open(fpath, "wb") as f:
+                f.write(resp.content)
+            return fpath
+        except Exception as e:
+            logger.error(f"拍照异常: {e}")
+            return None
+
+    # ==================== 控制 ====================
+
+    def _wait_pause(self):
+        """等待暂停状态解除"""
+        self._pause.wait()
+
+    def pause(self):
+        self._pause.clear()
+        self.status = MissionStatus.PAUSED
+        self.report["status"] = "paused"
+        self.report["step"] = "已暂停"
+        self._log("⏸️ 任务已暂停")
+
+    def resume(self):
+        self._pause.set()
+        self.status = MissionStatus.RUNNING
+        self.report["status"] = "running"
+        self._log("▶️ 任务已恢复")
+
+    def stop(self):
+        self._stop.set()
+        self._pause.set()  # 解除暂停
+        self._qr_event.set()  # 解除 QR 等待
+        if self.arm_client:
+            try:
+                self.arm_client.task_stop()
+            except Exception:
+                pass
+        self.agv.stop()
+        self.status = MissionStatus.IDLE
+        self.report["status"] = "idle"
+
+    def get_status(self) -> dict:
+        return {
+            "status": self.report["status"],
+            "step": self.report["step"],
+            "progress": self.report["progress"],
+            "total": self.report["total"],
+        }
+
+    def get_logs(self) -> dict:
+        """返回实时日志和完整状态"""
+        return self.report
+
+    # ==================== 状态报告 ====================
+
+    def _log(self, msg: str):
+        self.report["log"].append(msg)
+        # Keep last 500 entries
+        if len(self.report["log"]) > 500:
+            self.report["log"] = self.report["log"][-500:]
+        logger.info(msg)
+
+    def _step(self, text: str):
+        self.report["step"] = text
+
+    def _get_task(self, row: int, col: int) -> Optional[dict]:
+        """获取指定行列的任务记录"""
+        for t in self.report.get("tasks", []):
+            if t["row"] == row and t["col"] == col:
+                return t
+        return None
+
+    def _progress(self, machine_idx: int, side_code: int):
+        """side_code: 1=正面完成, 2=背面完成"""
+        if self.report["total"]:
+            self.report["progress"] = min(
+                int((machine_idx * 2 + side_code) / (self.report["total"] * 2) * 100),
+                99
+            )
+
+    # ==================== Nav2 导航 ====================
+    # (保留原实现)
+
+    def _nav2_check_available(self) -> bool:
+        try:
+            rc, out, err = self._run_ros2_cmd("ros2 action list")
+            if rc != 0:
+                return False
+            return "/navigate_to_pose" in out
+        except:
+            return False
+
+    def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0,
+                          timeout_sec: float = 120.0) -> bool:
+        if not self._nav2_check_available():
+            logger.error("Nav2 action server 不可用")
+            return False
+
+        qz = math.sin(yaw / 2.0)
+        qw = math.cos(yaw / 2.0)
+
+        pose_yaml = (
+            f"pose:\n"
+            f"  header:\n"
+            f"    stamp:\n"
+            f"      sec: 0\n"
+            f"      nanosec: 0\n"
+            f"    frame_id: 'map'\n"
+            f"  pose:\n"
+            f"    position:\n"
+            f"      x: {x}\n"
+            f"      y: {y}\n"
+            f"      z: 0.0\n"
+            f"    orientation:\n"
+            f"      x: 0.0\n"
+            f"      y: 0.0\n"
+            f"      z: {qz}\n"
+            f"      w: {qw}"
+        )
+
+        cmd = (
+            f"ros2 action send_goal /navigate_to_pose "
+            f"nav2_msgs/action/NavigateToPose "
+            f"'{pose_yaml}' --feedback"
+        )
+        full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
+
+        try:
+            process = subprocess.Popen(
+                full_cmd, shell=True,
+                stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
+            )
+            succeeded = False
+            elapsed = 0.0
+            while elapsed < timeout_sec:
+                import select
+                reads = [process.stdout]
+                ready, _, _ = select.select(reads, [], [], 1.0)
+                elapsed += 1.0
+                if process.stdout in ready:
+                    line = process.stdout.readline()
+                    if not line:
+                        break
+                    ls = line.strip()
+                    if "succeeded" in ls.lower():
+                        succeeded = True
+                        break
+                    elif "failed" in ls.lower() or "aborted" in ls.lower():
+                        break
+                    elif "canceled" in ls.lower() or "cancelled" in ls.lower():
+                        break
+                if process.poll() is not None:
+                    break
+            if process.poll() is None:
+                process.terminate()
+                try:
+                    process.wait(timeout=3)
+                except subprocess.TimeoutExpired:
+                    process.kill()
+            return succeeded
+        except Exception as e:
+            logger.error(f"Nav2 异常: {e}")
+            return False
+
+    def _nav2_cancel(self):
+        cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose 2>/dev/null || true'"
+        try:
+            subprocess.run(cancel_cmd, shell=True, timeout=3)
+        except:
+            pass
+
+    def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple:
+        full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
+        try:
+            result = subprocess.run(
+                full_cmd, shell=True,
+                capture_output=True, text=True, timeout=timeout
+            )
+            return result.returncode, result.stdout.strip(), result.stderr.strip()
+        except subprocess.TimeoutExpired:
+            return -1, "", "Timeout"
+        except Exception as e:
+            return -1, "", str(e)
@@ -0,0 +1,122 @@
+<!DOCTYPE html>
+<html lang="zh-CN">
+<head>
+  <meta charset="UTF-8">
+  <meta name="viewport" content="width=device-width, initial-scale=1.0">
+  <title>运行监控 - AGV 拍摄系统</title>
+  <link rel="stylesheet" href="/static/css/style.css">
+</head>
+<body>
+  <div id="app">
+    <header class="topbar">
+      <div class="logo">▶️ 任务运行</div>
+      <nav class="nav">
+        <a href="/" class="nav-link">🏠 首页</a>
+        <a href="/setting" class="nav-link">⚙️ 设置</a>
+        <a href="/running" class="nav-link active">▶️ 运行</a>
+      </nav>
+    </header>
+
+    <main class="container">
+      <!-- 状态概览 -->
+      <section class="card">
+        <div class="running-header">
+          <div class="running-status" :class="missionState">
+            <span class="pulse"></span>
+            [[ missionStateText ]]
+          </div>
+          <div class="running-progress" v-if="missionState === 'running' || missionState === 'waiting_qr'">
+            <span>进度 [[ Math.round(progress) ]]%</span>
+            <div class="progress-bar">
+              <div class="progress-fill" :style="{width: progress + '%'}"></div>
+            </div>
+          </div>
+        </div>
+        <div class="btn-row">
+          <button class="btn btn-success btn-large" @click="startMission" :disabled="missionState !== 'idle'">
+            ▶️ 开始任务
+          </button>
+          <button class="btn btn-warning btn-large" @click="pauseMission" :disabled="missionState !== 'running'">
+            ⏸️ 暂停
+          </button>
+          <button class="btn btn-primary btn-large" @click="resumeMission" :disabled="missionState !== 'paused'">
+            ▶️ 继续
+          </button>
+          <button class="btn btn-error btn-large" @click="stopMission" :disabled="missionState === 'idle'">
+            ⏹️ 停止
+          </button>
+        </div>
+      </section>
+
+      <!-- 任务清单 -->
+      <section class="card" v-if="tasks.length > 0">
+        <h2>📋 任务清单 ([[ tasks.length ]] 台机器)</h2>
+        <div class="task-grid">
+          <div v-for="task in tasks" :key="task.machine_id"
+               class="task-cell" :class="'task-' + task.status"
+               :title="task.step">
+            <div class="task-pos">[[ task.label ]]</div>
+            <div class="task-status-icon">
+              <span v-if="task.status === 'pending'">⏳</span>
+              <span v-else-if="task.status === 'active'" class="pulse-icon">🔄</span>
+              <span v-else-if="task.status === 'completed'">✅</span>
+              <span v-else>❓</span>
+            </div>
+            <div class="task-step-text">[[ task.step ]]</div>
+            <div class="task-info">
+              <div v-if="task.qr_value" class="task-qr">🏷 [[ task.qr_value.substring(0,8) ]]</div>
+              <div class="task-photos" v-if="task.photos_front || task.photos_back">
+                📷 [[ task.photos_front ]]正 [[ task.photos_back ]]背
+              </div>
+            </div>
+          </div>
+        </div>
+      </section>
+
+      <!-- 实时日志 -->
+      <section class="card" v-if="missionState === 'running' || missionState === 'waiting_qr'">
+        <h2>📜 实时日志</h2>
+        <div class="log-box" ref="logBox">
+          <div v-for="(log, i) in logs" :key="i" class="log-line">[[ log ]]</div>
+          <div v-if="logs.length === 0" class="log-empty">等待任务开始...</div>
+        </div>
+      </section>
+
+      <!-- 实时预览 -->
+      <section class="card">
+        <h2>📷 摄像头预览</h2>
+        <div class="camera-full">
+          <img :src="previewUrl" @error="onPreviewError">
+        </div>
+      </section>
+
+      <!-- 任务报告 -->
+      <section class="card" v-if="report">
+        <h2>📊 任务报告</h2>
+        <div class="report-summary">
+          <div class="stat ok">✅ 完成: [[ report.completed ]]</div>
+          <div class="stat error">❌ 失败: [[ report.failed ]]</div>
+          <div class="stat">总计: [[ report.total_points ]]</div>
+        </div>
+      </section>
+
+      <!-- 手动输入二维码弹窗 -->
+      <div class="modal-overlay" v-if="showQrModal">
+        <div class="modal">
+          <h3>⌨️ 手动输入二维码</h3>
+          <p>所有姿态均未识别到二维码，请手动输入：</p>
+          <input type="text" v-model="qrValue" placeholder="输入二维码内容" autofocus @keyup.enter="submitQr">
+          <div class="modal-actions">
+            <button class="btn btn-primary" @click="submitQr">确认</button>
+            <button class="btn" @click="cancelQr">跳过</button>
+          </div>
+        </div>
+      </div>
+
+    </main>
+  </div>
+
+  <script src="/static/js/vue3.global.prod.js"></script>
+  <script src="/static/js/running.js"></script>
+</body>
+</html>
@@ -0,0 +1,130 @@
+const { createApp } = Vue
+const API = ''
+
+createApp({
+  delimiters: ['[[', ']]'],
+  
+  data() {
+    return {
+      missionState: 'idle',
+      progress: 0,
+      tasks: [],
+      report: null,
+      previewUrl: API + '/api/camera/preview',
+      polling: null,
+      logs: [],
+      showQrModal: false,
+      qrValue: '',
+    }
+  },
+  computed: {
+    missionStateText() {
+      const map = {
+        idle: '空闲',
+        running: '任务运行中',
+        paused: '已暂停',
+        completed: '已完成',
+        waiting_qr: '等待输入二维码'
+      }
+      return map[this.missionState] || '未知'
+    },
+  },
+  mounted() {
+    this.poll()
+  },
+  beforeUnmount() {
+    if (this.polling) clearInterval(this.polling)
+  },
+  methods: {
+    poll() {
+      this.refresh()
+      this.pollLogs()
+      this.polling = setInterval(() => {
+        this.refresh()
+        this.pollLogs()
+      }, 2000)
+    },
+    async refresh() {
+      try {
+        const res = await fetch(API + '/api/mission/state')
+        const data = await res.json()
+        this.missionState = data.status || 'idle'
+        this.progress = data.progress || 0
+        if (data.tasks) this.tasks = data.tasks
+
+        // QR 弹窗
+        if (this.missionState === 'waiting_qr' && !this.showQrModal) {
+          this.showQrModal = true
+          this.qrValue = ''
+        }
+
+        // 完成后获取报告
+        if (this.missionState === 'idle' && this.tasks.length > 0) {
+          const reportRes = await fetch(API + '/api/mission/report')
+          const reportData = await reportRes.json()
+          this.report = reportData.report
+        }
+      } catch (e) {}
+    },
+    async pollLogs() {
+      if (this.missionState !== 'running' && this.missionState !== 'waiting_qr') return
+      try {
+        const res = await fetch(API + '/api/mission/log')
+        const data = await res.json()
+        if (data.log) this.logs = data.log
+        if (data.progress != null) this.progress = data.progress
+        if (data.tasks) this.tasks = data.tasks
+        // 自动滚到底
+        this.$nextTick(() => {
+          const box = this.$refs.logBox
+          if (box) box.scrollTop = box.scrollHeight
+        })
+      } catch (e) {}
+    },
+    async startMission() {
+      if (this.missionState !== 'idle') return
+      this.logs = []
+      this.progress = 0
+      this.report = null
+      this.showQrModal = false
+      await fetch(API + '/api/mission/start', { method: 'POST' })
+      this.missionState = 'running'
+    },
+    async pauseMission() {
+      await fetch(API + '/api/mission/pause', { method: 'POST' })
+      this.missionState = 'paused'
+    },
+    async resumeMission() {
+      await fetch(API + '/api/mission/resume', { method: 'POST' })
+      this.missionState = 'running'
+      this.showQrModal = false
+    },
+    async stopMission() {
+      await fetch(API + '/api/mission/stop', { method: 'POST' })
+      this.missionState = 'idle'
+      this.showQrModal = false
+    },
+    async submitQr() {
+      const val = this.qrValue.trim()
+      await fetch(API + '/api/mission/manual-qr', {
+        method: 'POST',
+        headers: { 'Content-Type': 'application/json' },
+        body: JSON.stringify({ qr: val || ' ' })
+      })
+      this.showQrModal = false
+      this.qrValue = ''
+    },
+    cancelQr() {
+      this.showQrModal = false
+      this.qrValue = ''
+      fetch(API + '/api/mission/manual-qr', {
+        method: 'POST',
+        headers: { 'Content-Type': 'application/json' },
+        body: JSON.stringify({ qr: 'SKIP' })
+      })
+    },
+    onPreviewError(e) {
+      e.target.style.display = 'none'
+    }
+  }
+}).mount('#app')
@@ -0,0 +1,67 @@
+#!/usr/bin/env python3
+"""修复激光雷达时间戳偏移的修正器 v5"""
+import os, sys, rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import LaserScan
+from builtin_interfaces.msg import Time
+
+LOCKFILE = "/tmp/scan_fixer.lock"
+
+if os.path.exists(LOCKFILE):
+    with open(LOCKFILE) as f:
+        old_pid = int(f.read().strip())
+    try:
+        os.kill(old_pid, 0)
+        print(f"Another fixer running PID {old_pid}, exit.", file=sys.stderr)
+        sys.exit(1)
+    except (OSError, ProcessLookupError):
+        print(f"Stale lock removed (PID {old_pid} dead)", file=sys.stderr)
+
+with open(LOCKFILE, "w") as f:
+    f.write(str(os.getpid()))
+
+def main():
+    rclpy.init(args=sys.argv[1:])
+    node = Node('scan_timestamp_fixer')
+    offset = 2.0
+    pub = node.create_publisher(LaserScan, '/scan_corrected', 10)
+    count = [0]
+
+    def cb(msg: LaserScan):
+        count[0] += 1
+        s, ns = msg.header.stamp.sec, msg.header.stamp.nanosec
+        s2 = s - int(offset)
+        ns2 = ns - int((offset % 1) * 1e9)
+        if ns2 < 0:
+            ns2 += 1000000000
+            s2 -= 1
+        out = LaserScan()
+        out.header.frame_id = msg.header.frame_id
+        out.header.stamp = Time(sec=s2, nanosec=ns2)
+        out.angle_min = msg.angle_min
+        out.angle_max = msg.angle_max
+        out.angle_increment = msg.angle_increment
+        out.time_increment = msg.time_increment
+        out.scan_time = msg.scan_time
+        out.range_min = msg.range_min
+        out.range_max = msg.range_max
+        out.ranges = msg.ranges
+        out.intensities = msg.intensities
+        pub.publish(out)
+        if count[0] % 200 == 0:
+            node.get_logger().info(f'#{count[0]} /scan={s} -> /scan_corrected={s2}')
+
+    node.create_subscription(LaserScan, '/scan', cb, 10)
+    node.get_logger().info(f'Fixer PID={os.getpid()}, offset={offset}s')
+
+    try:
+        while rclpy.ok():
+            rclpy.spin_once(node, timeout_sec=0.5)
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        if os.path.exists(LOCKFILE):
+            os.unlink(LOCKFILE)
+
+if __name__ == '__main__':
+    main()
@@ -0,0 +1,17 @@
+#!/bin/bash
+source /opt/ros/humble/setup.bash
+source /home/elephant/agv_pro_ros2/install/setup.bash
+export ROS_DOMAIN_ID=1
+cd /home/elephant/agv_pro_ros2
+nohup ros2 daemon start >/dev/null 2>&1 &
+sleep 5
+nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 &
+sleep 8
+nohup python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py > /tmp/scan_fixer.log 2>&1 &
+sleep 5
+nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py autostart:=True > /tmp/ros2_nav2.log 2>&1 &
+sleep 15
+cd /home/elephant/work/agv_app && nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
+sleep 5
+echo "ALL_STARTED"
+ps aux | grep -E 'lslidar|agv_pro_node|nav2_container|scan_timestamp_fixer|ros2-daemon|app.py' | grep -v grep
@@ -0,0 +1,17 @@
+#!/bin/bash
+source /opt/ros/humble/setup.bash
+source /home/elephant/agv_pro_ros2/install/setup.bash
+export ROS_DOMAIN_ID=1
+cd /home/elephant/agv_pro_ros2
+nohup ros2 daemon start >/dev/null 2>&1 &
+sleep 5
+nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 &
+sleep 8
+nohup python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py > /tmp/scan_fixer.log 2>&1 &
+sleep 5
+nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py autostart:=True > /tmp/ros2_nav2.log 2>&1 &
+sleep 15
+cd /home/elephant/work/agv_app && nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
+sleep 5
+echo "ALL_STARTED"
+ps aux | grep -E 'lslidar|agv_pro_node|nav2_container|scan_timestamp_fixer|ros2-daemon|app.py' | grep -v grep
@@ -0,0 +1,177 @@
+#!/bin/bash
+# ============================================================
+# Robot AGV 全量启动脚本 v2.6
+# 修复：
+#   - 清理 scan_fixer lock 文件防残留
+#   - Nav2 节点检测 grep -c 改为单行输出
+#   - nohup 启动 Nav2 用 bash -c 包裹（确保 source 环境）
+# ============================================================
+set -e
+
+AGV_APP_DIR="/home/elephant/work/agv_app"
+AGV_ROS2_DIR="/home/elephant/agv_pro_ros2"
+ROS_DOMAIN_ID_VAL=1
+
+echo "=========================================="
+echo "  Robot AGV 全量启动 v2.6"
+echo "=========================================="
+echo ""
+
+# ---------- 1. 清理旧进程（不杀 ros2-daemon） ----------
+echo "[1/7] 清理旧进程..."
+pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true
+pkill -f "ros2 launch agv_pro_navigation2" 2>/dev/null || true
+pkill -f "agv_pro_node" 2>/dev/null || true
+pkill -f "lslidar_driver_node" 2>/dev/null || true
+pkill -f "fix_scan_timestamp" 2>/dev/null || true
+pkill -f "python.*app.py" 2>/dev/null || true
+sleep 4
+
+# 清理 scan_fixer 锁文件（防残留 PID 导致启动失败）
+rm -f /tmp/scan_fixer.lock
+
+echo "   清理完成"
+
+# ---------- 2. 重启 ros2 daemon ----------
+echo "[2/7] 重启 ros2 daemon..."
+pkill -f "ros2-daemon" 2>/dev/null || true
+sleep 2
+nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 &
+sleep 5
+echo "   ros2 daemon 已就绪"
+
+# ---------- 3. 启动 bringup (含激光雷达) ----------
+echo "[3/7] 启动 AGV Bringup..."
+source /opt/ros/humble/setup.bash
+cd "$AGV_ROS2_DIR"
+source install/setup.bash
+nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py \
+    port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 &
+BRINGUP_PID=$!
+echo "   bringup PID: $BRINGUP_PID"
+
+echo "   等待 bringup 就绪..."
+for i in $(seq 1 20); do
+    if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/odom'; then
+        echo "   ✅ bringup 已就绪"
+        break
+    fi
+    sleep 2
+done
+
+# ---------- 4. 启动激光时间戳修正节点 ----------
+echo "[4/7] 启动激光时间戳修正节点..."
+pkill -f "fix_scan_timestamp" 2>/dev/null || true
+sleep 2
+
+# 确保 /scan 存在
+for i in $(seq 1 10); do
+    if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan'; then
+        echo "   /scan 话题已上线"
+        break
+    fi
+    sleep 2
+done
+
+nohup bash -c "source /opt/ros/humble/setup.bash && \
+    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \
+    > /tmp/scan_fixer.log 2>&1 &
+FIXER_PID=$!
+echo "   fix_scan_timestamp PID: $FIXER_PID"
+sleep 5
+
+# 验证 fixer 进程和 scan_corrected
+FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0)
+if [ "$FIXER_COUNT" -gt 1 ]; then
+    echo "   ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..."
+    pkill -f "fix_scan_timestamp" 2>/dev/null || true
+    sleep 2
+    rm -f /tmp/scan_fixer.lock
+    nohup bash -c "source /opt/ros/humble/setup.bash && \
+        ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \
+        > /tmp/scan_fixer.log 2>&1 &
+    FIXER_PID=$!
+    sleep 3
+fi
+
+if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan_corrected'; then
+    echo "   ✅ /scan_corrected 已上线"
+else
+    echo "   ⚠️ /scan_corrected 未上线，检查日志："
+    tail -5 /tmp/scan_fixer.log
+fi
+
+# ---------- 5. 启动 Nav2 ----------
+echo "[5/7] 启动 Nav2 导航..."
+source /opt/ros/humble/setup.bash
+cd "$AGV_ROS2_DIR"
+source install/setup.bash
+# 使用 bash -c 确保 source 环境变量传递到 nohup
+nohup bash -c "source /opt/ros/humble/setup.bash && \
+    source /home/elephant/agv_pro_ros2/install/setup.bash && \
+    export ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL && \
+    ros2 launch agv_pro_navigation2 navigation2_active.launch.py \
+    autostart:=True" > /tmp/ros2_nav2.log 2>&1 &
+NAV2_PID=$!
+echo "   Nav2 PID: $NAV2_PID"
+sleep 12
+
+echo "   等待 Nav2 节点就绪..."
+for i in $(seq 1 20); do
+    NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \
+        grep -cE 'lifecycle_manager_navigation|bt_navigator|controller_server' 2>/dev/null || echo 0)
+    # 去除可能的换行符，确保是单个数字
+    NODES=$(echo "$NODES" | tr -d '\n' | awk '{print $1}')
+    if [ "$NODES" -ge 3 ] 2>/dev/null; then
+        echo "   ✅ Nav2 节点已就绪 ($NODES 个)"
+        break
+    fi
+    sleep 3
+done
+
+# ---------- 6. 设置精度参数 ----------
+echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..."
+source /opt/ros/humble/setup.bash
+cd "$AGV_ROS2_DIR"
+source install/setup.bash
+
+for NODE in /controller_server /bt_navigator /planner_server; do
+    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
+    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true
+done
+ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true
+ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true
+ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL timeout 1 ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true
+echo "   精度参数已设置"
+
+# ---------- 7. 启动 Flask ----------
+echo "[7/7] 启动 Flask API..."
+cd "$AGV_APP_DIR"
+nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
+FLASK_PID=$!
+echo "   Flask PID: $FLASK_PID"
+sleep 4
+
+# ---------- 完成 ----------
+echo ""
+echo "=========================================="
+echo "  ✅ 启动完成"
+echo "=========================================="
+echo ""
+echo "  进程状态:"
+for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do
+    NAME="${PROC%%:*}"
+    PID="${PROC##*:}"
+    echo "    $NAME : $(ps aux | grep -w "$PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
+done
+echo ""
+echo "  日志文件:"
+echo "    bringup  : /tmp/ros2_bringup.log"
+echo "    Nav2     : /tmp/ros2_nav2.log"
+echo "    fixer    : /tmp/scan_fixer.log"
+echo "    Flask    : /tmp/agv_flask.log"
+echo ""
+echo "  关键验证命令:"
+echo "    curl http://localhost:5000/api/navigate/status"
+echo "    ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once"
+echo "    ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once"
@@ -0,0 +1,165 @@
+#!/bin/bash
+# ============================================================
+# Robot AGV 全量启动脚本 v2.2
+# 完整流程：
+#   清理旧进程（不杀 daemon） -> 启动 bringup ->
+#   启动激光时间戳修正节点 -> 启动 Nav2 ->
+#   设置导航精度参数 -> 启动 Flask
+# ============================================================
+set -e
+
+AGV_APP_DIR="/home/elephant/work/agv_app"
+AGV_ROS2_DIR="/home/elephant/agv_pro_ros2"
+ROS_DOMAIN_ID_VAL=1
+
+echo "=========================================="
+echo "  Robot AGV 全量启动 v2.2"
+echo "=========================================="
+echo ""
+
+# ---------- 1. 清理旧进程（不杀 ros2-daemon） ----------
+echo "[1/7] 清理旧进程..."
+pkill -f "ros2 launch agv_pro_bringup" 2>/dev/null || true
+pkill -f "ros2 launch agv_pro_navigation2" 2>/dev/null || true
+pkill -f "agv_pro_node" 2>/dev/null || true
+pkill -f "lslidar_driver_node" 2>/dev/null || true
+pkill -f "scan_timestamp_fixer" 2>/dev/null || true
+pkill -f "python.*app.py" 2>/dev/null || true
+sleep 4
+echo "   清理完成"
+
+# ---------- 2. 重启 ros2 daemon（仅杀 daemon进程本身，不杀整个环境） ----------
+echo "[2/7] 重启 ros2 daemon..."
+pkill -f "ros2-daemon" 2>/dev/null || true
+sleep 2
+nohup bash -c "source /opt/ros/humble/setup.bash && ros2 daemon start" >/dev/null 2>&1 &
+sleep 5
+echo "   ros2 daemon 已就绪"
+
+# ---------- 3. 启动 bringup (含激光雷达) ----------
+echo "[3/7] 启动 AGV Bringup..."
+source /opt/ros/humble/setup.bash
+cd "$AGV_ROS2_DIR"
+source install/setup.bash
+nohup ros2 launch agv_pro_bringup agv_pro_bringup.launch.py \
+    port_name:=/dev/agvpro_controller > /tmp/ros2_bringup.log 2>&1 &
+BRINGUP_PID=$!
+echo "   bringup PID: $BRINGUP_PID"
+
+echo "   等待 bringup 就绪..."
+for i in $(seq 1 20); do
+    if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/odom'; then
+        echo "   ✅ bringup 已就绪"
+        break
+    fi
+    sleep 2
+done
+
+# ---------- 4. 启动激光时间戳修正节点（单例，不重复启动） ----------
+echo "[4/7] 启动激光时间戳修正节点..."
+# 确保只有1个 fixer 进程在运行
+pkill -f "scan_timestamp_fixer" 2>/dev/null || true
+sleep 2
+
+for i in $(seq 1 10); do
+    if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan'; then
+        echo "   /scan 话题已上线"
+        break
+    fi
+    sleep 2
+done
+
+nohup bash -c "source /opt/ros/humble/setup.bash && \
+    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \
+    > /tmp/scan_fixer.log 2>&1 &
+FIXER_PID=$!
+echo "   scan_timestamp_fixer PID: $FIXER_PID"
+sleep 5
+
+# 验证只有1个 fixer 进程
+FIXER_COUNT=$(ps aux | grep -c "[f]ix_scan_timestamp" 2>/dev/null || echo 0)
+if [ "$FIXER_COUNT" -gt 1 ]; then
+    echo "   ⚠️ 发现 $FIXER_COUNT 个 fixer 进程，杀掉多余的..."
+    pkill -f "scan_timestamp_fixer" 2>/dev/null || true
+    sleep 2
+    nohup bash -c "source /opt/ros/humble/setup.bash && \
+        ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL python3 /home/elephant/work/scan_fixer/fix_scan_timestamp.py" \
+        > /tmp/scan_fixer.log 2>&1 &
+    sleep 3
+fi
+
+if ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 topic list 2>/dev/null | grep -q '/scan_corrected'; then
+    echo "   ✅ /scan_corrected 已上线"
+else
+    echo "   ⚠️ /scan_corrected 未上线，检查日志："
+    cat /tmp/scan_fixer.log
+fi
+
+# ---------- 5. 启动 Nav2 ----------
+echo "[5/7] 启动 Nav2 导航..."
+source /opt/ros/humble/setup.bash
+cd "$AGV_ROS2_DIR"
+source install/setup.bash
+nohup ros2 launch agv_pro_navigation2 navigation2_active.launch.py \
+    autostart:=True > /tmp/ros2_nav2.log 2>&1 &
+NAV2_PID=$!
+echo "   Nav2 PID: $NAV2_PID"
+sleep 12
+
+echo "   等待 Nav2 节点就绪..."
+for i in $(seq 1 15); do
+    NODES=$(ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 node list 2>/dev/null | \
+        grep -c "lifecycle_manager_navigation\|bt_navigator\|controller_server" 2>/dev/null || echo 0)
+    if [ "$NODES" -ge 3 ]; then
+        echo "   ✅ Nav2 节点已就绪 ($NODES 个)"
+        break
+    fi
+    sleep 3
+done
+
+# ---------- 6. 设置精度参数 ----------
+echo "[6/7] 设置导航精度参数 (xy_goal_tolerance=0.05m)..."
+source /opt/ros/humble/setup.bash
+cd "$AGV_ROS2_DIR"
+source install/setup.bash
+
+for NODE in /controller_server /bt_navigator /planner_server; do
+    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.xy_goal_tolerance 0.05 2>/dev/null || true
+    ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set $NODE general_goal_checker.yaw_goal_tolerance 0.05 2>/dev/null || true
+done
+ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server FollowPath.xy_goal_tolerance 0.05 2>/dev/null || true
+ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server general_goal_checker.stateful True 2>/dev/null || true
+ROS_DOMAIN_ID=$ROS_DOMAIN_ID_VAL ros2 param set /controller_server FollowPath.stateful True 2>/dev/null || true
+echo "   精度参数已设置"
+
+# ---------- 7. 启动 Flask ----------
+echo "[7/7] 启动 Flask API..."
+cd "$AGV_APP_DIR"
+nohup python3 app.py > /tmp/agv_flask.log 2>&1 &
+FLASK_PID=$!
+echo "   Flask PID: $FLASK_PID"
+sleep 4
+
+# ---------- 完成 ----------
+echo ""
+echo "=========================================="
+echo "  ✅ 启动完成"
+echo "=========================================="
+echo ""
+echo "  进程状态:"
+for PROC in "bringup:$BRINGUP_PID" "Nav2:$NAV2_PID" "fixer:$FIXER_PID" "Flask:$FLASK_PID"; do
+    NAME="${PROC%%:*}"
+    PID="${PROC##*:}"
+    echo "    $NAME : $(ps aux | grep -w "$PID" | grep -v grep | awk '{print $2}' || echo '已退出')"
+done
+echo ""
+echo "  日志文件:"
+echo "    bringup  : /tmp/ros2_bringup.log"
+echo "    Nav2     : /tmp/ros2_nav2.log"
+echo "    fixer    : /tmp/scan_fixer.log"
+echo "    Flask    : /tmp/agv_flask.log"
+echo ""
+echo "  关键验证命令:"
+echo "    curl http://localhost:5000/api/navigate/status"
+echo "    ROS_DOMAIN_ID=1 ros2 topic echo /scan_corrected --once"
+echo "    ROS_DOMAIN_ID=1 ros2 topic echo /amcl_pose --once"
@@ -0,0 +1,350 @@
+amcl:
+  ros__parameters:
+    use_sim_time: True
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 5.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan_corrected
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: True
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    wait_for_service_timeout: 1000
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_assisted_teleop_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_goal_updated_controller_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_assisted_teleop_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+      - nav2_is_battery_charging_condition_bt_node
+
+bt_navigator_navigate_through_poses_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+bt_navigator_navigate_to_pose_rclcpp_node:
+  ros__parameters:
+    use_sim_time: True
+
+controller_server:
+  ros__parameters:
+    use_sim_time: True
+    controller_frequency: 20.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 0.3
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+    # Goal checker parameters
+    #precise_goal_checker:
+    #  plugin: "nav2_controller::SimpleGoalChecker"
+    #  xy_goal_tolerance: 0.05
+    #  yaw_goal_tolerance: 0.05
+    #  stateful: True
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.05
+      yaw_goal_tolerance: 0.05
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 3.2
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -3.2
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 20
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 5.0
+      xy_goal_tolerance: 0.05
+      trans_stopped_velocity: 0.25
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: True
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan_corrected
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan_corrected
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+
+map_server:
+  ros__parameters:
+    use_sim_time: True
+    # Overridden in launch by the "map" launch configuration or provided default value.
+    # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
+    yaml_filename: ""
+
+map_saver:
+  ros__parameters:
+    use_sim_time: True
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: True
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+    drive_on_heading:
+      plugin: "nav2_behaviors/DriveOnHeading"
+    wait:
+      plugin: "nav2_behaviors/Wait"
+    assisted_teleop:
+      plugin: "nav2_behaviors/AssistedTeleop"
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 5.0
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: True
+
+waypoint_follower:
+  ros__parameters:
+    use_sim_time: True
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: True
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.26, 0.0, 1.0]
+    min_velocity: [-0.26, 0.0, -1.0]
+    max_accel: [2.5, 0.0, 3.2]
+    max_decel: [-2.5, 0.0, -3.2]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0
@@ -0,0 +1,362 @@
+amcl:
+  ros__parameters:
+    use_sim_time: False
+    alpha1: 0.4
+    alpha2: 0.3
+    alpha3: 0.1
+    alpha4: 0.1
+    alpha5: 0.04
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 2
+    robot_model_type: "nav2_amcl::OmniMotionModel"
+    scan_topic: scan_corrected
+    save_pose_rate: 0.5
+    sigma_hit: 0.02
+    tf_broadcast: true
+    transform_tolerance: 5.0
+    update_min_a: 0.06
+    update_min_d: 0.025
+    z_hit: 0.7
+    z_max: 0.001
+    z_rand: 0.059
+    z_short: 0.24
+
+    # Initial Pose
+    set_initial_pose: True
+    initial_pose.x: 0.0
+    initial_pose.y: 0.0
+    initial_pose.z: 0.0
+    initial_pose.yaw: 0.0
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: False
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    enable_groot_monitoring: True
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
+    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
+    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
+    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_goal_updated_controller_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 10.0
+    min_x_velocity_threshold: 0.001
+    min_y_velocity_threshold: 0.5
+    min_theta_velocity_threshold: 0.001
+    failure_tolerance: 3.0
+    progress_checker_plugin: "progress_checker"
+    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.15
+      movement_time_allowance: 8.0
+    # Goal checker parameters
+    #precise_goal_checker:
+    #  plugin: "nav2_controller::SimpleGoalChecker"
+    #  xy_goal_tolerance: 0.05
+    #  yaw_goal_tolerance: 0.05
+    #  stateful: True
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.05
+      yaw_goal_tolerance: 0.05
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.26
+      max_vel_y: 0.0
+      max_vel_theta: 0.5
+      min_speed_xy: 0.0
+      max_speed_xy: 0.26
+      min_speed_theta: 0.0
+      # Add high threshold velocity for turtlebot 3 issue.
+      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+      acc_lim_x: 2.5
+      acc_lim_y: 0.0
+      acc_lim_theta: 0.5
+      decel_lim_x: -2.5
+      decel_lim_y: 0.0
+      decel_lim_theta: -0.5
+      vx_samples: 20
+      vy_samples: 5
+      vtheta_samples: 40
+      sim_time: 1.7
+      linear_granularity: 0.05
+      angular_granularity: 0.025
+      transform_tolerance: 5.0
+      xy_goal_tolerance: 0.05
+      trans_stopped_velocity: 0.1
+      short_circuit_trajectory_evaluation: True
+      stateful: True
+      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+      BaseObstacle.scale: 0.02
+      PathAlign.scale: 23.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 18.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.0
+      GoalDist.scale: 24.0
+      RotateToGoal.scale: 32.0
+      RotateToGoal.slowing_factor: 5.0
+      RotateToGoal.lookahead_time: -1.0
+      
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]"
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 4.0
+        inflation_radius: 0.30
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan_corrected_corrected
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_footprint
+      use_sim_time: False
+      footprint: "[[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]]"
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan_corrected_corrected
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 4.0
+        inflation_radius: 0.30
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_server:
+  ros__parameters:
+    use_sim_time: False
+    # Overridden in launch by the "map" launch configuration or provided default value.
+    # To use in yaml, remove the default "map" value in the navigation2_active.launch.py file & provide full path to map below.
+    yaml_filename: ""
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 1.0
+    use_sim_time: False
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 2.0
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: True
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    recovery_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_recoveries/Spin"
+    backup:
+      plugin: "nav2_recoveries/BackUp"
+    wait:
+      plugin: "nav2_recoveries/Wait"
+    global_frame: odom
+    robot_base_frame: base_footprint
+    transform_timeout: 0.1
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"   
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200
+
+velocity_smoother:
+  ros__parameters:
+    use_sim_time: True
+    smoothing_frequency: 20.0
+    scale_velocities: False
+    feedback: "OPEN_LOOP"
+    max_velocity: [0.26, 0.0, 0.5]
+    min_velocity: [-0.26, 0.0, -0.5]
+    max_accel: [2.5, 0.0, 0.5]
+    max_decel: [-2.5, 0.0, -0.5]
+    odom_topic: "odom"
+    odom_duration: 0.1
+    deadband_velocity: [0.0, 0.0, 0.0]
+    velocity_timeout: 1.0
@@ -0,0 +1,624 @@
+#include "agv_pro_base/agv_pro_driver.h"
+
+uint16_t AGV_PRO::crc16_ibm(const uint8_t* data, size_t length) {
+  uint16_t crc = 0xFFFF;
+  for (size_t i = 0; i < length; ++i) {
+    crc ^= static_cast<uint16_t>(data[i]);
+    for (int j = 0; j < 8; ++j) {
+      if (crc & 0x0001)
+        crc = (crc >> 1) ^ 0xA001;
+      else
+        crc = crc >> 1;
+    }
+  }
+  return crc;
+}
+
+std::vector<uint8_t> AGV_PRO::build_serial_frame(uint8_t cmd_id, const std::vector<uint8_t>& payload)
+{
+  std::vector<uint8_t> frame(SEND_DATA_SIZE, 0x00);
+  frame[0] = 0xFE;
+  frame[1] = 0xFE;
+  frame[2] = 0x0B;
+  frame[3] = cmd_id;
+
+  for (size_t i = 0; i < payload.size() && i < 8; ++i) {
+    frame[4 + i] = payload[i];
+  }
+
+  uint16_t crc = crc16_ibm(frame.data(), 12);
+  frame[12] = (crc >> 8) & 0xff;
+  frame[13] = crc & 0xff;
+
+  return frame;
+}
+
+void AGV_PRO::print_hex(const std::string& label, const std::vector<uint8_t>& data, std::optional<size_t> override_size) {
+  std::stringstream ss;
+  for (auto b : data) {
+    ss << std::hex << std::uppercase << std::setfill('0') << std::setw(2)
+       << static_cast<int>(b) << " ";
+  }
+  size_t len = override_size.value_or(data.size());
+  RCLCPP_INFO(this->get_logger(), "%s (%zu bytes): [%s]", label.c_str(), len, ss.str().c_str());
+}
+
+void AGV_PRO::send_serial_frame(const std::vector<uint8_t>& frame, bool debug)
+{
+  try {
+    size_t bytes_transmit_size = boost::asio::write(*serial_port_, boost::asio::buffer(frame));
+    if (debug) {
+      print_hex("Sent", frame, bytes_transmit_size);
+    }
+  } catch (const std::exception &ex) {
+    RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port: %s", ex.what());
+  }
+}
+
+std::vector<uint8_t> AGV_PRO::read_serial_response(
+  const std::vector<uint8_t>& expected_header,
+  size_t payload_size,
+  double timeout_sec)
+{
+  std::vector<uint8_t> sliding_buf;
+  uint8_t byte = 0;
+
+  rclcpp::Time start_time = this->now();
+  rclcpp::Duration timeout = rclcpp::Duration::from_seconds(timeout_sec);
+
+  while ((this->now() - start_time) < timeout) {
+    boost::asio::mutable_buffers_1 buf(&byte, 1);
+    boost::system::error_code ec;
+    size_t n = serial_port_->read_some(buf, ec);
+    if (ec) {
+        RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+        return {};
+    }
+    if (n == 1) {
+      sliding_buf.push_back(byte);
+      if (sliding_buf.size() > expected_header.size()) {
+          sliding_buf.erase(sliding_buf.begin());
+      }
+      if (sliding_buf == expected_header) {
+          break;
+      }
+    }
+  }
+  
+  if (sliding_buf != expected_header) {
+    RCLCPP_WARN(this->get_logger(), "Timeout waiting for header");
+    return {};
+  }
+
+  size_t remain_len = payload_size + 2;
+  std::vector<uint8_t> remain_buf(remain_len);
+  size_t total_read = 0;
+
+  while (total_read < remain_len && (this->now() - start_time) < timeout) {
+    boost::asio::mutable_buffers_1 buf(&remain_buf[total_read], remain_len - total_read);
+    boost::system::error_code ec;
+    size_t n = serial_port_->read_some(buf, ec);
+    if (ec) {
+        RCLCPP_WARN(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+        return {};
+    }
+    total_read += n;
+  }
+
+  if (total_read != remain_len) {
+    RCLCPP_WARN(this->get_logger(), "Timeout or incomplete data payload");
+    return {};
+  }
+
+  std::vector<uint8_t> full_buf = expected_header;
+  full_buf.insert(full_buf.end(), remain_buf.begin(), remain_buf.end());
+
+  return full_buf;
+}
+
+bool AGV_PRO::is_power_on(){
+  auto power_query_frame = build_serial_frame(GET_POWER_STATE, {});
+  send_serial_frame(power_query_frame,true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x12};
+  auto power_query_response = read_serial_response(expected_header, 8, 12.0);
+
+  print_hex("recv_buf", power_query_response);
+  
+  if (power_query_response.size() != 14) return false;
+
+  uint16_t received_crc = (power_query_response[12] << 8) | power_query_response[13];
+  uint16_t computed_crc = crc16_ibm(power_query_response.data(), 12);
+  if (received_crc != computed_crc) {
+    RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
+    return false;
+  }
+
+  int is_poweron_status = static_cast<int8_t>(power_query_response[4]);
+  RCLCPP_INFO(this->get_logger(), "is_poweron_status: %d", is_poweron_status);
+
+  if (is_poweron_status == 0){
+    auto status_query_frame = build_serial_frame(POWER_ON, {});
+    send_serial_frame(status_query_frame,true);
+
+    rclcpp::sleep_for(std::chrono::milliseconds(1000));// Sleep for 1000 milliseconds to allow the device enough time to process the previous command
+
+    const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, 0x10};
+    auto status_query_response = read_serial_response(expected_header, 8, 5.0);// Read the serial response with the specified expected header, payload size, and timeout of 5 seconds
+    print_hex("recv_buf", status_query_response);
+  
+    if (status_query_response.size() != 14) return false;
+
+    uint16_t received_crc = (status_query_response[12] << 8) | status_query_response[13];
+    uint16_t computed_crc = crc16_ibm(status_query_response.data(), 12);
+    if (received_crc != computed_crc) {
+      RCLCPP_WARN(this->get_logger(), "CRC mismatch: received=0x%04X, expected=0x%04X", received_crc, computed_crc);
+      return false;
+    }
+
+    int poweron_status = static_cast<int8_t>(status_query_response[4]);
+    std::string status_msg;
+
+    switch (poweron_status) {
+      case 1:
+        status_msg = "Motor is operating normally.";
+        RCLCPP_INFO(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
+        return true;
+      case 2:
+        status_msg = "Emergency stop button is not released.";
+        RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
+        return false;
+      case 3:
+        status_msg = "Battery voltage is below 19.5V.";
+        RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
+        return false;
+      case 4:
+        status_msg = "CAN initialization error.";
+        RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
+        return false;
+      case 5:
+        status_msg = "Motor initialization error.";
+        RCLCPP_ERROR(this->get_logger(), "power_status: %d, %s", poweron_status, status_msg.c_str());
+        return false;
+      default:
+        RCLCPP_WARN(this->get_logger(), "power_status: %d, Unknown power status code", poweron_status);
+        return false;
+    }
+  }
+  else{
+    RCLCPP_INFO(this->get_logger(), "Motor is operating normally.");
+    return true;
+  }
+}
+
+void AGV_PRO::set_auto_report(bool enable){
+  auto frame = build_serial_frame(0x23, {static_cast<uint8_t>(enable)});
+  send_serial_frame(frame,true);
+}
+
+void AGV_PRO::clearSerialBuffer(int fd) {
+  if (tcflush(fd, TCIOFLUSH) < 0) {
+    RCLCPP_WARN(this->get_logger(), "Failed to flush serial buffer: %s", std::strerror(errno));
+  } else {
+    RCLCPP_INFO(this->get_logger(), "Serial buffer flushed.");
+  }
+}
+
+void AGV_PRO::disableDTR_RTS(int fd) {
+  int status;
+  if (::ioctl(fd, TIOCMGET, &status) == 0) {
+    status &= ~(TIOCM_DTR | TIOCM_RTS);
+    if (::ioctl(fd, TIOCMSET, &status) != 0) {
+      RCLCPP_WARN(this->get_logger(), "Failed to clear DTR and RTS: %s", std::strerror(errno));
+    } else {
+      RCLCPP_INFO(this->get_logger(), "DTR and RTS lines disabled successfully.");
+    }
+  } else {
+    RCLCPP_WARN(this->get_logger(), "Failed to read modem status: %s", std::strerror(errno));
+  }
+}
+
+void AGV_PRO::cmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
+{
+  linearX = std::clamp(msg->linear.x, -1.5, 1.5);
+  linearY = std::clamp(msg->linear.y, -1.0, 1.0);
+  angularZ = std::clamp(msg->angular.z, -1.0, 1.0);
+
+  int16_t x_send = static_cast<int16_t>(linearX * 100);
+  int16_t y_send = static_cast<int16_t>(linearY * 100);
+  int16_t rot_send = static_cast<int16_t>(angularZ * 100);
+
+  uint8_t buf[14] = { 0xfe,0xfe,0x0b,0x21 };
+
+  buf[4] = (x_send >> 8) & 0xff;
+  buf[5] = x_send & 0xff;
+  buf[6] = (y_send >> 8) & 0xff;
+  buf[7] = y_send & 0xff;
+  buf[8] = (rot_send >> 8) & 0xff;
+  buf[9] = rot_send & 0xff;
+  buf[10] = 0x00;
+  buf[11] = 0x00;
+
+  uint16_t crc = crc16_ibm(buf, 12);
+  buf[12] = (crc >> 8) & 0xff;
+  buf[13] = crc & 0xff;
+
+  std::vector<uint8_t> data_vec(buf, buf + sizeof(buf));
+
+  try
+  {
+    boost::asio::write(*serial_port_,boost::asio::buffer(data_vec));
+    // print_hex("Sent", data_vec);//debug
+  }
+  catch(const std::exception &ex)
+  {
+    RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
+  }
+}
+
+void AGV_PRO::handleSetDigitalOutput(
+  const std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::SetDigitalOutput::Response> response)
+{
+  uint8_t output_number = request->pin;
+  uint8_t output_state = request->state;
+
+  if (output_number < 1 || output_number > 6){
+    RCLCPP_ERROR(this->get_logger(), "Invalid output pin number: %u", output_number);
+    response->success = false;
+    response->message = "Invalid output pin number";
+    return;
+  }
+
+  auto frame = build_serial_frame(SET_OUTPUT_IO, {output_number, output_state});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, SET_OUTPUT_IO};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[4];
+  if (status == 0x01) {
+    RCLCPP_INFO(this->get_logger(), "SetDigitalOutput succeeded");
+    response->success = true;
+    response->message = "Success";
+  } else {
+    RCLCPP_ERROR(this->get_logger(), "SetDigitalOutput failed with status: 0x%02X", status);
+    response->success = false;
+    response->message = "Failed with status code";
+  }
+}
+
+void AGV_PRO::handleGetDigitalInput(
+  const std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Request> request,
+  std::shared_ptr<agv_pro_msgs::srv::GetDigitalInput::Response> response)
+{
+  uint8_t input_number = request->pin;
+
+  if (input_number < 1 || input_number > 6){
+    RCLCPP_ERROR(this->get_logger(), "Invalid input pin number: %u", input_number);
+    response->success = false;
+    response->message = "Invalid input pin number";
+    return;
+  }
+
+  auto frame = build_serial_frame(GET_INPUT_IO, {input_number});
+  send_serial_frame(frame, true);
+
+  const std::vector<uint8_t> expected_header = {0xFE, 0xFE, 0x0B, GET_INPUT_IO};
+  auto response_frame = read_serial_response(expected_header, 8, 5.0);
+  // print_hex("recv_buf", response_frame); //debug
+
+  uint8_t status = response_frame[5];
+  if (status == 0xff) {
+    RCLCPP_ERROR(this->get_logger(), "GetDigitalInput failed with status: 0x%02X", status);
+    response->success = false;
+  } else {
+    RCLCPP_INFO(this->get_logger(), "GetDigitalInput succeeded, state: %u", status);
+    response->state = static_cast<int32_t>(status);
+    response->success = true;
+    response->message = "Success";
+  }
+}
+
+bool AGV_PRO::readData()
+{
+  std::vector<uint8_t> buf_length(1);
+  std::vector<uint8_t> data_buf(RECEIVE_PAYLOAD_SIZE);
+
+  uint8_t byte = 0;
+  boost::system::error_code ec;
+  
+  while (true)
+  {
+    size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
+    if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+    }
+    if (ret != 1 || byte != 0xfe) {
+      continue;
+    }
+
+    ret = boost::asio::read(*serial_port_, boost::asio::buffer(&byte, 1), ec);
+    if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+    }
+    if (ret == 1 && byte == 0xfe) {
+      break; 
+    }
+  }
+
+  size_t ret = boost::asio::read(*serial_port_, boost::asio::buffer(buf_length), ec);
+  if (ec) {
+      RCLCPP_ERROR(this->get_logger(), "Serial read error: %s", ec.message().c_str());
+      return false;
+  }
+
+  if (buf_length[0] != RECEIVE_PAYLOAD_SIZE) {
+    //RCLCPP_ERROR(this->get_logger(), "The received length is incorrect:%u", buf_length[0]);
+    return false;
+  }
+
+  ret = boost::asio::read(*serial_port_, boost::asio::buffer(data_buf), ec);
+  if (ec || ret != data_buf.size())
+  {
+    RCLCPP_ERROR(this->get_logger(), "Failed to receive full payload");
+    return false;
+  }
+
+  std::vector<uint8_t> recv_buf;
+  recv_buf.push_back(0xFE);
+  recv_buf.push_back(0xFE);
+  recv_buf.push_back(0x1C);
+  recv_buf.insert(recv_buf.end(), data_buf.begin(), data_buf.end());
+  
+  // print_hex("recv_buf", recv_buf); //debug
+
+  if (recv_buf[3] != 0x25) {
+    //RCLCPP_WARN(this->get_logger(), "Command error:0x%02X", recv_buf[2]); //debug
+    return false;
+  }
+
+  uint16_t received_crc = recv_buf[RECEIVE_FRAME_SIZE-1] | (recv_buf[RECEIVE_FRAME_SIZE-2] << 8);
+  uint16_t computed_crc = crc16_ibm(recv_buf.data(), RECEIVE_FRAME_SIZE-2);
+
+  if (received_crc != computed_crc) {
+    RCLCPP_WARN(this->get_logger(), "CRC error: received 0x%04X, calculated 0x%04X", received_crc, computed_crc);
+    return false;
+  }
+
+  vx = static_cast<double>(static_cast<int8_t>(recv_buf[4])) * 0.01;
+  vy = static_cast<double>(static_cast<int8_t>(recv_buf[5])) * 0.01;
+  vtheta = static_cast<double>(static_cast<int8_t>(recv_buf[6])) * 0.01;
+
+  motor_status = recv_buf[7];
+  motor_error  = recv_buf[8];
+  battery_voltage = static_cast<float>(recv_buf[9]) / 10.0f;
+  enable_status = recv_buf[10];
+
+  imu_data.linear_acceleration.x = static_cast<double>(static_cast<int16_t>((recv_buf[11] << 8) | recv_buf[12])) * 0.01;
+  imu_data.linear_acceleration.y = static_cast<double>(static_cast<int16_t>((recv_buf[13] << 8) | recv_buf[14])) * 0.01;
+  imu_data.linear_acceleration.z = static_cast<double>(static_cast<int16_t>((recv_buf[15] << 8) | recv_buf[16])) * 0.01;
+
+  imu_data.angular_velocity.x = static_cast<double>(static_cast<int16_t>((recv_buf[17] << 8) | recv_buf[18])) * 0.01;
+  imu_data.angular_velocity.y = static_cast<double>(static_cast<int16_t>((recv_buf[19] << 8) | recv_buf[20])) * 0.01;
+  imu_data.angular_velocity.z = static_cast<double>(static_cast<int16_t>((recv_buf[21] << 8) | recv_buf[22])) * 0.01;
+
+  roll  = static_cast<double>(static_cast<int16_t>((recv_buf[23] << 8) | recv_buf[24])) * 0.01;
+  pitch = static_cast<double>(static_cast<int16_t>((recv_buf[25] << 8) | recv_buf[26])) * 0.01;
+  yaw   = static_cast<double>(static_cast<int16_t>((recv_buf[27] << 8) | recv_buf[28])) * 0.01;
+
+  // RCLCPP_INFO(this->get_logger(),
+  // "IMU Data - Accel[x: %.2f, y: %.2f, z: %.2f], "
+  // "Gyro[x: %.2f, y: %.2f, z: %.2f], "
+  // "RPY[roll: %.2f, pitch: %.2f, yaw: %.2f]",
+  // imu_data.linear_acceleration.x,
+  // imu_data.linear_acceleration.y,
+  // imu_data.linear_acceleration.z,
+  // imu_data.angular_velocity.x,
+  // imu_data.angular_velocity.y,
+  // imu_data.angular_velocity.z,
+  // roll, pitch, yaw);
+
+  return true;
+}
+
+void AGV_PRO::publisherVoltage()
+{
+  std_msgs::msg::Float32 voltage_msg,voltage_backup_msg;
+  voltage_msg.data = battery_voltage;
+  pub_voltage->publish(voltage_msg);
+}
+
+void AGV_PRO::publisherImuSensor()
+{
+  sensor_msgs::msg::Imu ImuSensor;
+
+  ImuSensor.header.stamp = this->get_clock()->now();
+  ImuSensor.header.frame_id = "imu_link";
+
+  tf2::Quaternion qua;
+  qua.setRPY(0, 0, yaw * M_PI / 180.0);
+
+  ImuSensor.orientation.x = qua[0];
+  ImuSensor.orientation.y = qua[1];
+  ImuSensor.orientation.z = qua[2];
+  ImuSensor.orientation.w = qua[3];
+
+  ImuSensor.angular_velocity.x = imu_data.angular_velocity.x;
+  ImuSensor.angular_velocity.y = imu_data.angular_velocity.y;
+  ImuSensor.angular_velocity.z = imu_data.angular_velocity.z;
+
+  ImuSensor.linear_acceleration.x = imu_data.linear_acceleration.x;
+  ImuSensor.linear_acceleration.y = imu_data.linear_acceleration.y;
+  ImuSensor.linear_acceleration.z = imu_data.linear_acceleration.z;
+
+  ImuSensor.orientation_covariance[0] = 1e6;
+  ImuSensor.orientation_covariance[4] = 1e6;
+  ImuSensor.orientation_covariance[8] = 1e-6;
+
+  ImuSensor.angular_velocity_covariance[0] = 1e6;
+  ImuSensor.angular_velocity_covariance[4] = 1e6;
+  ImuSensor.angular_velocity_covariance[8] = 1e-6;
+
+  pub_imu->publish(ImuSensor);
+}
+
+void AGV_PRO::publisherOdom(double dt)
+{
+  currentTime = this->get_clock()->now();
+
+  double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt;
+  double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt;
+  double delta_th = vtheta * dt;
+
+  x += delta_x;
+  y += delta_y;
+  theta += delta_th;
+
+  geometry_msgs::msg::TransformStamped odom_trans;
+  odom_trans.header.stamp = currentTime;
+  odom_trans.header.frame_id = frame_id_of_odometry_;
+  odom_trans.child_frame_id = child_frame_id_of_odometry_;
+
+  tf2::Quaternion quat;
+  quat.setRPY(0.0, 0.0, theta);
+  geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quat);
+
+  odom_trans.transform.translation.x = x; 
+  odom_trans.transform.translation.y = y; 
+  odom_trans.transform.translation.z = 0.0;
+  odom_trans.transform.rotation = odom_quat;
+
+  odomBroadcaster->sendTransform(odom_trans);
+
+  nav_msgs::msg::Odometry odom;
+  odom.header.stamp = currentTime;
+  odom.header.frame_id = frame_id_of_odometry_;
+  odom.child_frame_id = child_frame_id_of_odometry_;
+
+  odom.pose.pose.position.x = x;
+  odom.pose.pose.position.y = y;
+  odom.pose.pose.position.z = 0.0;
+  odom.pose.pose.orientation = odom_quat;
+  odom.pose.covariance = this->odom_pose_covariance;
+
+  odom.twist.twist.linear.x = vx;
+  odom.twist.twist.linear.y = vy;
+  odom.twist.twist.angular.z = vtheta;
+  odom.twist.covariance = this->odom_twist_covariance;
+
+  pub_odom->publish(odom);
+}
+
+void AGV_PRO::Control()
+{
+  if (true == readData())
+  {
+    currentTime = this->get_clock()->now();
+    double dt = 0.0;
+    if (lastTime.nanoseconds() != 0) {
+      dt = (currentTime - lastTime).seconds();
+    }
+
+    lastTime = currentTime;
+    publisherOdom(dt);
+    // RCLCPP_INFO(this->get_logger(), "dt:%f", dt);
+    publisherVoltage();
+    publisherImuSensor();
+  }
+}
+
+AGV_PRO::AGV_PRO(std::string node_name):rclcpp::Node(node_name)
+{
+  this->declare_parameter<std::string>("port_name","/dev/agvpro_controller");
+  this->declare_parameter<std::string>("odometry.frame_id", "odom");
+  this->declare_parameter<std::string>("odometry.child_frame_id", "base_footprint");
+  this->declare_parameter<std::string>("imu.frame_id", "imu_link");
+  this->declare_parameter<std::string>("namespace", "");
+
+  this->get_parameter_or<std::string>("port_name",device_name_,std::string("/dev/agvpro_controller"));
+  this->get_parameter_or<std::string>("odometry.frame_id",frame_id_of_odometry_,std::string("odom"));
+  this->get_parameter_or<std::string>("odometry.child_frame_id",child_frame_id_of_odometry_,std::string("base_footprint"));
+  this->get_parameter_or<std::string>("imu.frame_id",frame_id_of_imu_,std::string("imu_link"));        
+  this->get_parameter_or<std::string>("namespace",name_space_,std::string(""));
+
+  if (name_space_ != "") {
+    frame_id_of_odometry_ = name_space_ + "/" + frame_id_of_odometry_;
+    child_frame_id_of_odometry_ = name_space_ + "/" + child_frame_id_of_odometry_;
+    frame_id_of_imu_ = name_space_ + "/" + frame_id_of_imu_;
+  }
+
+  odomBroadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
+  pub_imu =  this->create_publisher<sensor_msgs::msg::Imu>("imu", 20);
+  pub_odom = this->create_publisher<nav_msgs::msg::Odometry>("odom", rclcpp::SensorDataQoS());
+  pub_voltage = create_publisher<std_msgs::msg::Float32>("voltage", 10);
+  cmd_sub = this->create_subscription<geometry_msgs::msg::Twist>(
+    "/cmd_vel", 10, std::bind(&AGV_PRO::cmdCallback, this, std::placeholders::_1));
+
+  set_output_service = this->create_service<agv_pro_msgs::srv::SetDigitalOutput>(
+    "set_digital_output",
+    std::bind(&AGV_PRO::handleSetDigitalOutput, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  get_input_service = this->create_service<agv_pro_msgs::srv::GetDigitalInput>(
+    "get_digital_input",
+    std::bind(&AGV_PRO::handleGetDigitalInput, this, std::placeholders::_1, std::placeholders::_2)
+  );
+
+  lastTime = this->get_clock()->now();
+      
+  try{
+    serial_port_ = std::make_unique<boost::asio::serial_port>(io_);
+
+    serial_port_->open(device_name_);
+    serial_port_->set_option(boost::asio::serial_port_base::baud_rate(1000000));
+    serial_port_->set_option(boost::asio::serial_port_base::character_size(8));
+    serial_port_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
+    serial_port_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
+    serial_port_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
+
+    int fd = serial_port_->native_handle();
+    this->clearSerialBuffer(fd);
+    this->disableDTR_RTS(fd);
+
+    rclcpp::sleep_for(std::chrono::milliseconds(3000));//esp32 Restart time
+
+    RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
+    RCLCPP_INFO(this->get_logger(), "Using device: %s", device_name_.c_str());
+
+    boost::asio::serial_port_base::baud_rate baud_option;
+    serial_port_->get_option(baud_option);
+    unsigned int current_baud = baud_option.value();
+    RCLCPP_INFO(this->get_logger(), "Baud_rate: %u", current_baud);
+  }
+  catch (const std::exception &ex){
+    RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
+    return;
+  }
+
+  if (this->is_power_on()) {
+    this->set_auto_report(1);
+
+    control_timer_ = this->create_wall_timer(
+    std::chrono::milliseconds(20),
+    std::bind(&AGV_PRO::Control, this)
+    );
+    RCLCPP_INFO(this->get_logger(), "Control timer started");
+  }
+  else {
+    RCLCPP_WARN(this->get_logger(), "Control timer not started.");
+  }
+}
+
+AGV_PRO::~AGV_PRO()
+{
+  if (serial_port_ && serial_port_->is_open()) {
+    this->set_auto_report(0);
+    serial_port_->cancel();
+    serial_port_->close();
+  } 
+}