go2_navigation_provincial/go2_autonomous_navigation/go2_dashboard/server.py

#!/usr/bin/env python3
from __future__ import annotations

import argparse
import json
import math
import os
import shlex
import signal
import subprocess
import threading
import time
import uuid
import yaml
from copy import deepcopy
from http import HTTPStatus
from http.server import SimpleHTTPRequestHandler, ThreadingHTTPServer
from pathlib import Path
from typing import Any
from urllib.error import HTTPError, URLError
from urllib.parse import parse_qs, urlparse
from urllib.request import Request, urlopen

import rclpy
from action_msgs.msg import GoalStatus
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist
from nav2_msgs.action import NavigateToPose
from nav_msgs.msg import OccupancyGrid
from rclpy.action import ActionClient
from rclpy.duration import Duration
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
from rclpy.time import Time
from sensor_msgs.msg import LaserScan, PointCloud2
from std_srvs.srv import Empty
from tf2_ros import Buffer, TransformException, TransformListener
from unitree_go.msg import LowState, SportModeState


def yaw_to_quaternion(yaw_rad: float) -> tuple[float, float, float, float]:
    half = yaw_rad * 0.5
    return 0.0, 0.0, math.sin(half), math.cos(half)


def quaternion_to_yaw_deg(x: float, y: float, z: float, w: float) -> float:
    siny_cosp = 2.0 * (w * z + x * y)
    cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
    return math.degrees(math.atan2(siny_cosp, cosy_cosp))


def now_s() -> float:
    return time.time()


def read_json_body(handler: SimpleHTTPRequestHandler) -> dict[str, Any]:
    length = int(handler.headers.get("Content-Length") or "0")
    if length <= 0:
        return {}
    raw = handler.rfile.read(length)
    try:
        value = json.loads(raw.decode("utf-8"))
    except json.JSONDecodeError as exc:
        raise ValueError(f"Invalid JSON body: {exc}") from exc
    if not isinstance(value, dict):
        raise ValueError("JSON body must be an object")
    return value


def require_float(data: dict[str, Any], key: str) -> float:
    try:
        value = float(data[key])
    except KeyError as exc:
        raise ValueError(f"Missing field: {key}") from exc
    except (TypeError, ValueError) as exc:
        raise ValueError(f"Field must be numeric: {key}") from exc
    if not math.isfinite(value):
        raise ValueError(f"Field must be finite: {key}")
    return value


def status_name(status: int) -> str:
    return {
        GoalStatus.STATUS_UNKNOWN: "unknown",
        GoalStatus.STATUS_ACCEPTED: "accepted",
        GoalStatus.STATUS_EXECUTING: "executing",
        GoalStatus.STATUS_CANCELING: "canceling",
        GoalStatus.STATUS_SUCCEEDED: "succeeded",
        GoalStatus.STATUS_CANCELED: "canceled",
        GoalStatus.STATUS_ABORTED: "aborted",
    }.get(status, f"status_{status}")


def normalize_yaw_deg(value: float) -> float:
    return (value + 180.0) % 360.0 - 180.0


def env_bool(name: str, default: bool) -> bool:
    value = os.environ.get(name)
    if value is None:
        return default
    return value.strip().lower() in {"1", "true", "yes", "y", "on"}


def truncate_text(value: str, limit: int = 1600) -> str:
    if len(value) <= limit:
        return value
    return value[:limit] + "...[truncated]"


def truncate_lines(lines: list[str], limit: int = 8) -> list[str]:
    if len(lines) <= limit:
        return lines
    return lines[-limit:]


INSPECTION_ACTIVE_STATES = {"starting", "navigating", "arrived", "correcting", "segmenting", "dwelling", "canceling"}
GOAL_TERMINAL_STATES = {"succeeded", "failed", "rejected", "canceled", "aborted", "unknown"}
GOAL_INACTIVE_STATES = GOAL_TERMINAL_STATES | {"stopped", "stop_failed"}
ROBOT_REQUIRED_PUBLISHER_TOPICS = (
    "/lf/sportmodestate",
    "/lf/lowstate",
    "/utlidar/robot_pose",
    "/utlidar/cloud_deskewed",
)
ROBOT_COMMAND_TOPIC = "/api/sport/request"
LOCALIZATION_CONFIRM_MAX_AGE_S = 300.0
LOCALIZATION_CONFIRM_MAX_DRIFT_M = 0.3
LOCALIZATION_CONFIRM_MAX_YAW_DRIFT_DEG = 20.0
ROBOT_SOURCE_FRESHNESS_S = 3.0
NAV_SCAN_TOPIC = os.environ.get("GO2_NAV_SCAN_TOPIC", "/go2/scan").strip() or "/go2/scan"
NAV_HEALTH_SCAN_FRESHNESS_S = float(os.environ.get("GO2_NAV_HEALTH_SCAN_FRESHNESS_S", "1.0"))
NAV_HEALTH_TF_FRESHNESS_S = float(os.environ.get("GO2_NAV_HEALTH_TF_FRESHNESS_S", "1.0"))
NAV_HEALTH_TF_STAMP_FRESHNESS_S = float(
    os.environ.get("GO2_NAV_HEALTH_TF_STAMP_FRESHNESS_S", str(max(NAV_HEALTH_TF_FRESHNESS_S, 1.0)))
)
NAV_HEALTH_POSE_FRESHNESS_S = float(os.environ.get("GO2_NAV_HEALTH_POSE_FRESHNESS_S", "1.0"))
NAV_HEALTH_MIN_VALID_SCAN_RANGES = int(os.environ.get("GO2_NAV_HEALTH_MIN_VALID_SCAN_RANGES", "3"))
NAV_HEALTH_MAX_AMCL_TF_DELTA_M = float(os.environ.get("GO2_NAV_HEALTH_MAX_AMCL_TF_DELTA_M", "0.35"))
NAV_HEALTH_MAX_AMCL_TF_YAW_DELTA_DEG = float(os.environ.get("GO2_NAV_HEALTH_MAX_AMCL_TF_YAW_DELTA_DEG", "25.0"))
INSPECTION_REQUIRE_AMCL_CONSISTENCY = env_bool("GO2_INSPECTION_REQUIRE_AMCL_CONSISTENCY", False)
INSPECTION_LOCALIZATION_REFRESH_TIMEOUT_S = float(os.environ.get("GO2_INSPECTION_LOCALIZATION_REFRESH_TIMEOUT_S", "2.0"))
INSPECTION_LOCALIZATION_REFRESH_SETTLE_S = float(os.environ.get("GO2_INSPECTION_LOCALIZATION_REFRESH_SETTLE_S", "0.8"))
INSPECTION_POSITION_TOLERANCE_M = float(os.environ.get("GO2_INSPECTION_POSITION_TOLERANCE_M", "0.25"))
INSPECTION_NAV_ABORT_NEAR_GOAL_TOLERANCE_M = float(
    os.environ.get("GO2_INSPECTION_NAV_ABORT_NEAR_GOAL_TOLERANCE_M", "0.35")
)
INSPECTION_NAV_SUCCESS_NEAR_GOAL_TOLERANCE_M = float(
    os.environ.get("GO2_INSPECTION_NAV_SUCCESS_NEAR_GOAL_TOLERANCE_M", "0.60")
)
INSPECTION_POSITION_CORRECTION_THRESHOLD_M = float(os.environ.get("GO2_INSPECTION_POSITION_CORRECTION_THRESHOLD_M", "0.20"))

INSPECTION_YAW_TOLERANCE_DEG = float(os.environ.get("GO2_INSPECTION_YAW_TOLERANCE_DEG", "6.0"))
INSPECTION_POSE_FRESHNESS_S = float(os.environ.get("GO2_INSPECTION_POSE_FRESHNESS_S", "5.0"))
INSPECTION_MAX_LINEAR_SPEED_MPS = float(os.environ.get("GO2_INSPECTION_MAX_LINEAR_SPEED_MPS", "0.15"))
INSPECTION_MAX_YAW_RATE_DEG_S = float(os.environ.get("GO2_INSPECTION_MAX_YAW_RATE_DEG_S", "15.0"))
INSPECTION_ARRIVAL_STABLE_HOLD_S = float(os.environ.get("GO2_INSPECTION_ARRIVAL_STABLE_HOLD_S", "0.2"))
INSPECTION_DWELL_DURATION_S = float(os.environ.get("GO2_INSPECTION_DWELL_DURATION_S", "3.0"))
INSPECTION_YAW_ALIGN_TIMEOUT_S = float(os.environ.get("GO2_INSPECTION_YAW_ALIGN_TIMEOUT_S", "12.0"))
INSPECTION_YAW_ALIGN_MIN_RATE_RAD_S = float(os.environ.get("GO2_INSPECTION_YAW_ALIGN_MIN_RATE_RAD_S", "0.35"))
INSPECTION_YAW_ALIGN_MAX_RATE_RAD_S = float(os.environ.get("GO2_INSPECTION_YAW_ALIGN_MAX_RATE_RAD_S", "0.55"))
INSPECTION_YAW_ALIGN_SLOWDOWN_DEG = float(os.environ.get("GO2_INSPECTION_YAW_ALIGN_SLOWDOWN_DEG", "35.0"))
INSPECTION_YAW_ALIGN_ARC_LINEAR_MPS = float(os.environ.get("GO2_INSPECTION_YAW_ALIGN_ARC_LINEAR_MPS", "0.12"))
INSPECTION_YAW_ALIGN_ARC_ENABLE_DEG = float(os.environ.get("GO2_INSPECTION_YAW_ALIGN_ARC_ENABLE_DEG", "10.0"))
REALTIME_SEGMENT_DEFAULT_URL = "http://192.168.0.199:5000/realtime-segment"
REALTIME_SEGMENT_DEFAULT_IMAGE_FIELD = "image"
REALTIME_SEGMENT_FRAME_MAX_AGE_S = 5.0
INSPECTION_NAV_BT_XML = os.environ.get("GO2_INSPECTION_NAV_BT_XML", "/home/limerly/go2_robot_system/install/go2_navigation2/share/go2_navigation2/behavior_trees/inspection_position_only.xml")


def safe_float_env(name: str, default: float) -> float:
    try:
        return float(os.environ.get(name, str(default)))
    except (TypeError, ValueError):
        return default


def parse_json_response(text: str) -> Any:
    if not text:
        return None
    try:
        return json.loads(text)
    except json.JSONDecodeError:
        return None


def extract_response_error(payload: Any, fallback: str) -> str:
    if isinstance(payload, dict):
        for key in ("error", "message", "detail", "reason"):
            value = payload.get(key)
            if value:
                return str(value)
    return fallback


def encode_multipart_form_data(fields: dict[str, str], file_field: str, filename: str, content_type: str, data: bytes) -> tuple[bytes, str]:
    boundary = f"----go2-dashboard-{uuid.uuid4().hex}"
    chunks: list[bytes] = []
    for key, value in fields.items():
        chunks.extend(
            [
                f"--{boundary}\r\n".encode("utf-8"),
                f'Content-Disposition: form-data; name="{key}"\r\n\r\n'.encode("utf-8"),
                str(value).encode("utf-8"),
                b"\r\n",
            ]
        )
    chunks.extend(
        [
            f"--{boundary}\r\n".encode("utf-8"),
            f'Content-Disposition: form-data; name="{file_field}"; filename="{filename}"\r\n'.encode("utf-8"),
            f"Content-Type: {content_type}\r\n\r\n".encode("utf-8"),
            data,
            b"\r\n",
            f"--{boundary}--\r\n".encode("utf-8"),
        ]
    )
    return b"".join(chunks), f"multipart/form-data; boundary={boundary}"


class PosePipeline:
    """Small pose-processing seam reserved for later localization optimization."""

    DEFAULT_CONFIG = {
        "version": 1,
        "algorithm": "passthrough_with_offsets",
        "source_priority": ["tf:map->base_link", "amcl:/amcl_pose"],
        "max_source_age_s": 2.0,
        "offset": {"x": 0.0, "y": 0.0, "yaw_deg": 0.0},
        "smoothing": {"enabled": False, "alpha": 0.65},
    }

    def __init__(self, config_path: Path) -> None:
        self.config_path = config_path
        self._lock = threading.RLock()
        self._last_pose: dict[str, Any] | None = None
        self.config = self._load_config()

    def _load_config(self) -> dict[str, Any]:
        config = deepcopy(self.DEFAULT_CONFIG)
        if self.config_path.exists():
            try:
                user_config = json.loads(self.config_path.read_text(encoding="utf-8"))
                if isinstance(user_config, dict):
                    config.update(user_config)
                    if isinstance(user_config.get("offset"), dict):
                        config["offset"] = {**self.DEFAULT_CONFIG["offset"], **user_config["offset"]}
                    if isinstance(user_config.get("smoothing"), dict):
                        config["smoothing"] = {**self.DEFAULT_CONFIG["smoothing"], **user_config["smoothing"]}
            except Exception:
                pass
        return config

    def priority(self) -> list[str]:
        value = self.config.get("source_priority", self.DEFAULT_CONFIG["source_priority"])
        return [str(item) for item in value] if isinstance(value, list) else list(self.DEFAULT_CONFIG["source_priority"])

    def max_source_age_s(self) -> float:
        try:
            return float(self.config.get("max_source_age_s", self.DEFAULT_CONFIG["max_source_age_s"]))
        except (TypeError, ValueError):
            return float(self.DEFAULT_CONFIG["max_source_age_s"])

    def process(self, raw_pose: dict[str, Any]) -> dict[str, Any]:
        with self._lock:
            pose = deepcopy(raw_pose)
            offset = self.config.get("offset") if isinstance(self.config.get("offset"), dict) else {}
            pose["x"] = float(pose["x"]) + float(offset.get("x", 0.0) or 0.0)
            pose["y"] = float(pose["y"]) + float(offset.get("y", 0.0) or 0.0)
            pose["yaw_deg"] = normalize_yaw_deg(float(pose["yaw_deg"]) + float(offset.get("yaw_deg", 0.0) or 0.0))
            smoothing = self.config.get("smoothing") if isinstance(self.config.get("smoothing"), dict) else {}
            if smoothing.get("enabled") and self._last_pose:
                alpha = max(0.0, min(1.0, float(smoothing.get("alpha", 0.65))))
                previous = self._last_pose
                pose["x"] = alpha * pose["x"] + (1.0 - alpha) * float(previous["x"])
                pose["y"] = alpha * pose["y"] + (1.0 - alpha) * float(previous["y"])
                yaw_delta = normalize_yaw_deg(float(pose["yaw_deg"]) - float(previous["yaw_deg"]))
                pose["yaw_deg"] = normalize_yaw_deg(float(previous["yaw_deg"]) + alpha * yaw_delta)
            pose["raw_source"] = raw_pose.get("source")
            pose["source"] = f"pose_pipeline:{self.config.get('algorithm', 'passthrough_with_offsets')}"
            pose["pipeline_config"] = {
                "algorithm": self.config.get("algorithm", "passthrough_with_offsets"),
                "offset": deepcopy(self.config.get("offset", {})),
                "smoothing": deepcopy(self.config.get("smoothing", {})),
            }
            self._last_pose = deepcopy(pose)
            return pose


class WaypointActionHook:
    def __init__(self, config_path: Path) -> None:
        self.config_path = config_path
        self._status = {
            "enabled": False,
            "state": "disabled",
            "reason": "inspection_dwell_mode",
            "updated_at_unix_s": now_s(),
            "config_path": str(self.config_path),
        }

    def enabled(self) -> bool:
        return False

    def status(self) -> dict[str, Any]:
        payload = deepcopy(self._status)
        payload["enabled"] = False
        payload["config"] = {
            "mode": "disabled",
            "required": False,
            "reason": "\u5de1\u68c0\u4e3b\u6d41\u7a0b\u53ea\u505a\u5bfc\u822a\u5230\u70b9\u3001\u5230\u70b9\u786e\u8ba4\u548c 3 \u79d2\u505c\u7559\uff0c\u4e0d\u518d\u6267\u884c waypoint_action\u3002",
        }
        return payload


class OpenSourceNavNode(Node):
    def __init__(
        self,
        map_yaml: Path,
        repo_root: Path,
        nav_log_dir: Path,
        pose_config: Path,
        waypoint_action_config: Path,
    ) -> None:
        super().__init__("go2_dashboard")
        self.map_yaml = map_yaml
        self.repo_root = repo_root
        self.nav_log_dir = nav_log_dir
        self.route_path = repo_root / "go2_dashboard" / "data" / "inspection_routes" / "default.json"
        self.patrol_route_path = repo_root / "config" / "ros2_patrol_routes" / "default.yaml"
        self.pose_pipeline = PosePipeline(pose_config)
        self.waypoint_action = WaypointActionHook(waypoint_action_config)
        self._lock = threading.RLock()
        self._map: dict[str, Any] | None = None
        self._fallback_map: dict[str, Any] | None = None
        self._pose: dict[str, Any] | None = None
        self._raw_poses: dict[str, dict[str, Any]] = {}
        self._robot_source_last_seen: dict[str, float] = {}
        self._scan_health: dict[str, Any] = {"topic": NAV_SCAN_TOPIC, "received_at_unix_s": None, "state": "waiting"}
        self._amcl_covariance: dict[str, Any] = {"position_uncertainty_m": None, "yaw_uncertainty_deg": None, "raw_xx": None, "raw_yy": None, "raw_yaw": None, "updated_at_unix_s": None}
        self._tf_health: dict[str, Any] = {"state": "waiting", "source": "tf:map->base_link"}
        self._goal: dict[str, Any] = {"state": "idle", "updated_at_unix_s": now_s()}
        self._current_goal_handle: Any | None = None
        self._current_goal_sequence: int | None = None
        self._goal_sequence = 0
        self._inspection_run_sequence = 0
        self._inspection_run: dict[str, Any] = {"state": "idle", "updated_at_unix_s": now_s()}
        self._localization_confirmation: dict[str, Any] | None = None
        self.camera_sampler: CameraSampler | None = None

        map_qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.create_subscription(OccupancyGrid, "/map", self._map_callback, map_qos)
        self.create_subscription(PoseWithCovarianceStamped, "/amcl_pose", self._pose_callback, 10)
        self.create_subscription(LaserScan, NAV_SCAN_TOPIC, lambda msg: self._scan_callback(msg, NAV_SCAN_TOPIC), 10)
        if NAV_SCAN_TOPIC != "/scan":
            self.create_subscription(LaserScan, "/scan", lambda msg: self._scan_callback(msg, "/scan"), 10)
        self.create_subscription(SportModeState, "/lf/sportmodestate", lambda msg: self._robot_source_callback("/lf/sportmodestate"), 10)
        self.create_subscription(LowState, "/lf/lowstate", lambda msg: self._robot_source_callback("/lf/lowstate"), 10)
        self.create_subscription(PoseStamped, "/utlidar/robot_pose", lambda msg: self._robot_source_callback("/utlidar/robot_pose"), 10)
        self.create_subscription(PointCloud2, "/utlidar/cloud_deskewed", lambda msg: self._robot_source_callback("/utlidar/cloud_deskewed"), 10)
        self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 10)
        self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel", 10)
        self.cmd_vel_nav_pub = self.create_publisher(Twist, "/cmd_vel_nav", 10)
        self.nav_client = ActionClient(self, NavigateToPose, "/navigate_to_pose")
        self.amcl_nomotion_client = self.create_client(Empty, "/request_nomotion")
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.create_timer(0.1, self._tf_pose_timer)

    def _invalidate_localization_confirmation(self, reason: str) -> None:
        with self._lock:
            self._localization_confirmation = {
                "state": "needs_confirmation",
                "reason": reason,
                "updated_at_unix_s": now_s(),
            }

    def _robot_source_callback(self, topic: str) -> None:
        with self._lock:
            self._robot_source_last_seen[topic] = now_s()

    def _map_callback(self, msg: OccupancyGrid) -> None:
        yaw_deg = quaternion_to_yaw_deg(
            msg.info.origin.orientation.x,
            msg.info.origin.orientation.y,
            msg.info.origin.orientation.z,
            msg.info.origin.orientation.w,
        )
        payload = {
            "name": "maps/current",
            "revision": map_file_revision(self.map_yaml),
            "frame_id": msg.header.frame_id or "map",
            "resolution": float(msg.info.resolution),
            "width": int(msg.info.width),
            "height": int(msg.info.height),
            "origin": {
                "x": float(msg.info.origin.position.x),
                "y": float(msg.info.origin.position.y),
                "yaw_deg": yaw_deg,
            },
            "cells": [int(value) for value in msg.data],
            "source": "ros:/map",
            "received_at_unix_s": now_s(),
        }
        with self._lock:
            self._map = payload

    def _scan_callback(self, msg: LaserScan, topic: str = NAV_SCAN_TOPIC) -> None:
        finite_ranges = [float(value) for value in msg.ranges if math.isfinite(float(value))]
        min_range_m = min(finite_ranges) if finite_ranges else None
        max_range_m = max(finite_ranges) if finite_ranges else None
        stamp_unix_s = float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec) / 1e9
        payload = {
            "topic": topic,
            "frame_id": msg.header.frame_id or "",
            "stamp_unix_s": stamp_unix_s,
            "received_at_unix_s": now_s(),
            "range_count": len(msg.ranges),
            "valid_range_count": len(finite_ranges),
            "min_range_m": min_range_m,
            "max_range_m": max_range_m,
            "range_min_m": float(msg.range_min),
            "range_max_m": float(msg.range_max),
            "state": "receiving",
        }
        with self._lock:
            self._scan_health = payload

    def _pose_callback(self, msg: PoseWithCovarianceStamped) -> None:
        pose = msg.pose.pose
        payload = {
            "x": float(pose.position.x),
            "y": float(pose.position.y),
            "z": float(pose.position.z),
            "yaw_deg": quaternion_to_yaw_deg(
                pose.orientation.x,
                pose.orientation.y,
                pose.orientation.z,
                pose.orientation.w,
            ),
            "frame_id": msg.header.frame_id or "map",
            "stamp_unix_s": float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec) / 1e9,
            "received_at_unix_s": now_s(),
            "source": "amcl:/amcl_pose",
        }
        cov = msg.pose.covariance
        cov_xx = float(cov[0])
        cov_yy = float(cov[7])
        cov_yaw = float(cov[35])
        position_uncertainty = math.sqrt(cov_xx + cov_yy)
        yaw_uncertainty_deg = math.degrees(math.sqrt(max(0.0, cov_yaw)))
        with self._lock:
            self._amcl_covariance = {
                "position_uncertainty_m": round(position_uncertainty, 4),
                "yaw_uncertainty_deg": round(yaw_uncertainty_deg, 2),
                "raw_xx": round(cov_xx, 6),
                "raw_yy": round(cov_yy, 6),
                "raw_yaw": round(cov_yaw, 6),
                "updated_at_unix_s": now_s(),
            }
        self._update_pose(payload)

    def _tf_pose_timer(self) -> None:
        transform = None
        source = None
        for target_frame in ("base_link",):
            try:
                transform = self.tf_buffer.lookup_transform("map", target_frame, Time(), timeout=Duration(seconds=0.02))
                source = f"tf:map->{target_frame}"
                break
            except TransformException:
                continue
        if transform is None or source is None:
            with self._lock:
                self._tf_health = {
                    "state": "missing",
                    "source": "tf:map->base_link",
                    "updated_at_unix_s": now_s(),
                    "reason": "map->base_link transform unavailable",
                }
            return
        translation = transform.transform.translation
        rotation = transform.transform.rotation
        tf_stamp = float(transform.header.stamp.sec) + float(transform.header.stamp.nanosec) / 1e9
        received_at = now_s()
        with self._lock:
            self._tf_health = {
                "state": "receiving",
                "source": source,
                "frame_id": "map",
                "child_frame_id": transform.child_frame_id,
                "stamp_unix_s": tf_stamp,
                "received_at_unix_s": received_at,
            }
        payload = {
            "x": float(translation.x),
            "y": float(translation.y),
            "z": float(translation.z),
            "yaw_deg": quaternion_to_yaw_deg(rotation.x, rotation.y, rotation.z, rotation.w),
            "frame_id": "map",
            "stamp_unix_s": tf_stamp,
            "received_at_unix_s": received_at,
            "source": source,
        }
        self._update_pose(payload)

    def _update_pose(self, payload: dict[str, Any]) -> None:
        with self._lock:
            source = str(payload.get("source") or "unknown")
            self._raw_poses[source] = deepcopy(payload)
            selected = self._select_raw_pose_locked()
            self._pose = self.pose_pipeline.process(selected) if selected else None

    def _pose_fresh_enough(self, pose: dict[str, Any] | None) -> bool:
        if not pose:
            return False
        received_age_s = max(0.0, now_s() - float(pose.get("received_at_unix_s", 0.0)))
        stamp_age_s = None
        if pose.get("stamp_unix_s"):
            stamp_age_s = max(0.0, now_s() - float(pose.get("stamp_unix_s", 0.0)))
        if stamp_age_s is None:
            return received_age_s <= self.pose_pipeline.max_source_age_s()
        return received_age_s <= self.pose_pipeline.max_source_age_s() and stamp_age_s <= max(self.pose_pipeline.max_source_age_s(), 0.5)

    def _select_raw_pose_locked(self) -> dict[str, Any] | None:
        cutoff = now_s() - self.pose_pipeline.max_source_age_s()
        for source in self.pose_pipeline.priority():
            pose = self._raw_poses.get(source)
            if pose and float(pose.get("received_at_unix_s", 0.0)) >= cutoff:
                return deepcopy(pose)
        fresh = [
            pose
            for pose in self._raw_poses.values()
            if float(pose.get("received_at_unix_s", 0.0)) >= cutoff
        ]
        if not fresh:
            return None
        return deepcopy(max(fresh, key=lambda item: float(item.get("received_at_unix_s", 0.0))))

    def _navigation_health_state(self, pose: dict[str, Any] | None) -> dict[str, Any]:
        current_time = now_s()
        with self._lock:
            scan = deepcopy(self._scan_health)
            tf_health = deepcopy(self._tf_health)
            raw_poses = deepcopy(self._raw_poses)

        checks: list[dict[str, Any]] = []
        blocking_reasons: list[str] = []

        def add_check(check_id: str, label: str, ok: bool, detail: str, reason: str | None = None, blocking: bool = True) -> None:
            checks.append({
                "id": check_id,
                "label": label,
                "ok": bool(ok),
                "reason": None if ok else (reason or detail),
                "detail": detail,
                "blocking": blocking,
            })
            if blocking and not ok:
                blocking_reasons.append(reason or detail)

        scan_received_at = scan.get("received_at_unix_s")
        scan_age_s = None if scan_received_at is None else max(0.0, current_time - float(scan_received_at))
        scan_stamp_age_s = None
        if scan.get("stamp_unix_s"):
            scan_stamp_age_s = max(0.0, current_time - float(scan["stamp_unix_s"]))
        valid_ranges = int(scan.get("valid_range_count") or 0)
        scan_fresh = scan_age_s is not None and scan_age_s <= NAV_HEALTH_SCAN_FRESHNESS_S
        scan_stamp_fresh = scan_stamp_age_s is None or scan_stamp_age_s <= max(NAV_HEALTH_SCAN_FRESHNESS_S, 1.0)
        scan_enough = valid_ranges >= NAV_HEALTH_MIN_VALID_SCAN_RANGES
        # A LaserScan with mostly +inf ranges can be valid in open space. Treat
        # low finite-return counts as a warning while keeping freshness/stamp
        # checks blocking; Nav2 costmaps still consume the full scan message.
        scan_ok = scan_fresh and scan_stamp_fresh
        scan_topic = str(scan.get("topic") or NAV_SCAN_TOPIC)
        scan_detail = f"{scan_topic} age={scan_age_s:.2f}s valid={valid_ranges}" if scan_age_s is not None else f"{scan_topic} has not been received"
        if scan_stamp_age_s is not None:
            scan_detail += f" stamp_age={scan_stamp_age_s:.2f}s"
        scan_reason = None
        if scan_age_s is None:
            scan_reason = f"{scan_topic} has not been received; local obstacle avoidance is unreliable"
        elif not scan_fresh:
            scan_reason = f"{scan_topic} data is stale {scan_age_s:.2f}s; local obstacle avoidance is unreliable"
        elif not scan_stamp_fresh:
            scan_reason = f"{scan_topic} stamp is stale {scan_stamp_age_s:.2f}s; TF alignment is unreliable"
        elif not scan_enough:
            scan_reason = f"{scan_topic} has too few finite ranges ({valid_ranges}); obstacle detection is unreliable"
        add_check("scan", scan_topic, scan_ok, scan_detail, scan_reason)
        if scan_ok and not scan_enough:
            checks[-1]["ok"] = False
            checks[-1]["reason"] = scan_reason
            checks[-1]["blocking"] = False

        tf_received_at = tf_health.get("received_at_unix_s") or tf_health.get("updated_at_unix_s")
        tf_age_s = None if tf_received_at is None else max(0.0, current_time - float(tf_received_at))
        tf_stamp_age_s = None
        if tf_health.get("stamp_unix_s"):
            tf_stamp_age_s = max(0.0, current_time - float(tf_health["stamp_unix_s"]))
        tf_fresh = tf_age_s is not None and tf_age_s <= NAV_HEALTH_TF_FRESHNESS_S and tf_health.get("state") == "receiving"
        tf_stamp_fresh = tf_stamp_age_s is None or tf_stamp_age_s <= NAV_HEALTH_TF_STAMP_FRESHNESS_S
        tf_ok = tf_fresh and tf_stamp_fresh
        tf_detail = f"{tf_health.get('source') or 'tf'} age={tf_age_s:.2f}s" if tf_age_s is not None else "map->base TF has not been received"
        if tf_stamp_age_s is not None:
            tf_detail += f" stamp_age={tf_stamp_age_s:.2f}s"
        tf_reason = None
        if tf_age_s is None:
            tf_reason = "\u672a\u6536\u5230 map->base TF\uff0c\u65e0\u6cd5\u786e\u8ba4\u5f53\u524d\u4f4d\u59ff"
        elif tf_health.get("state") != "receiving":
            tf_reason = "map->base TF \u4e0d\u53ef\u7528\uff0c\u5bfc\u822a\u5750\u6807\u94fe\u8def\u5f02\u5e38"
        elif not tf_fresh:
            tf_reason = f"map->base TF \u6570\u636e\u8fc7\u671f {tf_age_s:.2f}s\uff0c\u5f53\u524d\u4f4d\u7f6e\u4e0d\u53ef\u4fe1"
        elif not tf_stamp_fresh:
            tf_reason = f"map->base TF \u65f6\u95f4\u6233\u8fc7\u671f {tf_stamp_age_s:.2f}s\uff0c\u5b9a\u4f4d\u94fe\u8def\u4e0d\u53ef\u9760"
        add_check("tf", "TF map->base", tf_ok, tf_detail, tf_reason)

        pose_age_s = None
        pose_stamp_age_s = None
        pose_fresh = False
        if pose and pose.get("received_at_unix_s"):
            pose_age_s = max(0.0, current_time - float(pose["received_at_unix_s"]))
            pose_fresh = pose_age_s <= NAV_HEALTH_POSE_FRESHNESS_S and self._pose_fresh_enough(pose)
            if pose.get("stamp_unix_s"):
                pose_stamp_age_s = max(0.0, current_time - float(pose["stamp_unix_s"]))
        pose_detail = f"pose age={pose_age_s:.2f}s" if pose_age_s is not None else "current map pose unavailable"
        if pose_stamp_age_s is not None:
            pose_detail += f" stamp_age={pose_stamp_age_s:.2f}s"
        pose_reason = None
        if not pose:
            pose_reason = "\u5f53\u524d map \u4f4d\u59ff\u4e0d\u53ef\u7528\uff0c\u8bf7\u91cd\u65b0\u8bbe\u7f6e\u521d\u59cb\u4f4d\u59ff"
        elif not pose_fresh:
            pose_reason = f"\u5f53\u524d map \u4f4d\u59ff\u8fc7\u671f {pose_age_s:.2f}s\uff0c\u8bf7\u91cd\u65b0\u8bbe\u7f6e\u521d\u59cb\u4f4d\u59ff"
        add_check("pose", "\u5f53\u524d\u4f4d\u59ff", pose_fresh, pose_detail, pose_reason)

        tf_pose = None
        for source in ("tf:map->base_link",):
            if raw_poses.get(source):
                tf_pose = raw_poses[source]
                break

        amcl_pose = raw_poses.get("amcl:/amcl_pose")
        amcl_age_s = None
        amcl_stamp_age_s = None
        amcl_fresh = False
        if amcl_pose and amcl_pose.get("received_at_unix_s"):
            amcl_age_s = max(0.0, current_time - float(amcl_pose["received_at_unix_s"]))
            amcl_fresh = amcl_age_s <= NAV_HEALTH_POSE_FRESHNESS_S and self._pose_fresh_enough(amcl_pose)
            if amcl_pose.get("stamp_unix_s"):
                amcl_stamp_age_s = max(0.0, current_time - float(amcl_pose["stamp_unix_s"]))

        consistency_ok = True
        consistency_detail = "waiting for AMCL and TF pose"
        consistency_reason = None
        consistency_blocking = True
        pose_delta_m = None
        yaw_delta_deg = None
        if amcl_pose and tf_pose:
            pose_delta_m = math.hypot(float(amcl_pose["x"]) - float(tf_pose["x"]), float(amcl_pose["y"]) - float(tf_pose["y"]))
            yaw_delta_deg = abs(normalize_yaw_deg(float(amcl_pose["yaw_deg"]) - float(tf_pose["yaw_deg"])))
            consistency_detail = f"AMCL/TF dpos={pose_delta_m:.3f}m dyaw={yaw_delta_deg:.1f}deg"
            if amcl_fresh:
                consistency_ok = pose_delta_m <= NAV_HEALTH_MAX_AMCL_TF_DELTA_M and yaw_delta_deg <= NAV_HEALTH_MAX_AMCL_TF_YAW_DELTA_DEG
            else:
                consistency_ok = True
                consistency_blocking = False
                consistency_detail += "；/amcl_pose 已停更，按新鲜 TF 位姿继续"
            if amcl_fresh and not consistency_ok:
                consistency_reason = f"AMCL \u4e0e TF \u4f4d\u59ff\u4e0d\u4e00\u81f4\uff1a\u8ddd\u79bb {pose_delta_m:.3f}m\uff0c\u671d\u5411 {yaw_delta_deg:.1f}deg"

        tf_localization_ok = bool(scan_ok and tf_ok and pose_fresh and tf_pose)
        amcl_consistent_with_tf = bool(amcl_pose and tf_pose and consistency_ok)
        amcl_can_be_non_blocking = (
            tf_localization_ok
            and not INSPECTION_REQUIRE_AMCL_CONSISTENCY
            and (not amcl_pose or not amcl_fresh or amcl_consistent_with_tf or not consistency_blocking)
        )
        amcl_detail = f"/amcl_pose age={amcl_age_s:.2f}s" if amcl_age_s is not None else "/amcl_pose has not been received"
        if amcl_stamp_age_s is not None:
            amcl_detail += f" stamp_age={amcl_stamp_age_s:.2f}s"
        if not amcl_fresh and amcl_can_be_non_blocking:
            amcl_detail += "；scan/TF/当前位姿新鲜，按 TF 定位链路继续"
        amcl_reason = None
        if not amcl_pose:
            amcl_reason = "未收到 /amcl_pose，但 scan/TF/当前位姿新鲜，按 TF 定位链路继续" if amcl_can_be_non_blocking else "\u672a\u6536\u5230 /amcl_pose\uff0cAMCL \u5b9a\u4f4d\u4e0d\u53ef\u7528"
        elif not amcl_fresh:
            amcl_reason = (
                f"/amcl_pose 已停更 {amcl_age_s:.2f}s，但 scan/TF/当前位姿新鲜，按 TF 定位链路继续"
                if amcl_can_be_non_blocking
                else f"/amcl_pose \u5df2\u505c\u66f4 {amcl_age_s:.2f}s\uff0cAMCL \u6ca1\u6709\u6210\u529f\u5173\u8054 /scan \u548c TF\uff0c\u7981\u6b62\u5de1\u68c0"
            )
        add_check("amcl_pose", "AMCL 定位更新", amcl_fresh or amcl_can_be_non_blocking, amcl_detail, amcl_reason, blocking=not amcl_can_be_non_blocking)
        add_check("pose_consistency", "AMCL/TF \u4e00\u81f4\u6027", consistency_ok, consistency_detail, consistency_reason, blocking=consistency_blocking)

        ok = not blocking_reasons
        return {
            "ok": ok,
            "state": "ok" if ok else "blocked",
            "summary": "scan/TF/pose fresh" if ok else blocking_reasons[0],
            "blocking_reasons": blocking_reasons,
            "checks": checks,
            "scan": scan,
            "tf": tf_health,
            "pose_age_s": pose_age_s,
            "pose_stamp_age_s": pose_stamp_age_s,
            "amcl_pose_age_s": amcl_age_s,
            "amcl_pose_stamp_age_s": amcl_stamp_age_s,
            "amcl_tf_delta_m": pose_delta_m,
            "amcl_tf_yaw_delta_deg": yaw_delta_deg,
            "limits": {
                "scan_freshness_s": NAV_HEALTH_SCAN_FRESHNESS_S,
                "tf_freshness_s": NAV_HEALTH_TF_FRESHNESS_S,
                "tf_stamp_freshness_s": NAV_HEALTH_TF_STAMP_FRESHNESS_S,
                "pose_freshness_s": NAV_HEALTH_POSE_FRESHNESS_S,
                "min_valid_scan_ranges": NAV_HEALTH_MIN_VALID_SCAN_RANGES,
                "max_amcl_tf_delta_m": NAV_HEALTH_MAX_AMCL_TF_DELTA_M,
                "max_amcl_tf_yaw_delta_deg": NAV_HEALTH_MAX_AMCL_TF_YAW_DELTA_DEG,
            },
        }

    def _robot_ros_link_state(self) -> dict[str, Any]:
        publisher_counts: dict[str, int] = {}
        source_ages_s: dict[str, float | None] = {}
        stale_sources: list[str] = []
        current_time = now_s()
        with self._lock:
            last_seen = deepcopy(self._robot_source_last_seen)
        for topic in ROBOT_REQUIRED_PUBLISHER_TOPICS:
            count = len(self.get_publishers_info_by_topic(topic))
            publisher_counts[topic] = count
            seen_at = last_seen.get(topic)
            age_s = None if seen_at is None else max(0.0, current_time - float(seen_at))
            source_ages_s[topic] = age_s
            if age_s is None or age_s > ROBOT_SOURCE_FRESHNESS_S:
                stale_sources.append(topic)

        command_subscriber_count = len(self.get_subscriptions_info_by_topic(ROBOT_COMMAND_TOPIC))
        sources_fresh = not stale_sources
        command_ready = command_subscriber_count > 0
        ready = sources_fresh and command_ready
        return {
            "ready": ready,
            "sources_fresh": sources_fresh,
            "required_publishers": publisher_counts,
            "source_ages_s": source_ages_s,
            "source_freshness_s": ROBOT_SOURCE_FRESHNESS_S,
            "stale_sources": stale_sources,
            "command_topic": ROBOT_COMMAND_TOPIC,
            "command_subscriber_count": command_subscriber_count,
            "command_ready": command_ready,
            "missing_command_subscriber": not command_ready,
        }

    def _latest_nav_log_path(self) -> Path | None:
        try:
            files = [path for path in self.nav_log_dir.glob("*.log") if path.is_file()]
        except Exception:
            return None
        if not files:
            return None
        return max(files, key=lambda path: path.stat().st_mtime)

    def _nav_log_tail_lines(self, max_bytes: int = 240_000) -> tuple[Path | None, list[str]]:
        path = self._latest_nav_log_path()
        if path is None:
            return None, []
        try:
            with path.open("rb") as handle:
                handle.seek(0, os.SEEK_END)
                size = handle.tell()
                handle.seek(max(0, size - max_bytes), os.SEEK_SET)
                data = handle.read()
        except Exception:
            return path, []
        text = data.decode("utf-8", errors="replace")
        return path, text.splitlines()

    @staticmethod
    def _pick_relevant_nav_log_lines(lines: list[str]) -> list[str]:
        patterns = (
            "Failed to make progress",
            "No valid trajectories",
            "Goal failed",
            "Aborting handle",
            "Message Filter dropping message",
            "Extrapolation Error",
            "extrapolation into",
            "Failed to transform",
            "Timed out waiting",
            "Collision",
            "controller_server",
            "planner_server",
            "bt_navigator",
        )
        matched = [line.strip() for line in lines if any(pattern in line for pattern in patterns)]
        return truncate_lines(matched, 12)

    @staticmethod
    def _classify_nav_log_text(text: str) -> tuple[str, str]:
        lower = text.lower()
        if "failed to make progress" in lower:
            return (
                "nav2_controller_stalled",
                "Nav2 控制器无法继续推进。通常是局部轨迹全无效、障碍层/代价地图阻挡、目标附近姿态不稳定，或 TF 时间戳导致局部规划拿不到可用轨迹。",
            )
        if "no valid trajectories" in lower:
            return (
                "nav2_no_valid_trajectory",
                "Nav2 局部规划器没有找到可用轨迹。优先检查局部代价地图、障碍层、机器人足迹膨胀半径和目标点周围是否被占用。",
            )
        if "message filter dropping message" in lower or "extrapolation" in lower or "failed to transform" in lower:
            return (
                "tf_time_sync",
                "TF/传感器时间戳不同步或 TF 缓存对不上，Nav2 正在丢弃 scan/costmap 消息。优先检查 map->odom、odom->base_link、/scan 时间戳和系统时间。",
            )
        if "goal failed" in lower or "aborting handle" in lower:
            return (
                "nav2_goal_aborted",
                "Nav2 action 已中止，但日志没有给出更细原因。请查看控制器、规划器和 costmap 日志。",
            )
        return ("nav2_goal_failed", "Nav2 目标没有成功完成。请查看诊断控制台里的日志摘要。")

    def _diagnose_navigation_failure(self, goal_state: str, error: str | None = None) -> dict[str, Any]:
        with self._lock:
            pose = deepcopy(self._pose)
        navigation_health = self._navigation_health_state(pose)
        robot_ros = self._robot_ros_link_state()
        path, lines = self._nav_log_tail_lines()
        relevant_lines = self._pick_relevant_nav_log_lines(lines)
        log_text = "\n".join(relevant_lines)
        category, message = self._classify_nav_log_text(log_text)

        if not robot_ros.get("ready"):
            category = "robot_ros_link"
            stale = ", ".join(robot_ros.get("stale_sources") or [])
            if stale:
                message = f"机器人 ROS 源数据不新鲜：{stale}。优先检查网线/DDS/驱动话题。"
            elif robot_ros.get("missing_command_subscriber"):
                message = f"缺少 {robot_ros.get('command_topic') or ROBOT_COMMAND_TOPIC} 指令订阅者，速度指令可能没有进入机器狗控制桥。"
        elif not navigation_health.get("ok"):
            category = "navigation_health"
            message = navigation_health.get("summary") or "scan、TF 或当前位姿不新鲜。"

        next_step_by_category = {
            "nav2_controller_stalled": "先看 RViz 局部代价地图和 /scan 是否把目标前方封死，再看 TF 时间戳丢帧；必要时重设初始位姿后只跑第一个点。",
            "nav2_no_valid_trajectory": "检查目标点附近是否在障碍/膨胀区，确认 /scan 点云、局部 costmap 和机器人 footprint 没有把通路堵死。",
            "tf_time_sync": "检查 /scan stamp、map->odom、odom->base_link、base_link->utlidar_lidar；如果持续出现 extrapolation，先修时间戳/TF 发布链。",
            "robot_ros_link": "检查网线、ens160、DDS、机器人源话题和 /api/sport/request 订阅者。",
            "navigation_health": "先恢复 /scan、TF、当前位姿新鲜度，再重新确认定位。",
            "nav2_goal_aborted": "查看 Nav2 日志中的 controller_server、planner_server、bt_navigator 摘要。",
            "nav2_goal_failed": "查看 Nav2 日志摘要，并结合 scan/TF/pose 健康状态定位。",
        }
        return {
            "category": category,
            "message": message,
            "next_step": next_step_by_category.get(category, "查看诊断控制台和 Nav2 日志摘要。"),
            "goal_state": goal_state,
            "raw_error": error,
            "nav_log_path": str(path) if path else None,
            "nav_log_lines": relevant_lines,
            "navigation_health_summary": navigation_health.get("summary"),
            "navigation_health_ok": bool(navigation_health.get("ok")),
            "robot_ros_ready": bool(robot_ros.get("ready")),
            "robot_ros_stale_sources": robot_ros.get("stale_sources") or [],
        }

    def _localization_state(
        self,
        pose: dict[str, Any] | None,
        map_payload: dict[str, Any] | None,
        nav_launch_pid: int | None,
    ) -> dict[str, Any]:
        current_time = now_s()
        pose_age_s = None
        pose_fresh = False
        if pose and pose.get("received_at_unix_s"):
            pose_age_s = max(0.0, current_time - float(pose["received_at_unix_s"]))
            pose_fresh = self._pose_fresh_enough(pose)

        with self._lock:
            confirmation = deepcopy(self._localization_confirmation)

        state = "confirmed"
        reason = ""
        distance_from_confirmed_m = None
        yaw_delta_from_confirmed_deg = None
        confirmed_age_s = None

        if not pose:
            state = "pose_unavailable"
            reason = "No current pose in map frame; set 2D Pose Estimate first"
        elif not pose_fresh:
            state = "pose_stale"
            reason = "Current map pose is stale; set 2D Pose Estimate again"
        elif not map_payload:
            state = "map_unavailable"
            reason = "Map is unavailable"
        elif not confirmation or confirmation.get("state") != "confirmed":
            state = "needs_confirmation"
            reason = "Operator must confirm the displayed robot position and yaw before inspection"
        else:
            confirmed_age_s = max(0.0, current_time - float(confirmation.get("confirmed_at_unix_s", 0.0)))
            confirmed_pose = confirmation.get("pose") if isinstance(confirmation.get("pose"), dict) else None
            if confirmation.get("map_yaml") != str(self.map_yaml):
                state = "needs_confirmation"
                reason = "Map changed after localization confirmation"
            elif int(confirmation.get("map_revision") or -1) != int(map_payload.get("revision") or -2):
                state = "needs_confirmation"
                reason = "Map revision changed after localization confirmation"
            elif confirmation.get("nav_launch_pid") != nav_launch_pid:
                state = "needs_confirmation"
                reason = "Nav2 was restarted after localization confirmation"
            elif confirmed_age_s > LOCALIZATION_CONFIRM_MAX_AGE_S:
                state = "needs_confirmation"
                reason = "Localization confirmation expired"
            elif not confirmed_pose:
                state = "needs_confirmation"
                reason = "Localization confirmation has no pose snapshot"
            else:
                distance_from_confirmed_m = math.hypot(
                    float(pose["x"]) - float(confirmed_pose["x"]),
                    float(pose["y"]) - float(confirmed_pose["y"]),
                )
                yaw_delta_from_confirmed_deg = abs(normalize_yaw_deg(float(pose["yaw_deg"]) - float(confirmed_pose["yaw_deg"])))
                if distance_from_confirmed_m > LOCALIZATION_CONFIRM_MAX_DRIFT_M:
                    state = "needs_confirmation"
                    reason = "Robot moved after localization confirmation"
                elif yaw_delta_from_confirmed_deg > LOCALIZATION_CONFIRM_MAX_YAW_DRIFT_DEG:
                    state = "needs_confirmation"
                    reason = "Robot yaw changed after localization confirmation"

        return {
            "state": state,
            "confirmed": state == "confirmed",
            "reason": reason,
            "pose_fresh": pose_fresh,
            "pose_age_s": pose_age_s,
            "confirmation": confirmation,
            "confirmed_age_s": confirmed_age_s,
            "distance_from_confirmed_m": distance_from_confirmed_m,
            "yaw_delta_from_confirmed_deg": yaw_delta_from_confirmed_deg,
            "limits": {
                "confirmation_max_age_s": LOCALIZATION_CONFIRM_MAX_AGE_S,
                "confirmation_max_drift_m": LOCALIZATION_CONFIRM_MAX_DRIFT_M,
                "confirmation_max_yaw_drift_deg": LOCALIZATION_CONFIRM_MAX_YAW_DRIFT_DEG,
            },
        }

    def _preflight_state(
        self,
        pose: dict[str, Any] | None,
        map_payload: dict[str, Any] | None,
        nav_launch_pid: int | None,
        nav2_action_ready: bool,
    ) -> dict[str, Any]:
        robot_ros = self._robot_ros_link_state()
        navigation_health = self._navigation_health_state(pose)
        localization = self._localization_state(pose, map_payload, nav_launch_pid)
        try:
            self._assert_obstacle_avoidance_config()
        except Exception as exc:
            obstacle_ok = False
            obstacle_detail = str(exc)
        else:
            obstacle_ok = True
            obstacle_detail = "Nav2 obstacle and inflation layers enabled"
        robot_source_detail = (
            "fresh source topics"
            if robot_ros["sources_fresh"]
            else "Missing fresh robot source data: " + ", ".join(robot_ros["stale_sources"])
        )
        command_detail = (
            f"{robot_ros['command_subscriber_count']} subscriber(s) on {ROBOT_COMMAND_TOPIC}"
            if robot_ros["command_ready"]
            else f"Missing robot command subscriber: {ROBOT_COMMAND_TOPIC}"
        )
        checks = [
            {
                "id": "robot_sources",
                "label": "robot source data",
                "ok": bool(robot_ros["sources_fresh"]),
                "reason": None if robot_ros["sources_fresh"] else robot_source_detail,
                "detail": robot_source_detail,
                "next_step": "Check the Go2 ROS 2/DDS link and VM network interface.",
                "blocking": True,
            },
            {
                "id": "robot_command_subscriber",
                "label": "robot command link",
                "ok": bool(robot_ros["command_ready"]),
                "reason": None if robot_ros["command_ready"] else command_detail,
                "detail": command_detail,
                "next_step": "Start the twist bridge/driver and confirm it subscribes to /api/sport/request.",
                "blocking": False,
            },
            {
                "id": "nav2_launch",
                "label": "Nav2 launch",
                "ok": bool(nav_launch_pid),
                "reason": None if nav_launch_pid else "Nav2 launch is not running",
                "detail": f"pid {nav_launch_pid}" if nav_launch_pid else "not running",
                "next_step": "Click start navigation or run scripts/go2_os_start_nav.sh.",
                "blocking": True,
            },
            {
                "id": "nav2_action",
                "label": "Nav2 action",
                "ok": bool(nav2_action_ready),
                "reason": None if nav2_action_ready else "Nav2 /navigate_to_pose action server is not ready",
                "detail": "action server ready" if nav2_action_ready else "waiting for /navigate_to_pose",
                "next_step": "Wait for the Nav2 lifecycle nodes to become active; check nav logs if this persists.",
                "blocking": True,
            },
            {
                "id": "nav2_obstacle_avoidance",
                "label": "Nav2 obstacle avoidance",
                "ok": obstacle_ok,
                "reason": None if obstacle_ok else obstacle_detail,
                "detail": obstacle_detail,
                "next_step": "Check src/go2_navigation2/config/nav2_params.yaml obstacle and inflation layers.",
                "blocking": False,
            },
            {
                "id": "navigation_health",
                "label": "scan/TF/pose freshness",
                "ok": bool(navigation_health["ok"]),
                "reason": None if navigation_health["ok"] else navigation_health["summary"],
                "detail": navigation_health["summary"],
                "next_step": "Check /scan, map->base TF, AMCL pose, and robot ROS freshness before sending goals.",
                "blocking": False,
            },
            {
                "id": "localization",
                "label": "localization confirmation",
                "ok": bool(localization["confirmed"]),
                "reason": None if localization["confirmed"] else localization["reason"],
                "detail": "operator confirmed" if localization["confirmed"] else localization["reason"],
                "next_step": "Set 2D Pose Estimate, then confirm the displayed position and yaw.",
                "blocking": False,
            },
        ]
        blocking_reasons = [
            str(check["reason"])
            for check in checks
            if check.get("blocking") and not check.get("ok") and check.get("reason")
        ]
        return {
            "navigation_ready": bool(nav_launch_pid and nav2_action_ready and robot_ros["ready"]),
            "inspection_start_ready": not blocking_reasons,
            "blocking_reasons": blocking_reasons,
            "checks": checks,
            "robot_ros": robot_ros,
            "navigation_health": navigation_health,
            "localization": localization,
        }

    def _assert_inspection_preflight(self, pose: dict[str, Any] | None, map_payload: dict[str, Any]) -> None:
        nav_launch_pid = self._nav_launch_pid()
        nav2_action_ready = bool(self.nav_client.server_is_ready())
        preflight = self._preflight_state(pose, map_payload, nav_launch_pid, nav2_action_ready)
        if not preflight["inspection_start_ready"]:
            self.get_logger().warning("Preflight warnings (non-blocking): " + "; ".join(preflight["blocking_reasons"]))

    def _request_amcl_nomotion_update(self) -> dict[str, Any]:
        service_name = "/request_nomotion"
        result: dict[str, Any] = {
            "service": service_name,
            "requested_at_unix_s": now_s(),
            "available": False,
            "called": False,
            "ok": False,
        }
        if not self.amcl_nomotion_client.wait_for_service(timeout_sec=0.2):
            result["reason"] = f"{service_name} service unavailable"
            return result
        result["available"] = True
        future = self.amcl_nomotion_client.call_async(Empty.Request())
        deadline = now_s() + max(0.1, INSPECTION_LOCALIZATION_REFRESH_TIMEOUT_S)
        while now_s() < deadline:
            if future.done():
                break
            time.sleep(0.02)
        if not future.done():
            result["reason"] = f"{service_name} timeout"
            return result
        try:
            future.result()
        except Exception as exc:
            result["reason"] = f"{service_name} failed: {exc}"
            return result
        result["called"] = True
        result["ok"] = True
        return result

    def _wait_for_navigation_health_ok(self, timeout_s: float) -> tuple[bool, dict[str, Any]]:
        deadline = now_s() + max(0.1, timeout_s)
        latest: dict[str, Any] = {"ok": False, "summary": "navigation health not checked"}
        while now_s() < deadline:
            with self._lock:
                pose = deepcopy(self._pose)
            latest = self._navigation_health_state(pose)
            if latest.get("ok"):
                return True, latest
            time.sleep(0.1)
        with self._lock:
            pose = deepcopy(self._pose)
        latest = self._navigation_health_state(pose)
        return bool(latest.get("ok")), latest

    def _refresh_localization_for_inspection(self, run_id: int, stage: str, waypoint_label: str) -> tuple[bool, dict[str, Any]]:
        self._publish_zero_velocity_burst()
        request_result = self._request_amcl_nomotion_update()
        if INSPECTION_LOCALIZATION_REFRESH_SETTLE_S > 0.0:
            time.sleep(INSPECTION_LOCALIZATION_REFRESH_SETTLE_S)
        ok, health = self._wait_for_navigation_health_ok(INSPECTION_LOCALIZATION_REFRESH_TIMEOUT_S)
        result = {
            "stage": stage,
            "waypoint_label": waypoint_label,
            "request_nomotion_update": request_result,
            "navigation_health": health,
            "ok": ok,
            "updated_at_unix_s": now_s(),
        }
        self._patch_inspection_run(
            run_id,
            {
                "localization_refresh": deepcopy(result),
                "status_text": f"{waypoint_label}: localization refreshed" if ok else f"{waypoint_label}: localization refresh failed",
                "updated_at_unix_s": now_s(),
            },
        )
        return ok, result

    @staticmethod
    def _localization_refresh_error_detail(result: dict[str, Any]) -> dict[str, Any]:
        health = result.get("navigation_health") if isinstance(result.get("navigation_health"), dict) else {}
        request_result = result.get("request_nomotion_update") if isinstance(result.get("request_nomotion_update"), dict) else {}
        summary = health.get("summary") or request_result.get("reason") or "AMCL/TF/scan localization refresh failed"
        return {
            "category": "localization_refresh_failed",
            "message": f"Localization remains unreliable after laser refresh: {summary}",
            "next_step": "Reset the initial pose, then verify /go2/scan, map->odom, odom->base_link, base_link->utlidar_lidar, and AMCL/TF consistency before continuing inspection.",
            "navigation_health_summary": health.get("summary"),
            "navigation_health_ok": bool(health.get("ok")),
            "request_nomotion_update": request_result,
            "amcl_tf_delta_m": health.get("amcl_tf_delta_m"),
            "amcl_tf_yaw_delta_deg": health.get("amcl_tf_yaw_delta_deg"),
        }

    def state(self) -> dict[str, Any]:
        with self._lock:
            map_payload = deepcopy(self._map) if self._map else self._load_fallback_map()
            pose = deepcopy(self._pose)
            raw_poses = deepcopy(self._raw_poses)
            goal = deepcopy(self._goal)
            inspection_run = self._inspection_run_view(deepcopy(self._inspection_run))
        map_meta = None
        if map_payload:
            map_meta = {
                key: map_payload[key]
                for key in ("name", "revision", "frame_id", "resolution", "width", "height", "origin", "source")
                if key in map_payload
            }
        nodes = sorted(set(self.get_node_names()))
        pose_age_s = None
        if pose and pose.get("received_at_unix_s"):
            pose_age_s = max(0.0, now_s() - float(pose["received_at_unix_s"]))
        inspection_route = self._inspection_route_summary(map_payload)
        nav_launch_pid = self._nav_launch_pid()
        nav2_action_ready = bool(self.nav_client.server_is_ready())
        preflight = self._preflight_state(pose, map_payload, nav_launch_pid, nav2_action_ready)
        return {
            "name": "openSourceDashboardStates/current",
            "server_time_unix_s": now_s(),
            "mode": "go2_nav2",
            "repo_root": str(self.repo_root),
            "map_yaml": str(self.map_yaml),
            "ros": {
                "available": True,
                "nodes": nodes,
                "nav2_action_ready": nav2_action_ready,
                "nav2_launch_running": nav_launch_pid is not None,
                "nav2_launch_pid": nav_launch_pid,
            },
            "network": {
                "requested_ros_iface": os.environ.get("OPEN_SOURCE_DASHBOARD_REQUESTED_ROS_IFACE"),
                "ros_iface": os.environ.get("OPEN_SOURCE_DASHBOARD_ROS_IFACE"),
                "dds_peers": os.environ.get("OPEN_SOURCE_DASHBOARD_DDS_PEERS"),
                "warning": os.environ.get("OPEN_SOURCE_DASHBOARD_NETWORK_WARNING"),
                "policy_role": os.environ.get("OPEN_SOURCE_DASHBOARD_NETWORK_POLICY"),
                "primary_iface": os.environ.get("OPEN_SOURCE_DASHBOARD_NETWORK_PRIMARY_IFACE"),
                "fallback_iface": os.environ.get("OPEN_SOURCE_DASHBOARD_NETWORK_FALLBACK_IFACE"),
                "selected_iface": os.environ.get("OPEN_SOURCE_DASHBOARD_NETWORK_SELECTED_IFACE"),
                "selection_reason": os.environ.get("OPEN_SOURCE_DASHBOARD_NETWORK_SELECTION_REASON"),
                "blocking_error": os.environ.get("OPEN_SOURCE_DASHBOARD_NETWORK_BLOCKING_ERROR"),
                "fallback_active": os.environ.get("OPEN_SOURCE_DASHBOARD_NETWORK_FALLBACK_ACTIVE") == "1",
            },
            "map": map_meta,
            "pose": pose,
            "amcl_covariance": self._amcl_covariance,
            "pose_age_s": pose_age_s,
            "poses": {
                "selected_source": pose.get("raw_source") if pose else None,
                "raw": raw_poses,
            },
            "pose_pipeline": {
                "config": deepcopy(self.pose_pipeline.config),
                "config_path": str(self.pose_pipeline.config_path),
            },
            "robot_ros": preflight["robot_ros"],
            "navigation_health": preflight["navigation_health"],
            "localization": preflight["localization"],
            "preflight": {
                "navigation_ready": preflight["navigation_ready"],
                "inspection_start_ready": preflight["inspection_start_ready"],
                "blocking_reasons": preflight["blocking_reasons"],
                "checks": preflight["checks"],
            },
            "waypoint_action": self.waypoint_action.status(),
            "goal": goal,
            "inspection_route": inspection_route,
            "inspection_run": inspection_run,
        }

    def _empty_inspection_route(self) -> dict[str, Any]:
        return {
            "name": "inspectionRoutes/default",
            "display_name": "默认巡检路线",
            "map_yaml": str(self.map_yaml),
            "map_revision": None,
            "waypoints": [],
            "updated_at_unix_s": None,
        }

    def load_inspection_route(self) -> dict[str, Any]:
        if not self.route_path.exists():
            return self._empty_inspection_route()
        try:
            route = json.loads(self.route_path.read_text(encoding="utf-8"))
        except json.JSONDecodeError as exc:
            raise RuntimeError(f"Inspection route file is invalid JSON: {exc}") from exc
        if not isinstance(route, dict):
            raise RuntimeError("Inspection route file must contain a JSON object")
        merged = self._empty_inspection_route()
        merged.update(route)
        if not isinstance(merged.get("waypoints"), list):
            merged["waypoints"] = []
        return merged

    def _inspection_route_summary(self, map_payload: dict[str, Any] | None) -> dict[str, Any]:
        try:
            route = self.load_inspection_route()
        except Exception as exc:
            return {
                "name": "inspectionRoutes/default",
                "state": "error",
                "error": str(exc),
                "route_path": str(self.route_path),
            }
        waypoints = [item for item in route.get("waypoints", []) if isinstance(item, dict)]
        enabled = [item for item in waypoints if item.get("enabled", True)]
        current_revision = map_payload.get("revision") if map_payload else None
        route_revision = route.get("map_revision")
        map_match = (
            route.get("map_yaml") == str(self.map_yaml)
            and route_revision is not None
            and current_revision is not None
            and int(route_revision) == int(current_revision)
        )
        return {
            "name": route.get("name", "inspectionRoutes/default"),
            "display_name": route.get("display_name", "默认巡检路线"),
            "route_path": str(self.route_path),
            "waypoint_count": len(waypoints),
            "enabled_waypoint_count": len(enabled),
            "map_yaml": route.get("map_yaml"),
            "map_revision": route_revision,
            "current_map_revision": current_revision,
            "map_match": map_match,
            "updated_at_unix_s": route.get("updated_at_unix_s"),
        }

    def patch_inspection_route(self, body: dict[str, Any], update_mask: str) -> dict[str, Any]:
        fields = {item.strip() for item in update_mask.split(",") if item.strip()}
        if not fields:
            raise ValueError("Missing update_mask")
        unsupported = fields - {"display_name", "waypoints"}
        if unsupported:
            raise ValueError(f"Unsupported update_mask field(s): {', '.join(sorted(unsupported))}")

        route = self.load_inspection_route()
        if "display_name" in fields:
            display_name = str(body.get("display_name", route.get("display_name", "默认巡检路线"))).strip()
            route["display_name"] = display_name or "默认巡检路线"
        if "waypoints" in fields:
            raw_waypoints = body.get("waypoints")
            if not isinstance(raw_waypoints, list):
                raise ValueError("waypoints must be an array")
            route["waypoints"] = self._normalize_waypoints(raw_waypoints)

        map_payload = self.map_payload()
        route["name"] = "inspectionRoutes/default"
        route["map_yaml"] = str(self.map_yaml)
        route["map_revision"] = int(map_payload["revision"])
        route["updated_at_unix_s"] = now_s()
        self._write_inspection_route(route)
        return route

    def _normalize_waypoints(self, raw_waypoints: list[Any]) -> list[dict[str, Any]]:
        if len(raw_waypoints) > 64:
            raise ValueError("waypoints must contain at most 64 items")
        normalized: list[dict[str, Any]] = []
        seen_ids: set[str] = set()
        for index, item in enumerate(raw_waypoints, start=1):
            if not isinstance(item, dict):
                raise ValueError(f"waypoints[{index - 1}] must be an object")
            waypoint_id = str(item.get("id") or f"wp_{index:02d}").strip()
            if not waypoint_id:
                waypoint_id = f"wp_{index:02d}"
            if waypoint_id in seen_ids:
                waypoint_id = f"{waypoint_id}_{index:02d}"
            seen_ids.add(waypoint_id)
            display_name = str(item.get("display_name") or f"巡检点 {index}").strip() or f"巡检点 {index}"
            try:
                x = float(item.get("x"))
                y = float(item.get("y"))
                yaw_deg = float(item.get("yaw_deg"))
            except (TypeError, ValueError) as exc:
                raise ValueError(f"waypoints[{index - 1}] pose values must be numeric") from exc
            if not (math.isfinite(x) and math.isfinite(y) and math.isfinite(yaw_deg)):
                raise ValueError(f"waypoints[{index - 1}] contains non-finite pose values")
            frame_id = str(item.get("frame_id") or "map").strip()
            if frame_id != "map":
                raise ValueError("waypoint frame_id must be map")
            normalized.append(
                {
                    "id": waypoint_id,
                    "display_name": display_name,
                    "x": x,
                    "y": y,
                    "yaw_deg": normalize_yaw_deg(yaw_deg),
                    "frame_id": "map",
                    "enabled": bool(item.get("enabled", True)),
                }
            )
        return normalized

    def _write_inspection_route(self, route: dict[str, Any]) -> None:
        self.route_path.parent.mkdir(parents=True, exist_ok=True)
        temp_path = self.route_path.with_suffix(".json.tmp")
        temp_path.write_text(json.dumps(route, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
        temp_path.replace(self.route_path)

    def load_patrol_route(self) -> dict[str, Any]:
        if not self.patrol_route_path.exists():
            return {"route_name": "default", "frame_id": "map", "created_by": "manual", "waypoints": []}
        raw = yaml.safe_load(self.patrol_route_path.read_text(encoding="utf-8")) or {}
        if not isinstance(raw, dict):
            raw = {}
        waypoints = raw.get("waypoints") or []
        if not isinstance(waypoints, list):
            waypoints = []
        return {
            "route_name": raw.get("route_name", "default"),
            "frame_id": raw.get("frame_id", "map"),
            "created_by": raw.get("created_by", "manual"),
            "waypoints": waypoints,
        }

    def save_patrol_route(self, data: dict[str, Any]) -> dict[str, Any]:
        waypoints = data.get("waypoints")
        if not isinstance(waypoints, list):
            raise ValueError("waypoints must be a list")
        if len(waypoints) > 64:
            raise ValueError("waypoints must contain at most 64 items")
        validated = []
        for i, wp in enumerate(waypoints):
            if not isinstance(wp, dict):
                raise ValueError(f"waypoints[{i}] must be an object")
            name = str(wp.get("name", f"waypoint_{i+1}")).strip() or f"waypoint_{i+1}"
            try:
                x = float(wp["x"])
                y = float(wp["y"])
                yaw = float(wp["yaw"])
            except (KeyError, TypeError, ValueError) as exc:
                raise ValueError(f"waypoints[{i}] x/y/yaw must be numeric") from exc
            enabled = bool(wp.get("enabled", True))
            validated.append({
                "name": name,
                "frame_id": "map",
                "x": round(x, 3),
                "y": round(y, 3),
                "yaw": round(yaw, 3),
                "enabled": enabled,
            })
        route = {
            "route_name": str(data.get("route_name", "default")).strip() or "default",
            "frame_id": "map",
            "created_by": "dashboard",
            "waypoints": validated,
        }
        self.patrol_route_path.parent.mkdir(parents=True, exist_ok=True)
        tmp = self.patrol_route_path.with_suffix(".yaml.tmp")
        tmp.write_text(self._format_patrol_yaml(route), encoding="utf-8")
        tmp.replace(self.patrol_route_path)
        return route

    def _format_patrol_yaml(self, route: dict[str, Any]) -> str:
        lines = []
        lines.append(f"route_name: {route['route_name']}")
        lines.append(f"frame_id: {route['frame_id']}")
        lines.append(f"created_by: {route['created_by']}")
        lines.append("waypoints:")
        for wp in route.get("waypoints", []):
            lines.append(f"  - name: {wp['name']}")
            lines.append(f"    frame_id: {wp['frame_id']}")
            lines.append(f"    x: {wp['x']}")
            lines.append(f"    y: {wp['y']}")
            lines.append(f"    yaw: {wp['yaw']}")
            lines.append(f"    enabled: {'true' if wp.get('enabled', True) else 'false'}")
        return "\n".join(lines) + "\n"


    def map_payload(self) -> dict[str, Any]:

        with self._lock:
            if self._map:
                return deepcopy(self._map)
        fallback = self._load_fallback_map()
        if fallback:
            return fallback
        raise RuntimeError("No /map data received and map file fallback is unavailable")

    def publish_initial_pose(self, pose_data: dict[str, Any]) -> dict[str, Any]:
        x = require_float(pose_data, "x")
        y = require_float(pose_data, "y")
        yaw_deg = require_float(pose_data, "yaw_deg")
        self._invalidate_localization_confirmation("initial_pose_published")
        msg = PoseWithCovarianceStamped()
        msg.header.frame_id = "map"
        msg.header.stamp = Time().to_msg()
        msg.pose.pose.position.x = x
        msg.pose.pose.position.y = y
        qx, qy, qz, qw = yaw_to_quaternion(math.radians(yaw_deg))
        msg.pose.pose.orientation.x = qx
        msg.pose.pose.orientation.y = qy
        msg.pose.pose.orientation.z = qz
        msg.pose.pose.orientation.w = qw
        msg.pose.covariance[0] = 0.25
        msg.pose.covariance[7] = 0.25
        msg.pose.covariance[35] = 0.0685
        for _ in range(3):
            self.initial_pose_pub.publish(msg)
            time.sleep(0.04)
        return {"name": "initialPose/published", "pose": {"x": x, "y": y, "yaw_deg": yaw_deg, "frame_id": "map"}}

    def confirm_localization(self) -> dict[str, Any]:
        map_payload = self.map_payload()
        with self._lock:
            pose = deepcopy(self._pose)
        nav_launch_pid = self._nav_launch_pid()
        nav2_action_ready = bool(self.nav_client.server_is_ready())
        preflight = self._preflight_state(pose, map_payload, nav_launch_pid, nav2_action_ready)
        robot_ros = preflight["robot_ros"]
        localization = preflight["localization"]
        if not nav_launch_pid:
            raise RuntimeError("Nav2 launch is not running")
        if not nav2_action_ready:
            raise RuntimeError("Nav2 /navigate_to_pose action server is not ready")
        if not robot_ros["ready"]:
            reasons = []
            if robot_ros["stale_sources"]:
                reasons.append("missing fresh source data: " + ", ".join(robot_ros["stale_sources"]))
            if robot_ros["missing_command_subscriber"]:
                reasons.append(f"missing command subscriber: {ROBOT_COMMAND_TOPIC}")
            raise RuntimeError("Robot ROS link is not ready: " + "; ".join(reasons))
        if not localization["pose_fresh"] or not pose:
            raise RuntimeError("Current map pose is unavailable or stale; set initial pose first")

        confirmation = {
            "state": "confirmed",
            "confirmed_at_unix_s": now_s(),
            "map_yaml": str(self.map_yaml),
            "map_revision": int(map_payload["revision"]),
            "nav_launch_pid": nav_launch_pid,
            "pose": {
                "x": float(pose["x"]),
                "y": float(pose["y"]),
                "yaw_deg": float(pose["yaw_deg"]),
                "frame_id": "map",
                "source": pose.get("raw_source") or pose.get("source"),
            },
        }
        with self._lock:
            self._localization_confirmation = deepcopy(confirmation)
        return {"name": "localizationChecks/current", **confirmation}

    def _clear_inactive_current_goal_locked(self) -> bool:
        if self._current_goal_handle is None:
            return False
        goal_state = str(self._goal.get("state") or "")
        inspection_state = str(self._inspection_run.get("state") or "")
        if goal_state in GOAL_INACTIVE_STATES and inspection_state not in INSPECTION_ACTIVE_STATES:
            self._current_goal_handle = None
            self._current_goal_sequence = None
            return True
        return False

    def navigate_to_pose(self, pose_data: dict[str, Any]) -> dict[str, Any]:
        x = require_float(pose_data, "x")
        y = require_float(pose_data, "y")
        yaw_deg = require_float(pose_data, "yaw_deg")
        with self._lock:
            self._clear_inactive_current_goal_locked()
            if self._inspection_run.get("state") in INSPECTION_ACTIVE_STATES:
                raise RuntimeError("Inspection route is running; stop it before sending a manual goal")
            if self._current_goal_handle is not None:
                raise RuntimeError("A navigation goal is already active")
        with self._lock:
            pose = deepcopy(self._pose)
        if not pose or now_s() - float(pose.get("received_at_unix_s", 0.0)) > 5.0:
            raise RuntimeError("当前地图位姿不可用或已过期，请先重新设置初始位姿")
        network_blocking_error = os.environ.get("OPEN_SOURCE_DASHBOARD_NETWORK_BLOCKING_ERROR")
        if network_blocking_error:
            raise RuntimeError(f"\u673a\u5668\u4eba\u7f51\u7edc\u94fe\u8def\u5f02\u5e38\uff0c\u7981\u6b62\u4e0b\u53d1\u76ee\u6807\u70b9\uff1a{network_blocking_error}")
        map_payload = self.load_map_payload()
        nav_launch_pid = self._nav_launch_pid()
        nav2_action_ready = bool(self.nav_client.server_is_ready())
        preflight = self._preflight_state(pose, map_payload, nav_launch_pid, nav2_action_ready)
        if not preflight["localization"]["confirmed"]:
            reason = preflight["localization"].get("reason") or "定位未确认"
            raise RuntimeError(f"定位未确认，禁止下发目标点：{reason}。请重新设置初始位姿并点击确认定位。")
        if not self._is_free_goal(x, y):
            raise ValueError("Goal must be inside known free space on the current map")
        if not self.nav_client.wait_for_server(timeout_sec=1.0):
            raise RuntimeError("Nav2 /navigate_to_pose action server is not ready")

        return self._send_navigation_goal(x, y, yaw_deg, context=None)

    def _send_navigation_goal(self, x: float, y: float, yaw_deg: float, context: dict[str, Any] | None, behavior_tree: str | None = None) -> dict[str, Any]:
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = PoseStamped()
        goal_msg.pose.header.frame_id = "map"
        # Nav2 can re-transform a goal during replanning. A zero stamp asks TF
        # for the latest transform and avoids stale-goal extrapolation on
        # multi-waypoint runs.
        goal_msg.pose.header.stamp = Time().to_msg()
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        qx, qy, qz, qw = yaw_to_quaternion(math.radians(yaw_deg))
        goal_msg.pose.pose.orientation.x = qx
        goal_msg.pose.pose.orientation.y = qy
        goal_msg.pose.pose.orientation.z = qz
        goal_msg.pose.pose.orientation.w = qw
        if behavior_tree:
            goal_msg.behavior_tree = behavior_tree

        with self._lock:
            self._goal_sequence += 1
            sequence = self._goal_sequence
            target = {"x": x, "y": y, "yaw_deg": yaw_deg, "frame_id": "map"}
            if behavior_tree:
                target["behavior_tree"] = behavior_tree
            if context:
                target.update(deepcopy(context))
            self._goal = {
                "state": "sending",
                "sequence": sequence,
                "target": target,
                "updated_at_unix_s": now_s(),
            }
        future = self.nav_client.send_goal_async(goal_msg)
        future.add_done_callback(lambda done, goal_sequence=sequence: self._goal_response_callback(done, goal_sequence))
        return {
            "name": "navigateToPose/sending",
            "sequence": sequence,
            "target": target,
        }

    def _is_free_goal(self, x: float, y: float) -> bool:
        grid = self.map_payload()
        origin = grid["origin"]
        resolution = float(grid["resolution"])
        width = int(grid["width"])
        height = int(grid["height"])
        mx = int(math.floor((x - float(origin["x"])) / resolution))
        my = int(math.floor((y - float(origin["y"])) / resolution))
        if mx < 0 or my < 0 or mx >= width or my >= height:
            return False
        cells = grid.get("cells") or []
        value = int(cells[my * width + mx])
        return 0 <= value < 50

    def stop_motion(self) -> dict[str, Any]:
        with self._lock:
            active_run_id = self._inspection_run.get("run_id") if self._inspection_run.get("state") in INSPECTION_ACTIVE_STATES else None
            if active_run_id is not None:
                self._inspection_run["cancel_requested"] = True
                self._inspection_run["state"] = "canceling"
                self._inspection_run["error"] = "\u5df2\u624b\u52a8\u505c\u6b62\u5de1\u68c0"
                self._inspection_run["updated_at_unix_s"] = now_s()
            goal_handle = self._current_goal_handle
            previous_sequence = self._goal_sequence
            self._goal_sequence += 1
            self._goal = {"state": "stopping", "sequence": self._goal_sequence, "updated_at_unix_s": now_s(), "previous": deepcopy(self._goal)}
        if goal_handle is not None:
            try:
                cancel_future = goal_handle.cancel_goal_async()
                cancel_future.add_done_callback(lambda done, goal_sequence=previous_sequence: self._cancel_callback(done, goal_sequence))
            except Exception as exc:
                self.get_logger().warning(f"Failed to cancel current goal: {exc}")
        zero = Twist()
        for _ in range(12):
            self.cmd_vel_pub.publish(zero)
            self.cmd_vel_nav_pub.publish(zero)
            time.sleep(0.04)
        if active_run_id is not None:
            self._finish_inspection_run(active_run_id, "canceled", error="\u5df2\u624b\u52a8\u505c\u6b62\u5de1\u68c0")
        return {"name": "robotCommands/stopMove", "state": "stopping"}

    def start_navigation(self) -> dict[str, Any]:
        # Basic communication check
        robot_ros = self._robot_ros_link_state()
        if not robot_ros["sources_fresh"]:
            stale = ", ".join(robot_ros["stale_sources"])
            raise RuntimeError(f"机器人 ROS 通信未就绪，缺少数据源: {stale}。请检查机器狗是否开机、网络连接是否正常。")
        if not robot_ros["command_ready"]:
            raise RuntimeError(f"机器人指令链路未就绪，缺少订阅者: {ROBOT_COMMAND_TOPIC}。请确保机器狗驱动正在运行。")
        existing_pid = self._nav_launch_pid()
        if existing_pid is not None:
            with self._lock:
                pose = deepcopy(self._pose)
            map_payload = self.map_payload()
            preflight = self._preflight_state(pose, map_payload, existing_pid, bool(self.nav_client.server_is_ready()))
            return {"name": "navigation/alreadyRunning", "pid": existing_pid, "preflight": preflight}
        self.nav_log_dir.mkdir(parents=True, exist_ok=True)
        log_path = self.nav_log_dir / f"nav_{time.strftime('%Y%m%d_%H%M%S')}_dashboard.log"
        env = os.environ.copy()
        env["GO2_OS_USE_RVIZ"] = "true"
        if "DISPLAY" not in env:
            env["DISPLAY"] = ":0"
        if "WAYLAND_DISPLAY" not in env:
            env["WAYLAND_DISPLAY"] = "wayland-0"
        env["XDG_RUNTIME_DIR"] = f"/run/user/{os.getuid()}"
        map_name = self.map_yaml.stem
        cmd = [str(self.repo_root / "scripts" / "go2_start_nav.sh"), map_name]
        self._invalidate_localization_confirmation("navigation_started")
        log_file = log_path.open("ab")
        proc = subprocess.Popen(
            cmd,
            cwd=self.repo_root,
            env=env,
            stdout=log_file,
            stderr=subprocess.STDOUT,
            stdin=subprocess.DEVNULL,
            start_new_session=True,
        )
        (self.nav_log_dir / "nav.pid").write_text(str(proc.pid), encoding="utf-8")
        time.sleep(1.0)
        if proc.poll() is not None:
            log_tail = ""
            try:
                log_tail = "\n".join(log_path.read_text(encoding="utf-8", errors="replace").splitlines()[-20:])
            except Exception:
                pass
            raise RuntimeError(f"Nav2 launch exited early with code {proc.returncode}. {log_tail}")
        with self._lock:
            pose = deepcopy(self._pose)
        map_payload = self.map_payload()
        preflight = self._preflight_state(pose, map_payload, proc.pid, bool(self.nav_client.server_is_ready()))
        return {"name": "navigation/started", "pid": proc.pid, "log_path": str(log_path), "preflight": preflight}

    def stop_navigation(self) -> dict[str, Any]:
        pid = self._nav_launch_pid()
        if pid is None:
            return {"name": "navigation/notRunning"}
        try:
            os.killpg(os.getpgid(pid), signal.SIGTERM)
        except (ProcessLookupError, PermissionError):
            try:
                os.kill(pid, signal.SIGTERM)
            except (ProcessLookupError, PermissionError):
                pass
        time.sleep(1.0)
        still_running = self._nav_launch_pid()
        if still_running:
            try:
                os.killpg(os.getpgid(still_running), signal.SIGKILL)
            except (ProcessLookupError, PermissionError):
                try:
                    os.kill(still_running, signal.SIGKILL)
                except (ProcessLookupError, PermissionError):
                    pass
        pid_file = self.nav_log_dir / "nav.pid"
        if pid_file.exists():
            pid_file.unlink()
        return {"name": "navigation/stopped", "pid": pid}

    def _assert_obstacle_avoidance_config(self) -> None:
        params_path = self.repo_root / "src" / "go2_navigation2" / "config" / "nav2_params.yaml"
        if not params_path.exists():
            raise RuntimeError(f"Nav2 obstacle avoidance config is missing: {params_path}")
        try:

            payload = yaml.safe_load(params_path.read_text(encoding="utf-8")) or {}
            local_params = payload["local_costmap"]["local_costmap"]["ros__parameters"]
            global_params = payload["global_costmap"]["global_costmap"]["ros__parameters"]
            local_plugins = set(local_params.get("plugins") or [])
            global_plugins = set(global_params.get("plugins") or [])
            local_obstacle_enabled = (
                ("voxel_layer" in local_plugins and local_params.get("voxel_layer", {}).get("enabled", True) is not False)
                or ("obstacle_layer" in local_plugins and local_params.get("obstacle_layer", {}).get("enabled", True) is not False)
            )
            global_obstacle_enabled = "obstacle_layer" in global_plugins and global_params.get("obstacle_layer", {}).get("enabled", True) is not False
            local_inflation_enabled = "inflation_layer" in local_plugins and float(local_params.get("inflation_layer", {}).get("inflation_radius", 0.0)) > 0.0
            global_inflation_enabled = "inflation_layer" in global_plugins and float(global_params.get("inflation_layer", {}).get("inflation_radius", 0.0)) > 0.0
        except Exception as exc:
            raise RuntimeError(f"无法校验 Nav2 避障配置：{exc}") from exc
        if not (local_obstacle_enabled and global_obstacle_enabled and local_inflation_enabled and global_inflation_enabled):
            raise RuntimeError("Nav2 避障配置未启用，无法开始导航")

    def start_inspection_route(self) -> dict[str, Any]:
        patrol_data = self.load_patrol_route()
        waypoints = [wp for wp in patrol_data.get("waypoints", []) if wp.get("enabled", True)]
        if not waypoints:
            raise ValueError("巡检路线没有可用巡检点（请在航点编辑器中添加航点）")
        with self._lock:
            self._clear_inactive_current_goal_locked()
            pose = deepcopy(self._pose)
            if self._inspection_run.get("state") in INSPECTION_ACTIVE_STATES:
                raise RuntimeError("巡检路线正在运行中")
            if self._current_goal_handle is not None:
                raise RuntimeError("当前已经有一个导航目标在执行")
        if not self.nav_client.wait_for_server(timeout_sec=1.0):
            raise RuntimeError("Nav2 /navigate_to_pose action server 未就绪")

        route = {
            "name": "inspectionRoutes/patrol",
            "display_name": patrol_data.get("route_name", "default"),
            "waypoints": [
                {
                    "id": f"wp_{i+1}",
                    "display_name": wp.get("name", f"waypoint_{i+1}"),
                    "x": float(wp["x"]),
                    "y": float(wp["y"]),
                    "yaw": float(wp["yaw"]),
                    "yaw_deg": math.degrees(float(wp["yaw"])),
                    "enabled": True,
                }
                for i, wp in enumerate(waypoints)
            ],
        }
        waypoints_for_nav = route["waypoints"]

        with self._lock:
            self._inspection_run_sequence += 1
            run_id = self._inspection_run_sequence
            self._inspection_run = {
                "name": "inspectionRuns/current",
                "run_id": run_id,
                "state": "starting",
                "status_text": "巡检启动中",
                "route_name": route["name"],
                "route_display_name": route["display_name"],
                "current_index": 0,
                "completed_count": 0,
                "total_count": len(waypoints_for_nav),
                "current_waypoint": None,
                "arrival_check": None,
                "realtime_segment": None,
                "cancel_requested": False,
                "started_at_unix_s": now_s(),
                "updated_at_unix_s": now_s(),
                "dwell_started_at_unix_s": None,
                "dwell_duration_s": INSPECTION_DWELL_DURATION_S,
                "dwell_elapsed_s": 0.0,
                "dwell_remaining_s": INSPECTION_DWELL_DURATION_S,
                "dwell_progress_ratio": 0.0,
            }
        thread = threading.Thread(target=self._inspection_route_worker, args=(run_id, route, waypoints_for_nav), daemon=True)
        thread.start()
        return {
            "name": "inspectionRuns/current",
            "run_id": run_id,
            "state": "starting",
            "status_text": "巡检启动中",
            "total_count": len(waypoints_for_nav),
        }

    def cancel_inspection_run(self) -> dict[str, Any]:
        response = self.stop_motion()
        return {"name": "inspectionRuns/current", "state": "canceled", "stop": response}

    def _inspection_route_worker(self, run_id: int, route: dict[str, Any], waypoints: list[dict[str, Any]]) -> None:
        goal_timeout_s = float(os.environ.get("GO2_INSPECTION_GOAL_TIMEOUT_S", "600"))
        arrival_timeout_s = float(os.environ.get("GO2_INSPECTION_ARRIVAL_TIMEOUT_S", "20"))
        localization_ready_for_next = False
        try:
            for index, waypoint in enumerate(waypoints):
                if self._inspection_run_canceled(run_id):
                    return
                waypoint_label = self._inspection_waypoint_label(index)
                context = {
                    "inspection_run_id": run_id,
                    "waypoint_id": waypoint.get("id"),
                    "waypoint_display_name": waypoint.get("display_name"),
                    "waypoint_index": index,
                }
                if localization_ready_for_next:
                    localization_ready_for_next = False
                else:
                    refresh_ok, refresh_result = self._refresh_localization_for_inspection(run_id, "before_navigation", waypoint_label)
                    if not refresh_ok:
                        self._finish_inspection_run(
                            run_id,
                            "failed",
                            error=f"{waypoint_label} localization refresh failed before navigation",
                            error_detail=self._localization_refresh_error_detail(refresh_result),
                        )
                        return
                self._patch_inspection_run(
                    run_id,
                    {
                        "state": "navigating",
                        "status_text": f"前往第 {index + 1} 个点",
                        "current_index": index,
                        "current_waypoint": deepcopy(waypoint),
                        "goal_sequence": None,
                        "arrival_check": None,
                        "realtime_segment": None,
                        "dwell_started_at_unix_s": None,
                        "dwell_elapsed_s": 0.0,
                        "dwell_remaining_s": INSPECTION_DWELL_DURATION_S,
                        "dwell_progress_ratio": 0.0,
                        "updated_at_unix_s": now_s(),
                    },
                )
                nav_yaw_deg = float(waypoint["yaw_deg"])
                result = self._send_navigation_goal(float(waypoint["x"]), float(waypoint["y"]), nav_yaw_deg, context=context, behavior_tree=None)
                sequence = int(result["sequence"])
                self._patch_inspection_run(run_id, {"goal_sequence": sequence, "updated_at_unix_s": now_s()})
                goal_state = self._wait_for_goal_terminal(run_id, sequence, goal_timeout_s)
                if goal_state != "succeeded":
                    failure_detail = None
                    if goal_state in {"aborted", "failed", "timeout"}:
                        failure_detail = self._diagnose_navigation_failure(goal_state)
                    with self._lock:
                        pose_for_fallback = deepcopy(self._pose)
                    distance_for_fallback = None
                    if pose_for_fallback:
                        distance_for_fallback = math.hypot(float(pose_for_fallback["x"]) - float(waypoint["x"]), float(pose_for_fallback["y"]) - float(waypoint["y"]))
                    if goal_state in {"aborted", "failed", "timeout"} and distance_for_fallback is not None and distance_for_fallback <= INSPECTION_NAV_ABORT_NEAR_GOAL_TOLERANCE_M:
                        self._publish_zero_velocity_burst()
                        near_goal_check = {
                            "ready": False,
                            "fresh": True,
                            "in_position": distance_for_fallback <= INSPECTION_NAV_ABORT_NEAR_GOAL_TOLERANCE_M,
                            "in_yaw": False,
                            "stopped": True,
                            "distance_m": distance_for_fallback,
                            "yaw_error_deg": None,
                            "pose_age_s": None,
                            "linear_speed_mps": 0.0,
                            "yaw_rate_deg_s": 0.0,
                            "stable_for_s": 0.0,
                            "required_stable_s": INSPECTION_ARRIVAL_STABLE_HOLD_S,
                            "position_tolerance_m": INSPECTION_NAV_ABORT_NEAR_GOAL_TOLERANCE_M,
                            "yaw_tolerance_deg": INSPECTION_YAW_TOLERANCE_DEG,
                            "near_goal_after_nav_abort": True,
                            "reason": f"Nav2 {goal_state}，但已接近巡检点 {distance_for_fallback:.3f}m，继续原地对准朝向",
                            "failure_category": failure_detail.get("category") if failure_detail else None,
                            "error_detail": failure_detail.get("message") if failure_detail else None,
                            "checked_at_unix_s": now_s(),
                        }
                        self._patch_inspection_run(
                            run_id,
                            {
                                "arrival_check": near_goal_check,
                                "nav_failure_detail": failure_detail,
                                "status_text": f"{waypoint_label}，Nav2近点中止，继续对准朝向",
                                "updated_at_unix_s": now_s(),
                            },
                        )
                    else:
                        self._publish_zero_velocity_burst()
                        if goal_state == "canceled":
                            self._finish_inspection_run(run_id, "canceled", error="已手动停止巡检")
                        elif goal_state == "timeout":
                            self._finish_inspection_run(run_id, "failed", error=f"{waypoint_label}导航超时", error_detail=failure_detail)
                        else:
                            self._finish_inspection_run(run_id, "failed", error=f"{waypoint_label}导航失败（{goal_state}）", error_detail=failure_detail)
                        return

                arrival_message = self._inspection_waypoint_label(index)
                if goal_state != "succeeded":
                    refresh_ok, refresh_result = self._refresh_localization_for_inspection(run_id, "after_navigation", waypoint_label)
                    if not refresh_ok:
                        self._finish_inspection_run(
                            run_id,
                            "failed",
                            error=f"{waypoint_label} localization refresh failed after navigation",
                            error_detail=self._localization_refresh_error_detail(refresh_result),
                        )
                        return
                self._patch_inspection_run(
                    run_id,
                    {
                        "state": "arrived",
                        "status_text": f"{arrival_message}，正在对准朝向",
                        "updated_at_unix_s": now_s(),
                    },
                )
                arrival_position_tolerance_m = INSPECTION_POSITION_TOLERANCE_M
                if goal_state == "succeeded":
                    # Nav2 already accepted the position-only goal. Use a slightly wider
                    # confirmation band here so final yaw alignment is not blocked by
                    # small AMCL/map-frame settling differences after controller success.
                    arrival_position_tolerance_m = max(
                        arrival_position_tolerance_m,
                        INSPECTION_NAV_SUCCESS_NEAR_GOAL_TOLERANCE_M,
                    )
                if (
                    goal_state in {"aborted", "failed", "timeout"}
                    and distance_for_fallback is not None
                    and distance_for_fallback <= INSPECTION_NAV_ABORT_NEAR_GOAL_TOLERANCE_M
                ):
                    arrival_position_tolerance_m = INSPECTION_NAV_ABORT_NEAR_GOAL_TOLERANCE_M
                align_check, align_reason = self._align_waypoint_yaw(run_id, sequence, index, waypoint, arrival_position_tolerance_m)
                if align_reason is not None:
                    self._publish_zero_velocity_burst()
                    if align_reason == "canceled":
                        self._finish_inspection_run(run_id, "canceled", error="已手动停止巡检")
                    else:
                        detail = f"：{align_check.get('reason')}" if align_check and align_check.get("reason") else ""
                        self._finish_inspection_run(run_id, "failed", error=f"{waypoint_label}朝向对准失败{detail}")
                    return
                arrival_check = align_check
                self._patch_inspection_run(
                    run_id,
                    {
                        "state": "arrived",
                        "status_text": arrival_message,
                        "arrival_check": deepcopy(arrival_check),
                        "updated_at_unix_s": now_s(),
                    },
                )

                # --- Position correction: use stationary laser to fix AMCL drift ---
                correction_result = self._correct_position_at_waypoint(run_id, index, waypoint, waypoint_label)
                if correction_result.get("canceled"):
                    self._finish_inspection_run(run_id, "canceled", error="已手动停止巡检")
                    return
                if correction_result.get("failed"):
                    self._finish_inspection_run(run_id, "failed", error=correction_result["error"])
                    return

                # Re-align yaw after position correction (correction may drift heading)
                re_align_check, re_align_reason = self._align_waypoint_yaw(run_id, sequence, index, waypoint, INSPECTION_POSITION_TOLERANCE_M)
                if re_align_reason is not None:
                    self._publish_zero_velocity_burst()
                    if re_align_reason == "canceled":
                        self._finish_inspection_run(run_id, "canceled", error="已手动停止巡检")
                    else:
                        self._finish_inspection_run(run_id, "failed", error=f"{waypoint_label}修正后朝向对准失败")
                    return
                segment_result = self._call_realtime_segment_for_waypoint(run_id, index, waypoint)
                self._patch_inspection_run(
                    run_id,
                    {
                        "realtime_segment": deepcopy(segment_result),
                        "updated_at_unix_s": now_s(),
                    },
                )
                if self._inspection_run_canceled(run_id):
                    self._finish_inspection_run(run_id, "canceled", error="已手动停止巡检")
                    return
                if not segment_result.get("continue_inspection", True):
                    self._publish_zero_velocity_burst()
                    self._finish_inspection_run(
                        run_id,
                        "failed",
                        error=f"{waypoint_label}实时识别接口调用失败：{segment_result.get('message', '未知错误')}。已停止巡检。",
                    )
                    return

                self._patch_inspection_run(
                    run_id,
                    {
                        "state": "dwelling",
                        "status_text": arrival_message,
                        "arrival_check": deepcopy(arrival_check),
                        "dwell_started_at_unix_s": now_s(),
                        "dwell_duration_s": INSPECTION_DWELL_DURATION_S,
                        "dwell_elapsed_s": 0.0,
                        "dwell_remaining_s": INSPECTION_DWELL_DURATION_S,
                        "dwell_progress_ratio": 0.0,
                        "updated_at_unix_s": now_s(),
                    },
                )
                dwell_ok, dwell_reason, dwell_check = self._run_inspection_dwell(run_id, sequence, index, waypoint, arrival_message)
                if not dwell_ok:
                    self._publish_zero_velocity_burst()
                    if dwell_reason == "canceled":
                        self._finish_inspection_run(run_id, "canceled", error="已手动停止巡检")
                    else:
                        detail = f"：{dwell_reason}" if dwell_reason else ""
                        self._finish_inspection_run(run_id, "failed", error=f"{waypoint_label}停留失败{detail}")
                    return

                next_count = index + 1
                if next_count < len(waypoints):
                    next_waypoint = deepcopy(waypoints[next_count])
                    refresh_ok, refresh_result = self._refresh_localization_for_inspection(run_id, "before_next_waypoint", waypoint_label)
                    if not refresh_ok:
                        self._finish_inspection_run(
                            run_id,
                            "failed",
                            error=f"{waypoint_label} localization refresh failed before next waypoint",
                            error_detail=self._localization_refresh_error_detail(refresh_result),
                        )
                        return
                    localization_ready_for_next = True
                    self._patch_inspection_run(
                        run_id,
                        {
                            "state": "navigating",
                            "status_text": f"前往第 {next_count + 1} 个点",
                            "current_index": next_count,
                            "current_waypoint": next_waypoint,
                            "completed_count": next_count,
                            "goal_sequence": None,
                            "arrival_check": None,
                            "realtime_segment": None,
                            "dwell_started_at_unix_s": None,
                            "dwell_elapsed_s": 0.0,
                            "dwell_remaining_s": INSPECTION_DWELL_DURATION_S,
                            "dwell_progress_ratio": 0.0,
                            "updated_at_unix_s": now_s(),
                        },
                    )
                    continue

                self._patch_inspection_run(
                    run_id,
                    {
                        "state": "completed",
                        "status_text": "巡检完成",
                        "completed_count": next_count,
                        "arrival_check": deepcopy(dwell_check or arrival_check),
                        "dwell_elapsed_s": INSPECTION_DWELL_DURATION_S,
                        "dwell_remaining_s": 0.0,
                        "dwell_progress_ratio": 1.0,
                        "finished_at_unix_s": now_s(),
                        "updated_at_unix_s": now_s(),
                    },
                )
                return
            self._finish_inspection_run(run_id, "completed", error=None)
        except Exception as exc:
            self._publish_zero_velocity_burst()
            self._finish_inspection_run(run_id, "failed", error=f"巡检异常：{exc}")


    def _correct_position_at_waypoint(self, run_id: int, index: int, waypoint, waypoint_label: str) -> dict[str, Any]:
        """Correct position using direct cmd_vel XY translation (sport mode)."""
        import time as _time
        result: dict[str, Any] = {"corrected": False, "canceled": False, "failed": False}

        self._patch_inspection_run(
            run_id,
            {
                "state": "correcting",
                "status_text": f"{waypoint_label}，位置修正中",
                "updated_at_unix_s": now_s(),
            },
        )

        # Stop and wait for gait to settle
        self._publish_zero_velocity_burst()
        _time.sleep(1.5)

        # Force AMCL to process stationary laser scan
        nomotion_result = self._request_amcl_nomotion_update()
        _time.sleep(1.0)

        # Check position error after AMCL update
        with self._lock:
            pose = deepcopy(self._pose)
        if not pose or pose.get("x") is None:
            result["error"] = f"{waypoint_label}位置修正失败：无法获取当前位姿"
            result["failed"] = True
            return result

        wp_x = float(waypoint.get("x", 0))
        wp_y = float(waypoint.get("y", 0))
        current_x = float(pose["x"])
        current_y = float(pose["y"])
        position_error = math.hypot(wp_x - current_x, wp_y - current_y)

        self._patch_inspection_run(
            run_id,
            {
                "state": "correcting",
                "status_text": f"{waypoint_label}，位置误差 {position_error:.3f}m，修正中",
                "position_correction": {
                    "initial_error_m": position_error,
                    "threshold_m": INSPECTION_POSITION_CORRECTION_THRESHOLD_M,
                    "nomotion_update": nomotion_result,
                    "updated_at_unix_s": now_s(),
                },
                "updated_at_unix_s": now_s(),
            },
        )

        # If error is within threshold, no correction needed
        if position_error <= INSPECTION_POSITION_CORRECTION_THRESHOLD_M:
            self._patch_inspection_run(
                run_id,
                {
                    "state": "correcting",
                    "status_text": f"{waypoint_label}，位置修正完成（误差 {position_error:.3f}m）",
                    "position_correction": {
                        "initial_error_m": position_error,
                        "final_error_m": position_error,
                        "correction_needed": False,
                        "threshold_m": INSPECTION_POSITION_CORRECTION_THRESHOLD_M,
                        "updated_at_unix_s": now_s(),
                    },
                    "updated_at_unix_s": now_s(),
                },
            )
            result["corrected"] = True
            result["initial_error_m"] = position_error
            result["final_error_m"] = position_error
            return result

        # Position error exceeds threshold - use direct cmd_vel correction
        self.get_logger().info(
            f"Position correction at {waypoint_label}: error={position_error:.3f}m > threshold={INSPECTION_POSITION_CORRECTION_THRESHOLD_M}m, using cmd_vel"
        )

        if self._inspection_run_canceled(run_id):
            result["canceled"] = True
            return result

        # Direct cmd_vel proportional control loop
        CORRECTION_MAX_SPEED = 0.25
        CORRECTION_MIN_SPEED = 0.20
        CORRECTION_TIMEOUT_S = 30.0
        CORRECTION_SETTLE_S = 0.8
        CORRECTION_LOOP_HZ = 30.0

        deadline = now_s() + CORRECTION_TIMEOUT_S
        converged = False

        while now_s() < deadline:
            if self._inspection_run_canceled(run_id):
                self._publish_zero_velocity_burst()
                result["canceled"] = True
                return result

            with self._lock:
                pose = deepcopy(self._pose)
            if not pose or pose.get("x") is None:
                _time.sleep(1.0 / CORRECTION_LOOP_HZ)
                continue

            current_x = float(pose["x"])
            current_y = float(pose["y"])
            current_yaw_rad = math.radians(float(pose["yaw_deg"]))
            err_x_map = wp_x - current_x
            err_y_map = wp_y - current_y
            dist = math.hypot(err_x_map, err_y_map)

            if dist <= INSPECTION_POSITION_CORRECTION_THRESHOLD_M:
                converged = True
                break

            # Transform map-frame error to base_link frame
            cos_yaw = math.cos(-current_yaw_rad)
            sin_yaw = math.sin(-current_yaw_rad)
            err_x_base = cos_yaw * err_x_map - sin_yaw * err_y_map
            err_y_base = sin_yaw * err_x_map + cos_yaw * err_y_map

            # Proportional speed with min/max clamp
            speed = min(CORRECTION_MAX_SPEED, max(CORRECTION_MIN_SPEED, dist * 0.5))
            vx = (err_x_base / dist) * speed
            vy = (err_y_base / dist) * speed

            twist = Twist()
            twist.linear.x = vx
            twist.linear.y = vy
            twist.angular.z = 0.0
            self.cmd_vel_pub.publish(twist)

            _time.sleep(1.0 / CORRECTION_LOOP_HZ)

        # Stop movement
        self._publish_zero_velocity_burst()
        _time.sleep(CORRECTION_SETTLE_S)

        # Refresh localization after correction
        self._request_amcl_nomotion_update()
        _time.sleep(0.8)

        # Check final position error
        with self._lock:
            pose = deepcopy(self._pose)
        final_error = position_error
        if pose and pose.get("x") is not None:
            final_error = math.hypot(wp_x - float(pose["x"]), wp_y - float(pose["y"]))

        converge_text = "ok" if converged else "timeout"
        self._patch_inspection_run(
            run_id,
            {
                "state": "correcting",
                "status_text": f"{waypoint_label}，位置修正完成（{position_error:.3f}m -> {final_error:.3f}m {converge_text}）",
                "position_correction": {
                    "initial_error_m": position_error,
                    "final_error_m": final_error,
                    "correction_needed": True,
                    "converged": converged,
                    "method": "cmd_vel_direct",
                    "threshold_m": INSPECTION_POSITION_CORRECTION_THRESHOLD_M,
                    "updated_at_unix_s": now_s(),
                },
                "updated_at_unix_s": now_s(),
            },
        )

        self.get_logger().info(
            f"Position correction complete at {waypoint_label}: {position_error:.3f}m -> {final_error:.3f}m ({converge_text})"
        )
        result["corrected"] = True
        result["initial_error_m"] = position_error
        result["final_error_m"] = final_error
        return result

    def _call_realtime_segment_for_waypoint(self, run_id: int, index: int, waypoint: dict[str, Any]) -> dict[str, Any]:
        url = os.environ.get("GO2_REALTIME_SEGMENT_URL", REALTIME_SEGMENT_DEFAULT_URL).strip() or REALTIME_SEGMENT_DEFAULT_URL
        image_field = os.environ.get("GO2_REALTIME_SEGMENT_IMAGE_FIELD", REALTIME_SEGMENT_DEFAULT_IMAGE_FIELD).strip() or REALTIME_SEGMENT_DEFAULT_IMAGE_FIELD
        timeout_s = max(1.0, safe_float_env("GO2_REALTIME_SEGMENT_TIMEOUT_S", 8.0))
        frame_timeout_s = max(0.1, safe_float_env("GO2_REALTIME_SEGMENT_FRAME_TIMEOUT_S", 2.0))
        continue_on_failure = env_bool("GO2_REALTIME_SEGMENT_CONTINUE_ON_FAILURE", False)
        waypoint_label = self._inspection_waypoint_label(index)
        started_at = now_s()
        base_result = {
            "name": f"inspectionRuns/{run_id}/realtimeSegmentCalls/{index + 1}",
            "state": "running",
            "ok": False,
            "method": "POST",
            "url": url,
            "image_field": image_field,
            "waypoint_index": index,
            "waypoint_id": waypoint.get("id"),
            "waypoint_display_name": waypoint.get("display_name"),
            "continue_on_failure": continue_on_failure,
            "decision": "调用中，尚未决定是否继续巡检",
            "started_at_unix_s": started_at,
            "updated_at_unix_s": started_at,
        }
        self._patch_inspection_run(
            run_id,
            {
                "state": "segmenting",
                "status_text": f"{waypoint_label}，调用实时识别接口",
                "realtime_segment": deepcopy(base_result),
                "updated_at_unix_s": now_s(),
            },
        )

        def finish_failure(message: str, **extra: Any) -> dict[str, Any]:
            finished_at = now_s()
            decision = "接口调用失败，已继续巡检" if continue_on_failure else "接口调用失败，已停止巡检"
            return {
                **base_result,
                **extra,
                "state": "failed",
                "ok": False,
                "message": message,
                "continue_inspection": continue_on_failure,
                "decision": decision,
                "finished_at_unix_s": finished_at,
                "updated_at_unix_s": finished_at,
                "duration_s": max(0.0, finished_at - started_at),
            }

        sampler = self.camera_sampler
        if sampler is None:
            return finish_failure("摄像头采样器未初始化，无法上传图片")

        frame_item = sampler.wait_for_fresh_frame(frame_timeout_s, REALTIME_SEGMENT_FRAME_MAX_AGE_S)
        if frame_item is None:
            camera_status = sampler.status()
            if camera_status.get("error"):
                reason = f"没有取得可上传图片：{camera_status['error']}"
            elif camera_status.get("frame_age_s") is not None:
                reason = f"没有取得新鲜图片，最近一帧已过期 {float(camera_status['frame_age_s']):.1f} 秒"
            else:
                reason = "没有取得可上传图片，摄像头暂未返回 JPEG 帧"
            return finish_failure(reason, camera=camera_status)

        frame_id, frame, frame_updated_at = frame_item
        fields = {
            "inspection_run_id": str(run_id),
            "waypoint_index": str(index),
            "waypoint_display_name": str(waypoint.get("display_name") or waypoint_label),
            "waypoint_id": str(waypoint.get("id") or ""),
        }
        body, content_type = encode_multipart_form_data(fields, image_field, "go2-inspection-frame.jpg", "image/jpeg", frame)
        request = Request(
            url,
            data=body,
            method="POST",
            headers={
                "Accept": "application/json",
                "Content-Type": content_type,
                "Content-Length": str(len(body)),
            },
        )
        try:
            with urlopen(request, timeout=timeout_s) as response:
                status = int(response.status)
                response_text = response.read(1024 * 1024).decode("utf-8", "replace")
        except HTTPError as exc:
            response_text = exc.read(1024 * 1024).decode("utf-8", "replace")
            response_payload = parse_json_response(response_text)
            message = extract_response_error(response_payload, f"HTTP {exc.code}")
            return finish_failure(
                f"实时识别接口返回 HTTP {exc.code}：{message}",
                http_status=int(exc.code),
                response_json=response_payload,
                response_text=truncate_text(response_text),
                frame_id=frame_id,
                frame_age_s=max(0.0, now_s() - frame_updated_at),
            )
        except (TimeoutError, URLError, OSError) as exc:
            return finish_failure(
                f"实时识别接口请求失败：{exc}",
                frame_id=frame_id,
                frame_age_s=max(0.0, now_s() - frame_updated_at),
            )

        response_payload = parse_json_response(response_text)
        finished_at = now_s()
        return {
            **base_result,
            "state": "succeeded",
            "ok": True,
            "message": "实时识别接口调用成功",
            "continue_inspection": True,
            "decision": "接口调用成功，继续巡检",
            "http_status": status,
            "response_json": response_payload,
            "response_text": truncate_text(response_text),
            "frame_id": frame_id,
            "frame_age_s": max(0.0, finished_at - frame_updated_at),
            "finished_at_unix_s": finished_at,
            "updated_at_unix_s": finished_at,
            "duration_s": max(0.0, finished_at - started_at),
        }

    def _wait_for_goal_terminal(self, run_id: int, sequence: int, timeout_s: float) -> str:
        deadline = now_s() + max(1.0, timeout_s)
        while now_s() < deadline:
            if self._inspection_run_canceled(run_id):
                return "canceled"
            with self._lock:
                goal = deepcopy(self._goal)
            if goal.get("sequence") == sequence and goal.get("state") in GOAL_TERMINAL_STATES:
                return str(goal.get("state"))
            time.sleep(0.2)
        return "timeout"

    def _inspection_run_canceled(self, run_id: int) -> bool:
        with self._lock:
            return self._inspection_run.get("run_id") != run_id or bool(self._inspection_run.get("cancel_requested")) or self._inspection_run.get("state") == "canceled"

    def _patch_inspection_run(self, run_id: int, patch: dict[str, Any]) -> None:
        with self._lock:
            if self._inspection_run.get("run_id") != run_id or self._inspection_run.get("state") not in INSPECTION_ACTIVE_STATES:
                return
            self._inspection_run.update(deepcopy(patch))

    def _finish_inspection_run(
        self,
        run_id: int,
        state: str,
        error: str | None = None,
        error_detail: dict[str, Any] | None = None,
    ) -> None:
        with self._lock:
            if self._inspection_run.get("run_id") != run_id:
                return
            self._inspection_run["state"] = state
            self._inspection_run["cancel_requested"] = state == "canceled"
            self._inspection_run["error"] = error
            if error_detail:
                self._inspection_run["error_detail"] = error_detail.get("message")
                self._inspection_run["failure_category"] = error_detail.get("category")
                self._inspection_run["failure_next_step"] = error_detail.get("next_step")
                self._inspection_run["diagnostics"] = deepcopy(error_detail)
            else:
                self._inspection_run.pop("error_detail", None)
                self._inspection_run.pop("failure_category", None)
                self._inspection_run.pop("failure_next_step", None)
                self._inspection_run.pop("diagnostics", None)
            self._inspection_run["operator_required"] = state == "failed"
            self._inspection_run["finished_at_unix_s"] = now_s()
            self._inspection_run["updated_at_unix_s"] = now_s()

    def _inspection_run_view(self, inspection_run: dict[str, Any]) -> dict[str, Any]:
        state = inspection_run.get("state")
        dwell_duration_s = max(0.0, float(inspection_run.get("dwell_duration_s") or INSPECTION_DWELL_DURATION_S))
        dwell_started_at = inspection_run.get("dwell_started_at_unix_s")
        if state == "completed":
            inspection_run["dwell_duration_s"] = dwell_duration_s
            inspection_run["dwell_elapsed_s"] = dwell_duration_s
            inspection_run["dwell_remaining_s"] = 0.0
            inspection_run["dwell_progress_ratio"] = 1.0 if dwell_duration_s > 0.0 else 1.0
            return inspection_run
        if state not in {"arrived", "dwelling"}:
            return inspection_run
        if not dwell_started_at:
            inspection_run["dwell_duration_s"] = dwell_duration_s
            inspection_run.setdefault("dwell_elapsed_s", 0.0)
            inspection_run.setdefault("dwell_remaining_s", dwell_duration_s)
            inspection_run.setdefault("dwell_progress_ratio", 0.0)
            return inspection_run
        elapsed = max(0.0, now_s() - float(dwell_started_at))
        remaining = max(0.0, dwell_duration_s - elapsed)
        ratio = 1.0 if dwell_duration_s <= 0.0 else min(1.0, elapsed / dwell_duration_s)
        inspection_run["dwell_duration_s"] = dwell_duration_s
        inspection_run["dwell_elapsed_s"] = elapsed
        inspection_run["dwell_remaining_s"] = remaining
        inspection_run["dwell_progress_ratio"] = ratio
        return inspection_run

    def _inspection_waypoint_label(self, index: int) -> str:
        if index <= 0:
            return "已到第 1 个点（初始点）"
        return f"已到第 {index + 1} 个点"

    def _inspection_waypoint_check(
        self,
        waypoint: dict[str, Any],
        last_pose: dict[str, Any] | None,
        stable_since: float | None,
        required_stable_s: float,
        position_tolerance_m: float | None = None,
    ) -> tuple[dict[str, Any], dict[str, Any] | None, float | None]:
        position_tolerance_m = INSPECTION_POSITION_TOLERANCE_M if position_tolerance_m is None else max(0.01, float(position_tolerance_m))
        with self._lock:
            pose = deepcopy(self._pose)
        current_time = now_s()
        latest_check: dict[str, Any] = {
            "ready": False,
            "fresh": False,
            "in_position": False,
            "in_yaw": False,
            "stopped": False,
            "distance_m": None,
            "yaw_error_deg": None,
            "pose_age_s": None,
            "linear_speed_mps": 0.0,
            "yaw_rate_deg_s": 0.0,
            "stable_for_s": 0.0,
            "required_stable_s": required_stable_s,
            "position_tolerance_m": position_tolerance_m,
            "yaw_tolerance_deg": INSPECTION_YAW_TOLERANCE_DEG,
            "pose_freshness_limit_s": INSPECTION_POSE_FRESHNESS_S,
            "max_linear_speed_mps": INSPECTION_MAX_LINEAR_SPEED_MPS,
            "max_yaw_rate_deg_s": INSPECTION_MAX_YAW_RATE_DEG_S,
            "reason": "定位数据不可用",
            "checked_at_unix_s": current_time,
        }
        if not pose:
            return latest_check, None, None

        pose_age_s = max(0.0, current_time - float(pose.get("received_at_unix_s", 0.0)))
        distance_m = math.hypot(float(pose["x"]) - float(waypoint["x"]), float(pose["y"]) - float(waypoint["y"]))
        signed_yaw_error_deg = normalize_yaw_deg(float(waypoint["yaw_deg"]) - float(pose["yaw_deg"]))
        yaw_error_deg = abs(signed_yaw_error_deg)
        linear_speed_mps = 0.0
        yaw_rate_deg_s = 0.0
        if last_pose and current_time > float(last_pose.get("_checked_at_unix_s", 0.0)):
            dt = current_time - float(last_pose["_checked_at_unix_s"])
            linear_speed_mps = math.hypot(
                float(pose["x"]) - float(last_pose["x"]),
                float(pose["y"]) - float(last_pose["y"]),
            ) / max(dt, 0.001)
            yaw_rate_deg_s = abs(normalize_yaw_deg(float(pose["yaw_deg"]) - float(last_pose["yaw_deg"]))) / max(dt, 0.001)

        fresh = pose_age_s <= INSPECTION_POSE_FRESHNESS_S
        in_position = distance_m <= position_tolerance_m
        in_yaw = yaw_error_deg <= INSPECTION_YAW_TOLERANCE_DEG
        stopped = linear_speed_mps <= INSPECTION_MAX_LINEAR_SPEED_MPS and yaw_rate_deg_s <= INSPECTION_MAX_YAW_RATE_DEG_S
        if fresh and in_position and in_yaw and stopped:
            stable_since = stable_since or current_time
        else:
            stable_since = None
        stable_for_s = 0.0 if stable_since is None else max(0.0, current_time - stable_since)
        ready = fresh and in_position and in_yaw and stopped and stable_for_s >= max(0.0, required_stable_s)
        if not fresh:
            reason = "\u5b9a\u4f4d\u6570\u636e\u5df2\u8fc7\u671f"
        elif not in_position:
            reason = "\u4f4d\u7f6e\u504f\u5dee\u8d85\u51fa\u9608\u503c"
        elif not in_yaw:
            reason = "\u671d\u5411\u504f\u5dee\u8d85\u51fa\u9608\u503c"
        elif not stopped:
            reason = "\u673a\u5668\u4eba\u5c1a\u672a\u505c\u7a33"
        elif stable_for_s < max(0.0, required_stable_s):
            reason = f"等待稳定保持 {stable_for_s:.1f}/{max(0.0, required_stable_s):.1f}s"
        else:
            reason = None

        latest_check.update(
            {
                "ready": ready,
                "fresh": fresh,
                "in_position": in_position,
                "in_yaw": in_yaw,
                "stopped": stopped,
                "distance_m": distance_m,
                "yaw_error_deg": yaw_error_deg,
                "signed_yaw_error_deg": signed_yaw_error_deg,
                "pose_age_s": pose_age_s,
                "linear_speed_mps": linear_speed_mps,
                "yaw_rate_deg_s": yaw_rate_deg_s,
                "stable_for_s": stable_for_s,
                "reason": reason,
            }
        )
        pose_snapshot = deepcopy(pose)
        pose_snapshot["_checked_at_unix_s"] = current_time
        return latest_check, pose_snapshot, stable_since

    def _navigation_yaw_for_position_goal(self, waypoint: dict[str, Any]) -> float:
        with self._lock:
            pose = deepcopy(self._pose)
        if pose and now_s() - float(pose.get("received_at_unix_s", 0.0)) <= INSPECTION_POSE_FRESHNESS_S:
            return float(pose["yaw_deg"])
        return float(waypoint["yaw_deg"])

    @staticmethod
    def _heading_to_waypoint(current_waypoint: dict[str, Any], next_waypoint: dict[str, Any]) -> float:
        dx = float(next_waypoint["x"]) - float(current_waypoint["x"])
        dy = float(next_waypoint["y"]) - float(current_waypoint["y"])
        if math.hypot(dx, dy) < 0.05:
            return float(next_waypoint.get("yaw_deg", current_waypoint.get("yaw_deg", 0.0)))
        return math.degrees(math.atan2(dy, dx))

    def _publish_yaw_align_command(self, yaw_error_deg: float) -> float:
        twist = Twist()
        abs_error = abs(yaw_error_deg)
        if abs_error <= INSPECTION_YAW_TOLERANCE_DEG:
            self.cmd_vel_nav_pub.publish(twist)
            self.cmd_vel_pub.publish(twist)
            return 0.0
        max_rate = max(INSPECTION_YAW_ALIGN_MIN_RATE_RAD_S, INSPECTION_YAW_ALIGN_MAX_RATE_RAD_S)
        min_rate = min(INSPECTION_YAW_ALIGN_MIN_RATE_RAD_S, max_rate)
        slowdown = max(INSPECTION_YAW_TOLERANCE_DEG, INSPECTION_YAW_ALIGN_SLOWDOWN_DEG)
        ratio = min(1.0, max(0.0, (abs_error - INSPECTION_YAW_TOLERANCE_DEG) / max(0.001, slowdown - INSPECTION_YAW_TOLERANCE_DEG)))
        rate = min_rate + (max_rate - min_rate) * ratio
        twist.angular.z = math.copysign(rate, yaw_error_deg)
        # chain as Nav2. Publishing them directly to /cmd_vel races with the
        # velocity_smoother final output and can be overwritten by zero commands.
        self.cmd_vel_nav_pub.publish(twist)
        return math.degrees(twist.angular.z)

    def _align_waypoint_yaw(
        self,
        run_id: int,
        sequence: int,
        index: int,
        waypoint: dict[str, Any],
        position_tolerance_m: float | None = None,
        status_text: str | None = None,
    ) -> tuple[dict[str, Any] | None, str | None]:
        deadline = now_s() + max(1.0, INSPECTION_YAW_ALIGN_TIMEOUT_S)
        stable_since: float | None = None
        last_pose: dict[str, Any] | None = None
        latest_check: dict[str, Any] | None = None
        while now_s() < deadline:
            if self._inspection_run_canceled(run_id):
                return latest_check, "canceled"
            latest_check, pose_snapshot, stable_since = self._inspection_waypoint_check(
                waypoint,
                last_pose,
                stable_since,
                INSPECTION_ARRIVAL_STABLE_HOLD_S,
                position_tolerance_m,
            )
            last_pose = pose_snapshot
            yaw_error = latest_check.get("signed_yaw_error_deg")
            command_rate_deg_s = 0.0
            if latest_check.get("fresh") and latest_check.get("in_position") and yaw_error is not None:
                command_rate_deg_s = self._publish_yaw_align_command(float(yaw_error))
            else:
                self._publish_zero_velocity_burst()
            latest_check["yaw_aligning"] = True
            latest_check["yaw_command_deg_s"] = command_rate_deg_s
            self._patch_inspection_run(
                run_id,
                {
                    "arrival_check": deepcopy(latest_check),
                    "status_text": status_text or f"{self._inspection_waypoint_label(index)}，正在对准朝向",
                    "updated_at_unix_s": now_s(),
                },
            )
            if latest_check.get("ready"):
                self._publish_zero_velocity_burst()
                return latest_check, None
            time.sleep(0.1)
        self._publish_zero_velocity_burst()
        return latest_check, "timeout"

    def _wait_for_waypoint_arrival(
        self,
        run_id: int,
        sequence: int,
        index: int,
        waypoint: dict[str, Any],
        timeout_s: float,
        position_tolerance_m: float | None = None,
    ) -> tuple[dict[str, Any] | None, str | None]:
        deadline = now_s() + max(1.0, timeout_s)
        stable_since: float | None = None
        last_pose: dict[str, Any] | None = None
        latest_check: dict[str, Any] | None = None
        while now_s() < deadline:
            if self._inspection_run_canceled(run_id):
                return None, "canceled"
            latest_check, pose_snapshot, stable_since = self._inspection_waypoint_check(
                waypoint,
                last_pose,
                stable_since,
                INSPECTION_ARRIVAL_STABLE_HOLD_S,
                position_tolerance_m,
            )
            last_pose = pose_snapshot
            self._patch_inspection_run(
                run_id,
                {
                    "arrival_check": deepcopy(latest_check),
                    "updated_at_unix_s": now_s(),
                },
            )
            if latest_check.get("ready"):
                return latest_check, None
            time.sleep(0.1)
        return latest_check, "timeout"

    def _run_inspection_dwell(
        self,
        run_id: int,
        sequence: int,
        index: int,
        waypoint: dict[str, Any],
        dwell_message: str,
    ) -> tuple[bool, str | None, dict[str, Any] | None]:
        dwell_duration_s = INSPECTION_DWELL_DURATION_S
        dwell_started_at = now_s()
        self._patch_inspection_run(
            run_id,
            {
                "state": "dwelling",
                "status_text": dwell_message,
                "dwell_started_at_unix_s": dwell_started_at,
                "dwell_duration_s": dwell_duration_s,
                "dwell_elapsed_s": 0.0,
                "dwell_remaining_s": dwell_duration_s,
                "dwell_progress_ratio": 0.0,
                "updated_at_unix_s": now_s(),
            },
        )
        deadline = dwell_started_at + dwell_duration_s
        stable_since: float | None = None
        last_pose: dict[str, Any] | None = None
        latest_check: dict[str, Any] | None = None
        while now_s() < deadline:
            if self._inspection_run_canceled(run_id):
                return False, "canceled", latest_check
            latest_check, pose_snapshot, stable_since = self._inspection_waypoint_check(
                waypoint,
                last_pose,
                stable_since,
                0.0,
            )
            last_pose = pose_snapshot
            if latest_check:
                dwell_ready = bool(latest_check.get("fresh")) and bool(latest_check.get("stopped"))
                latest_check["ready"] = dwell_ready
                if dwell_ready:
                    latest_check["reason"] = None
                elif not latest_check.get("fresh"):
                    latest_check["reason"] = "\u5b9a\u4f4d\u6570\u636e\u5df2\u8fc7\u671f"
                else:
                    latest_check["reason"] = "\u673a\u5668\u4eba\u5c1a\u672a\u505c\u7a33"
            if latest_check and not latest_check.get("fresh"):
                return False, "\u5b9a\u4f4d\u6570\u636e\u5df2\u8fc7\u671f", latest_check
            time.sleep(0.1)
        self._patch_inspection_run(
            run_id,
            {
                "dwell_elapsed_s": dwell_duration_s,
                "dwell_remaining_s": 0.0,
                "dwell_progress_ratio": 1.0,
                "updated_at_unix_s": now_s(),
            },
        )
        return True, None, latest_check

    def _goal_response_callback(self, future: Any, sequence: int) -> None:
        try:
            goal_handle = future.result()
        except Exception as exc:
            with self._lock:
                if sequence == self._goal_sequence:
                    self._current_goal_handle = None
                    self._current_goal_sequence = None
                    self._goal = {"state": "failed", "sequence": sequence, "error": str(exc), "updated_at_unix_s": now_s()}
            return
        if not goal_handle.accepted:
            with self._lock:
                if sequence == self._goal_sequence:
                    self._current_goal_handle = None
                    self._current_goal_sequence = None
                    self._goal = {"state": "rejected", "sequence": sequence, "updated_at_unix_s": now_s()}
            return
        with self._lock:
            if sequence != self._goal_sequence:
                should_cancel_stale_goal = True
            else:
                should_cancel_stale_goal = False
                self._current_goal_handle = goal_handle
                self._current_goal_sequence = sequence
                self._goal["state"] = "accepted"
                self._goal["updated_at_unix_s"] = now_s()
        if should_cancel_stale_goal:
            try:
                goal_handle.cancel_goal_async()
            except Exception as exc:
                self.get_logger().warning(f"Failed to cancel stale goal response: {exc}")
            return
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(lambda done, goal_sequence=sequence: self._goal_result_callback(done, goal_sequence))

    def _goal_result_callback(self, future: Any, sequence: int) -> None:
        try:
            result = future.result()
            state = status_name(int(result.status))
        except Exception as exc:
            state = "failed"
            error = str(exc)
        else:
            error = None
        failure_detail = None
        if state in {"aborted", "failed", "unknown"}:
            failure_detail = self._diagnose_navigation_failure(state, error)
        target = None
        with self._lock:
            if sequence == self._current_goal_sequence:
                self._current_goal_handle = None
                self._current_goal_sequence = None
            if sequence != self._goal_sequence:
                return
            self._goal["state"] = state
            self._goal["error"] = error
            if failure_detail:
                self._goal["error_detail"] = failure_detail.get("message")
                self._goal["failure_category"] = failure_detail.get("category")
                self._goal["failure_next_step"] = failure_detail.get("next_step")
                self._goal["diagnostics"] = deepcopy(failure_detail)
            else:
                self._goal.pop("error_detail", None)
                self._goal.pop("failure_category", None)
                self._goal.pop("failure_next_step", None)
                self._goal.pop("diagnostics", None)
            self._goal["updated_at_unix_s"] = now_s()
            target = deepcopy(self._goal.get("target"))

    def _publish_zero_velocity_burst(self) -> None:
        zero = Twist()
        for _ in range(5):
            self.cmd_vel_pub.publish(zero)
            self.cmd_vel_nav_pub.publish(zero)
            time.sleep(0.04)

    def _cancel_callback(self, future: Any, sequence: int | None = None) -> None:
        try:
            cancel_response = future.result()
            cancel_count = len(cancel_response.goals_canceling)
        except Exception as exc:
            with self._lock:
                self._goal = {"state": "stop_failed", "error": str(exc), "updated_at_unix_s": now_s()}
            return
        with self._lock:
            if sequence is not None and sequence == self._current_goal_sequence and not cancel_count:
                self._current_goal_handle = None
                self._current_goal_sequence = None
            self._goal = {"state": "canceling" if cancel_count else "stopped", "updated_at_unix_s": now_s()}

    def _load_fallback_map(self) -> dict[str, Any] | None:
        with self._lock:
            if self._fallback_map:
                return deepcopy(self._fallback_map)
        try:
            payload = load_map_yaml(self.map_yaml)
        except Exception as exc:
            self.get_logger().warning(f"Failed to load map fallback {self.map_yaml}: {exc}")
            return None
        with self._lock:
            self._fallback_map = payload
            return deepcopy(payload)

    @staticmethod
    def _nav_launch_pid() -> int | None:
        try:
            result = subprocess.run(
                ["pgrep", "-af", "go2_navigation2 go2_nav2.launch.py"],
                check=False,
                capture_output=True,
                text=True,
                timeout=1.0,
            )
        except Exception:
            return None
        for line in result.stdout.splitlines():
            parts = line.strip().split(maxsplit=1)
            if len(parts) != 2:
                continue
            pid_text, command = parts
            is_ros2_launch = (
                "/bin/ros2 launch go2_navigation2 go2_nav2.launch.py" in command
                or command.startswith("ros2 launch go2_navigation2 go2_nav2.launch.py")
            )
            if not is_ros2_launch:
                continue
            try:
                return int(pid_text)
            except ValueError:
                continue
        return None


class CameraSampler:
    def __init__(self, iface: str, interval_s: float = 0.18, video_url: str = "") -> None:
        self.iface = iface
        self.interval_s = interval_s
        self.video_url = video_url or os.environ.get("GO2_DASHBOARD_VIDEO_URL", "")
        self._condition = threading.Condition()
        self._frame: bytes | None = None
        self._frame_id = 0
        self._updated_at = 0.0
        self._error: str | None = None
        self._source = "http" if self.video_url else "sdk"
        self._thread = threading.Thread(target=self._run, daemon=True)
        self._thread.start()

    def _run(self) -> None:
        if self.video_url:
            self._run_http()
        else:
            self._run_sdk()

    def _run_http(self) -> None:
        import urllib.request
        while True:
            try:
                req = urllib.request.Request(self.video_url)
                with urllib.request.urlopen(req, timeout=10.0) as resp:
                    content_type = resp.headers.get("Content-Type", "")
                    if "multipart" in content_type:
                        self._read_mjpeg_stream(resp)
                    else:
                        data = resp.read()
                        if data.startswith(b"\xff\xd8"):
                            with self._condition:
                                self._frame = data
                                self._frame_id += 1
                                self._updated_at = now_s()
                                self._error = None
                                self._condition.notify_all()
                        time.sleep(self.interval_s)
            except Exception as exc:
                with self._condition:
                    self._error = f"HTTP video fetch failed: {exc}"
                    self._condition.notify_all()
                time.sleep(2.0)

    def _read_mjpeg_stream(self, resp) -> None:
        buf = b""
        while True:
            chunk = resp.read(8192)
            if not chunk:
                break
            buf += chunk
            while True:
                start = buf.find(b"\xff\xd8")
                if start == -1:
                    buf = buf[-1:]
                    break
                end = buf.find(b"\xff\xd9", start + 2)
                if end == -1:
                    break
                frame = buf[start:end + 2]
                buf = buf[end + 2:]
                with self._condition:
                    self._frame = frame
                    self._frame_id += 1
                    self._updated_at = now_s()
                    self._error = None
                    self._condition.notify_all()

    def _run_sdk(self) -> None:
        try:
            from unitree_sdk2py.core.channel import ChannelFactoryInitialize
            from unitree_sdk2py.go2.video.video_client import VideoClient

            ChannelFactoryInitialize(0, self.iface)
            client = VideoClient()
            client.SetTimeout(3.0)
            client.Init()
        except Exception as exc:
            with self._condition:
                self._error = f"Camera SDK init failed: {exc}"
                self._condition.notify_all()
            return

        while True:
            try:
                code, data = client.GetImageSample()
                if code == 0 and data:
                    frame = bytes(data)
                    if frame.startswith(b"\xff\xd8"):
                        with self._condition:
                            self._frame = frame
                            self._frame_id += 1
                            self._updated_at = now_s()
                            self._error = None
                            self._condition.notify_all()
                    else:
                        with self._condition:
                            self._error = "Camera SDK returned non-JPEG data"
                else:
                    with self._condition:
                        self._error = f"Camera SDK returned code {code}"
                time.sleep(self.interval_s)
            except Exception as exc:
                with self._condition:
                    self._error = f"Camera sample failed: {exc}"
                    self._condition.notify_all()
                time.sleep(1.0)

    def wait_for_frame(self, previous_id: int, timeout_s: float) -> tuple[int, bytes] | None:
        deadline = now_s() + timeout_s
        with self._condition:
            while self._frame_id <= previous_id:
                remaining = deadline - now_s()
                if remaining <= 0:
                    return None
                self._condition.wait(remaining)
            if self._frame is None:
                return None
            return self._frame_id, self._frame

    def wait_for_fresh_frame(self, timeout_s: float, max_age_s: float) -> tuple[int, bytes, float] | None:
        deadline = now_s() + timeout_s
        with self._condition:
            while True:
                if self._frame is not None and self._updated_at and now_s() - self._updated_at <= max_age_s:
                    return self._frame_id, bytes(self._frame), self._updated_at
                remaining = deadline - now_s()
                if remaining <= 0:
                    return None
                self._condition.wait(remaining)

    def status(self) -> dict[str, Any]:
        with self._condition:
            age_s = None if not self._updated_at else max(0.0, now_s() - self._updated_at)
            source = self.video_url if self._source == "http" else "unitree_sdk2py.VideoClient.GetImageSample"
            return {
                "video_url": "/api/video",
                "source": source,
                "iface": self.iface,
                "available": self._frame is not None and (age_s is None or age_s < 3.0),
                "frame_id": self._frame_id,
                "frame_age_s": age_s,
                "error": self._error,
            }


def read_map_yaml_values(map_yaml: Path) -> dict[str, str]:
    values: dict[str, str] = {}
    for raw_line in map_yaml.read_text(encoding="utf-8").splitlines():
        line = raw_line.split("#", 1)[0].strip()
        if not line or ":" not in line:
            continue
        key, value = line.split(":", 1)
        values[key.strip()] = value.strip()
    return values


def map_image_path(map_yaml: Path, values: dict[str, str] | None = None) -> Path:
    values = values or read_map_yaml_values(map_yaml)
    image_value = values.get("image")
    if not image_value:
        raise ValueError("map yaml does not contain image")
    image_path = Path(image_value)
    if not image_path.is_absolute():
        image_path = map_yaml.parent / image_path
    return image_path


def map_file_revision(map_yaml: Path) -> int:
    values = read_map_yaml_values(map_yaml)
    image_path = map_image_path(map_yaml, values)
    return int(max(map_yaml.stat().st_mtime_ns, image_path.stat().st_mtime_ns))


def load_map_yaml(map_yaml: Path) -> dict[str, Any]:
    values = read_map_yaml_values(map_yaml)
    image_path = map_image_path(map_yaml, values)
    resolution = float(values.get("resolution", "0.05"))
    origin_raw = values.get("origin", "[0, 0, 0]").strip()
    origin_parts = [float(item.strip()) for item in origin_raw.strip("[]").split(",")]
    while len(origin_parts) < 3:
        origin_parts.append(0.0)
    width, height, pixels = read_pgm(image_path)
    cells = [0] * (width * height)
    for src_y in range(height):
        map_y = height - 1 - src_y
        for x in range(width):
            value = pixels[src_y * width + x]
            if value >= 250:
                occ = 0
            elif 190 <= value <= 220:
                occ = -1
            elif value <= 100:
                occ = 100
            else:
                occ = -1
            cells[map_y * width + x] = occ
    return {
        "name": "maps/current",
        "revision": map_file_revision(map_yaml),
        "frame_id": "map",
        "resolution": resolution,
        "width": width,
        "height": height,
        "origin": {"x": origin_parts[0], "y": origin_parts[1], "yaw_deg": math.degrees(origin_parts[2])},
        "cells": cells,
        "source": str(map_yaml),
        "received_at_unix_s": now_s(),
    }


def read_pgm(path: Path) -> tuple[int, int, bytes]:
    data = path.read_bytes()
    index = 0

    def token() -> bytes:
        nonlocal index
        while index < len(data):
            byte = data[index]
            if byte == ord("#"):
                while index < len(data) and data[index] not in (10, 13):
                    index += 1
            elif chr(byte).isspace():
                index += 1
            else:
                break
        start = index
        while index < len(data) and not chr(data[index]).isspace():
            index += 1
        return data[start:index]

    magic = token()
    if magic != b"P5":
        raise ValueError(f"unsupported PGM format: {magic!r}")
    width = int(token())
    height = int(token())
    max_value = int(token())
    if max_value > 255:
        raise ValueError("16-bit PGM is not supported")
    while index < len(data) and chr(data[index]).isspace():
        index += 1
    pixels = data[index : index + width * height]
    if len(pixels) != width * height:
        raise ValueError("PGM pixel data is truncated")
    return width, height, pixels


class DashboardHandler(SimpleHTTPRequestHandler):
    server_version = "OpenSourceGo2Dashboard/1.0"

    def do_GET(self) -> None:
        parsed = urlparse(self.path)
        if parsed.path == "/api/pose_stream":
            self._stream_pose()
            return
        if parsed.path == "/api/script/patrol_executor/status":
            import subprocess
            pids = subprocess.run(["pgrep", "-f", "patrol_executor_node"], capture_output=True, text=True).stdout.split()
            self._send_json({"running": bool(pids)})
            return
        if parsed.path == "/api/health":
            self._send_json({"ok": True, "name": "openSourceGo2Dashboard"})
            return
        if parsed.path == "/api/video":
            self._stream_video()
            return
        if parsed.path == "/api/state":
            payload = self.server.nav_node.state()  # type: ignore[attr-defined]
            payload["camera"] = self.server.camera_sampler.status()  # type: ignore[attr-defined]
            self._send_json(payload)
            return
        if parsed.path == "/api/map":
            try:
                self._send_json(self.server.nav_node.map_payload())  # type: ignore[attr-defined]
            except Exception as exc:
                self._send_error(HTTPStatus.SERVICE_UNAVAILABLE, str(exc))
            return
        if parsed.path == "/api/patrol-route":
            try:
                self._send_json(self.server.nav_node.load_patrol_route())
            except Exception as exc:
                self._send_error(HTTPStatus.SERVICE_UNAVAILABLE, str(exc))
            return
        if parsed.path == "/api/inspection-routes/default":

            try:
                self._send_json(self.server.nav_node.load_inspection_route())  # type: ignore[attr-defined]
            except Exception as exc:
                self._send_error(HTTPStatus.SERVICE_UNAVAILABLE, str(exc))
            return
        if parsed.path == "/api/v1/dashboardStates/current":
            payload = self.server.nav_node.state()  # type: ignore[attr-defined]
            payload["camera"] = self.server.camera_sampler.status()  # type: ignore[attr-defined]
            payload["legacy_warning"] = "Refresh the page; this browser is still using an old dashboard script."
            self._send_json(payload)
            return
        if parsed.path == "/api/v1/events":
            self._send_json({"events": [], "legacy_warning": "Refresh the page to load the current dashboard."})
            return
        if parsed.path.startswith("/api/v1/"):
            self._send_error(HTTPStatus.GONE, "Legacy GO2 dashboard API has been removed")
            return
        if parsed.path == "/":
            self.path = "/index.html"
        return super().do_GET()


    def end_headers(self) -> None:
        if not self.path.startswith("/api/video"):
            self.send_header("Cache-Control", "no-store")
        super().end_headers()
    def do_PATCH(self) -> None:
        parsed = urlparse(self.path)
        try:
            body = read_json_body(self)
            if parsed.path == "/api/patrol-route":
                result = self.server.nav_node.save_patrol_route(body)
                self._send_json(result)
                return
            if parsed.path == "/api/inspection-routes/default":
                query = parse_qs(parsed.query)
                update_mask = ",".join(query.get("update_mask", []))
                payload = self.server.nav_node.patch_inspection_route(body, update_mask)  # type: ignore[attr-defined]
            else:
                self._send_error(HTTPStatus.NOT_FOUND, "Resource was not found")
                return
        except ValueError as exc:
            self._send_error(HTTPStatus.BAD_REQUEST, str(exc))
            return
        except RuntimeError as exc:
            self._send_error(HTTPStatus.SERVICE_UNAVAILABLE, str(exc))
            return
        except Exception as exc:
            self._send_error(HTTPStatus.INTERNAL_SERVER_ERROR, str(exc))
            return
        self._send_json(payload)

    def do_POST(self) -> None:
        parsed = urlparse(self.path)
        try:
            body = read_json_body(self)
            if parsed.path == "/api/initial_pose":
                payload = self.server.nav_node.publish_initial_pose(body)  # type: ignore[attr-defined]
            elif parsed.path == "/api/goal":
                payload = self.server.nav_node.navigate_to_pose(body)  # type: ignore[attr-defined]
            elif parsed.path == "/api/navigation/stop":
                payload = self.server.nav_node.stop_navigation()
                self._send_json(payload)
            elif parsed.path == "/api/stop":
                payload = self.server.nav_node.stop_motion()  # type: ignore[attr-defined]
            elif parsed.path == "/api/navigation/start":
                payload = self.server.nav_node.start_navigation()  # type: ignore[attr-defined]
            elif parsed.path == "/api/localization-checks/current:confirm":
                payload = self.server.nav_node.confirm_localization()  # type: ignore[attr-defined]
            elif parsed.path == "/api/inspection-routes/default:start":
                payload = self.server.nav_node.start_inspection_route()  # type: ignore[attr-defined]
            elif parsed.path == "/api/script/start_nav":
                import subprocess, threading
                repo = str(self.server.nav_node.repo_root)  # type: ignore[attr-defined]
                def _run_nav():
                    subprocess.Popen(["bash", f"{repo}/scripts/go2_start_nav.sh"],
                                     stdout=open(f"{repo}/log/start_nav.log", "a"),
                                     stderr=subprocess.STDOUT, stdin=subprocess.DEVNULL)
                threading.Thread(target=_run_nav, daemon=True).start()
                payload = {"ok": True, "msg": "go2_start_nav.sh started"}
            elif parsed.path == "/api/script/run_patrol":
                import subprocess
                nav_node = self.server.nav_node  # type: ignore[attr-defined]
                robot_ros = nav_node._robot_ros_link_state()
                if not robot_ros["sources_fresh"]:
                    stale = ", ".join(robot_ros["stale_sources"])
                    raise RuntimeError(f"机器人 ROS 通信未就绪，缺少数据源: {stale}")
                if not nav_node.nav_client.wait_for_server(timeout_sec=1.0):
                    raise RuntimeError("Nav2 action server 未就绪，请先启动导航")
                repo = str(nav_node.repo_root)
                log_dir = Path(repo) / "logs"
                log_dir.mkdir(parents=True, exist_ok=True)
                log_file = log_dir / "run_patrol.log"
                env = os.environ.copy()
                if "DISPLAY" not in env:
                    env["DISPLAY"] = ":0"
                if "WAYLAND_DISPLAY" not in env:
                    env["WAYLAND_DISPLAY"] = "wayland-0"
                env["XDG_RUNTIME_DIR"] = f"/run/user/{os.getuid()}"
                subprocess.Popen(
                    ["bash", f"{repo}/scripts/go2_run_patrol.sh"],
                    cwd=repo,
                    env=env,
                    stdout=log_file.open("ab"),
                    stderr=subprocess.STDOUT,
                    stdin=subprocess.DEVNULL,
                    start_new_session=True
                )
                payload = {"ok": True, "msg": "巡检已启动"}
            elif parsed.path == "/api/script/patrol_executor/toggle":
                import subprocess, signal
                repo = str(self.server.nav_node.repo_root)  # type: ignore[attr-defined]
                pids = subprocess.run(["pgrep", "-f", "patrol_executor_node"], capture_output=True, text=True).stdout.split()
                if pids:
                    for pid in pids:
                        try: __import__("os").kill(int(pid), signal.SIGTERM)
                        except Exception: pass
                    payload = {"ok": True, "running": False, "msg": "巡检服务已停止"}
                else:
                    log_dir = Path(repo) / "logs"
                    log_dir.mkdir(parents=True, exist_ok=True)
                    log_file = log_dir / "patrol_executor.log"
                    env = os.environ.copy()
                    if "DISPLAY" not in env:
                        env["DISPLAY"] = ":0"
                    if "WAYLAND_DISPLAY" not in env:
                        env["WAYLAND_DISPLAY"] = "wayland-0"
                    env["XDG_RUNTIME_DIR"] = f"/run/user/{os.getuid()}"
                    subprocess.Popen(
                        ["bash", f"{repo}/scripts/go2_start_patrol_executor.sh"],
                        cwd=repo,
                        env=env,
                        stdout=log_file.open("ab"),
                        stderr=subprocess.STDOUT,
                        stdin=subprocess.DEVNULL,
                        start_new_session=True
                    )
                    payload = {"ok": True, "running": True, "msg": "巡检服务已启动"}
            elif parsed.path == "/api/script/patrol_executor/status":
                import subprocess
                repo = str(self.server.nav_node.repo_root)  # type: ignore[attr-defined]
                pids = subprocess.run(["pgrep", "-f", "patrol_executor_node"], capture_output=True, text=True).stdout.split()
                payload = {"running": bool(pids)}
            elif parsed.path == "/api/inspection-runs/current:cancel":
                payload = self.server.nav_node.cancel_inspection_run()  # type: ignore[attr-defined]
            elif parsed.path.startswith("/api/v1/"):
                self._send_error(HTTPStatus.GONE, "Legacy GO2 dashboard API has been removed")
                return
            else:
                self._send_error(HTTPStatus.NOT_FOUND, "Resource was not found")
                return
        except ValueError as exc:
            self._send_error(HTTPStatus.BAD_REQUEST, str(exc))
            return
        except RuntimeError as exc:
            self._send_error(HTTPStatus.SERVICE_UNAVAILABLE, str(exc))
            return
        except Exception as exc:
            self._send_error(HTTPStatus.INTERNAL_SERVER_ERROR, str(exc))
            return
        self._send_json(payload)

    def _send_json(self, payload: dict[str, Any], status: HTTPStatus = HTTPStatus.OK) -> None:
        data = json.dumps(payload, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
        self.send_response(status)
        self.send_header("Content-Type", "application/json; charset=utf-8")
        self.send_header("Cache-Control", "no-store")
        self.send_header("Content-Length", str(len(data)))
        self.end_headers()
        self.wfile.write(data)

    def _send_error(self, status: HTTPStatus, message: str) -> None:
        self._send_json({"error": {"code": int(status), "status": status.name, "message": message}}, status)

    def _stream_pose(self) -> None:
        import math, time
        self.send_response(200)
        self.send_header("Content-Type", "text/event-stream")
        self.send_header("Cache-Control", "no-store")
        self.send_header("Access-Control-Allow-Origin", "*")
        self.end_headers()
        try:
            while True:
                pose = self.server.nav_node._pose  # type: ignore[attr-defined]
                if pose:
                    x = float(pose.get("x", 0))
                    y = float(pose.get("y", 0))
                    yaw_deg = float(pose.get("yaw_deg", 0))
                    yaw_rad = math.radians(yaw_deg)
                    msg = "x=%.3f  y=%.3f  yaw=%.3f rad  (%.1f deg)" % (x, y, yaw_rad, yaw_deg)
                    self.wfile.write(("data: " + msg + chr(10) + chr(10)).encode("utf-8"))
                    self.wfile.flush()
                time.sleep(0.5)
        except (BrokenPipeError, ConnectionResetError):
            return

    def _stream_video(self) -> None:
        sampler = self.server.camera_sampler  # type: ignore[attr-defined]
        first = sampler.wait_for_frame(0, 6.0)
        if first is None:
            self._send_error(HTTPStatus.SERVICE_UNAVAILABLE, "Camera frame is not available")
            return
        boundary = "go2frame"
        self.send_response(HTTPStatus.OK)
        self.send_header("Content-Type", f"multipart/x-mixed-replace; boundary={boundary}")
        self.send_header("Cache-Control", "no-store")
        self.end_headers()
        frame_id, frame = first
        try:
            while True:
                self._write_mjpeg_frame(boundary, frame)
                next_frame = sampler.wait_for_frame(frame_id, 5.0)
                if next_frame is None:
                    continue
                frame_id, frame = next_frame
        except (BrokenPipeError, ConnectionResetError):
            return

    def _write_mjpeg_frame(self, boundary: str, frame: bytes) -> None:
        header = (
            f"--{boundary}\r\n"
            "Content-Type: image/jpeg\r\n"
            f"Content-Length: {len(frame)}\r\n\r\n"
        ).encode("ascii")
        self.wfile.write(header)
        self.wfile.write(frame)
        self.wfile.write(b"\r\n")
        self.wfile.flush()

    def log_message(self, format: str, *args: Any) -> None:
        print(f"{self.address_string()} - {format % args}", flush=True)


def main() -> int:
    parser = argparse.ArgumentParser(description="Open-source Go2 Nav2 dashboard")
    parser.add_argument("--host", default="0.0.0.0")
    parser.add_argument("--port", type=int, default=8090)
    parser.add_argument("--map-yaml", default=str(Path.home() / "go2_maps" / "competition_map.yaml"))
    parser.add_argument("--repo-root", default=str(Path.home() / "go2_robot_system"))
    parser.add_argument("--static-dir", default=str(Path(__file__).resolve().parent / "static"))
    parser.add_argument("--nav-log-dir", default=str(Path.home() / "go2_robot_system" / "log" / "go2_nav"))
    parser.add_argument("--camera-iface", default=os.environ.get("GO2_CAMERA_IFACE", "ens160"))
    parser.add_argument(
        "--pose-config",
        default=str(Path.home() / "go2_robot_system" / "go2_dashboard" / "config" / "pose_pipeline.json"),
    )
    parser.add_argument(
        "--waypoint-action-config",
        default=str(Path.home() / "go2_robot_system" / "go2_dashboard" / "config" / "waypoint_action.json"),
    )
    args = parser.parse_args()

    static_dir = Path(args.static_dir).resolve()
    os.chdir(static_dir)

    rclpy.init()
    nav_node = OpenSourceNavNode(
        Path(args.map_yaml).expanduser(),
        Path(args.repo_root).expanduser(),
        Path(args.nav_log_dir).expanduser(),
        Path(args.pose_config).expanduser(),
        Path(args.waypoint_action_config).expanduser(),
    )
    executor = MultiThreadedExecutor(num_threads=2)
    executor.add_node(nav_node)
    spin_thread = threading.Thread(target=executor.spin, daemon=True)
    spin_thread.start()
    camera_sampler = CameraSampler(args.camera_iface)
    nav_node.camera_sampler = camera_sampler

    class Server(ThreadingHTTPServer):
        daemon_threads = True

    httpd = Server((args.host, args.port), DashboardHandler)
    httpd.nav_node = nav_node  # type: ignore[attr-defined]
    httpd.camera_sampler = camera_sampler  # type: ignore[attr-defined]

    stopping = threading.Event()

    def handle_signal(signum: int, _frame: Any) -> None:
        if stopping.is_set():
            return
        stopping.set()
        print(f"Received signal {signum}, shutting down", flush=True)
        threading.Thread(target=httpd.shutdown, daemon=True).start()

    signal.signal(signal.SIGTERM, handle_signal)
    signal.signal(signal.SIGINT, handle_signal)

    print(f"Open-source Go2 dashboard listening on http://{args.host}:{args.port}", flush=True)
    try:
        httpd.serve_forever()
    finally:
        httpd.server_close()
        executor.shutdown()
        nav_node.destroy_node()
        rclpy.shutdown()
    return 0


if __name__ == "__main__":
    raise SystemExit(main())