#!/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())