鸽子的回忆录

class MyPlugin(BasePlugin):
    def on_load(self):
        # 注册一个带路由的页面组件
        self.register_frontend_component(
            component_name='MyDashboard',
            component_type='dashboard',
            entry_file='MyDashboard.vue',
            title='我的仪表盘',
            icon='mdi-chart-line',
            dashboard={
                'default_size': 'large',
                'allowed_sizes': ['medium', 'large', 'wide'],
                'order': 10,
            },
        )

        # 注册一个配置页面
        self.register_config_entry(
            component_name='MyConfig',
            entry_file='MyConfig.vue',
            title='我的插件配置',
        )

def normalize_frontend_entry_file(entry_file: str, allowed_extensions=None) -> str:
    allowed_extensions = tuple(allowed_extensions or ('.vue', '.js', '.ts'))
    entry_file = str(entry_file or '').strip().replace('\\', '/')
    if not entry_file:
        raise ValueError("前端入口文件不能为空")
    if os.path.isabs(entry_file):
        raise ValueError("前端入口文件不能使用绝对路径")
    parts = [part for part in entry_file.split('/') if part]
    if not parts or any(part == '..' for part in parts):
        raise ValueError("前端入口文件不能包含路径穿越")
    if not entry_file.endswith(allowed_extensions):
        raise ValueError(f"前端入口文件必须是: {', '.join(allowed_extensions)}")
    return '/'.join(parts)

def normalize_dashboard_definition(definition: Dict[str, Any]) -> Dict[str, Any]:
    allowed_sizes = _normalize_dashboard_sizes(definition.get('allowed_sizes'))
    default_size = _normalize_dashboard_size(definition.get('default_size'))
    min_size = _normalize_dashboard_size(definition.get('min_size'))
    if default_size not in allowed_sizes:
        default_size = allowed_sizes[0]
    if min_size not in allowed_sizes:
        min_size = allowed_sizes[0]
    return {
        'title': str(definition.get('title') or '').strip(),
        'icon': str(definition.get('icon') or '').strip(),
        'default_size': default_size,
        'min_size': min_size,
        'allowed_sizes': allowed_sizes,
        'order': int(definition.get('order') or 0),
        'default_hidden': bool(definition.get('default_hidden', False)),
    }

def register_plugin_dependencies(self, plugin_id: str, dependencies: List[Dict]):
    normalized = []
    for dep in dependencies or []:
        if not isinstance(dep, dict) or not dep.get('id'):
            raise ValueError(f"无效的插件依赖定义: {dep}")
        normalized.append({
            'id': str(dep['id']).strip(),
            'version': str(dep.get('version') or '').strip(),
            'optional': bool(dep.get('optional', False)),
        })
    self._plugin_dependencies[plugin_id] = normalized

def _assert_cross_plugin_access(self, caller_plugin_id, target_plugin_id, permission, callable_name=''):
    if caller_plugin_id != target_plugin_id:
        if not self.plugin_depends_on(caller_plugin_id, target_plugin_id):
            raise PermissionError(
                f"插件 {caller_plugin_id} 与插件 {target_plugin_id} 未声明依赖关系"
            )

# 插件 A 注册 API
self.register_plugin_api('send_sms', handler)

# 插件 B 调用插件 A 的 API
result = self.call_plugin_api('plugin_a_id', 'send_sms', phone, message)

# 插件 A 注册 Hook
self.register_plugin_hook('on_order_created', handler)

# 插件 B 触发插件 A 的 Hook
result = self.trigger_plugin_hook('plugin_a_id', 'on_order_created', order_data)

def unregister_plugin(self, plugin_id: str):
    self._plugins.pop(plugin_id, None)
    self._plugin_contexts.pop(plugin_id, None)
    self._node_actions.pop(plugin_id, None)
    self._api_routes.pop(plugin_id, None)
    self._frontend_components.pop(plugin_id, None)
    self._config_entries.pop(plugin_id, None)
    # 连 Python 模块都从 sys.modules 里删掉
    module_name = self._plugin_modules.pop(plugin_id, None)
    if module_name and module_name in sys.modules:
        del sys.modules[module_name]

class SecurityLevel(Enum):
    UNSIGNED = "unsigned"          # 不需要签名
    CHANNEL_AUTH = "channel_auth"  # 连接认证过了就行
    SIGNED = "signed"              # 必须 HMAC 签名

# 注册表的设计——frozen dataclass，不可变
@dataclass(frozen=True)
class ActionSpec:
    name: str
    direction: Direction
    security: SecurityLevel
    payload_schema: Optional[Type] = None
    description: str = ""

def generate_signature(token: str, action: str, payload: dict) -> dict:
    timestamp = str(time.time())
    nonce = secrets.token_hex(16)
    payload_hash = hashlib.sha256(
        json.dumps(payload, sort_keys=True, separators=(',', ':')).encode('utf-8')
    ).hexdigest()
    message = f"{action}:{timestamp}:{nonce}:{payload_hash}"
    signature = hmac.new(
        token.encode('utf-8'),
        message.encode('utf-8'),
        hashlib.sha256
    ).hexdigest()
    return {
        "timestamp": timestamp,
        "nonce": nonce,
        "signature": signature,
    }

def verify_signature(token, action, payload, sign_data, nonce_scope=None):
    timestamp = sign_data.get("timestamp")
    nonce = sign_data.get("nonce")
    signature = sign_data.get("signature")
    
    # 1. 时间窗校验——5分钟容差
    req_time = float(timestamp)
    if abs(time.time() - req_time) > ALLOWED_TIME_DRIFT:  # 300秒
        return False
    
    # 2. 重算签名并比对
    payload_hash = hashlib.sha256(
        json.dumps(payload, sort_keys=True, separators=(',', ':')).encode('utf-8')
    ).hexdigest()
    message = f"{action}:{timestamp}:{nonce}:{payload_hash}"
    expected = hmac.new(
        token.encode('utf-8'),
        message.encode('utf-8'),
        hashlib.sha256
    ).hexdigest()
    if not hmac.compare_digest(expected, signature):
        return False
    
    # 3. Nonce 唯一性——同一个 scope 内 nonce 只能用一次
    if nonce_scope is not None and not _claim_nonce(nonce_scope, action, nonce):
        return False
    
    return True

def _claim_nonce(nonce_scope, action, nonce):
    scope_hash = hashlib.sha256(str(nonce_scope or "default").encode("utf-8")).hexdigest()[:16]
    action_hash = hashlib.sha256(str(action or "").encode("utf-8")).hexdigest()[:16]
    key = f"{NONCE_CACHE_PREFIX}:{scope_hash}:{action_hash}:{nonce}"
    return bool(cache.add(key, "1", timeout=ALLOWED_TIME_DRIFT + 30))

class AsyncBaseConsumer(AsyncWebsocketConsumer, ABC):
    verify_incoming_signatures = True   # 入站方向验签
    sign_outgoing_actions = True        # 出站方向签名

    async def receive(self, text_data=None, bytes_data=None):
        data = json.loads(text_data)
        action = data.get('action')
        payload = data.get('data', None)

        # 如果这个动作需要签名，但消息里没有 _sign 字段——直接拒绝
        if self.verify_incoming_signatures and needs_signature(action):
            sign_data = data.get('_sign')
            if not sign_data:
                return await self.send(json.dumps({
                    "action": "error:signature_required",
                    "data": {"action": action}
                }))
            node_token = self.__get_node_token()
            if not verify_signature(node_token, action, payload or {}, sign_data,
                                    nonce_scope=self.__signature_nonce_scope()):
                return await self.close(3001)  # 签名验证失败——断开连接

        # 分发到对应的 action handler
        if action in self.actions:
            await self.actions[action](self, payload)

    async def send_action(self, action, payload=None):
        data = {"action": action, "data": deepcopy(payload)}
        if self.sign_outgoing_actions and needs_signature(action):
            node_token = self.__get_node_token()
            if node_token:
                data['_sign'] = generate_signature(node_token, action, payload or {})
        await self.send(json.dumps(data, cls=ComplexEncoder))

SENSITIVE_LOG_KEYS = {
    'password', 'passwd', 'pwd', 'token', 'node_token',
    'client_token', 'server_token', 'access', 'refresh',
    'authorization', 'signature', '_sign', 'secret', 'secret_key',
}

def sanitize_ws_payload(value, depth=0):
    if isinstance(value, dict):
        result = {}
        for key, item in value.items():
            if str(key).lower() in SENSITIVE_LOG_KEYS:
                result[key] = '***'
            else:
                result[key] = sanitize_ws_payload(item, depth + 1)
        return result
    # ...

def _to_change_gray(self, frame):
    small = cv2.resize(frame, (160, 90), interpolation=cv2.INTER_AREA)
    return cv2.cvtColor(small, cv2.COLOR_BGR2GRAY)

@staticmethod
def _compute_ssim(prev_gray, curr_gray):
    a = prev_gray.astype(np.float32)
    b = curr_gray.astype(np.float32)
    c1 = (0.01 * 255) ** 2
    c2 = (0.03 * 255) ** 2

    mu_a = cv2.GaussianBlur(a, (11, 11), 1.5)
    mu_b = cv2.GaussianBlur(b, (11, 11), 1.5)
    mu_a2 = mu_a * mu_a
    mu_b2 = mu_b * mu_b
    mu_ab = mu_a * mu_b

    sigma_a2 = cv2.GaussianBlur(a * a, (11, 11), 1.5) - mu_a2
    sigma_b2 = cv2.GaussianBlur(b * b, (11, 11), 1.5) - mu_b2
    sigma_ab = cv2.GaussianBlur(a * b, (11, 11), 1.5) - mu_ab

    num = (2 * mu_ab + c1) * (2 * sigma_ab + c2)
    den = (mu_a2 + mu_b2 + c1) * (sigma_a2 + sigma_b2 + c2)
    den = np.where(den == 0.0, 1e-6, den)
    ssim_map = num / den
    return float(np.mean(ssim_map))

@staticmethod
def _build_detection_signature(detections, frame_w, frame_h):
    inv_w = 1.0 / float(max(1, frame_w))
    inv_h = 1.0 / float(max(1, frame_h))
    signature = []
    for det in detections:
        x1, y1, x2, y2 = det.box_xyxy
        signature.append((
            int(det.class_id),
            round(float(det.confidence), 2),
            round(float(x1) * inv_w, 3),
            round(float(y1) * inv_h, 3),
            round(float(x2) * inv_w, 3),
            round(float(y2) * inv_h, 3),
        ))
    signature.sort()
    return tuple(signature)

def _should_upload_changed(self, frame, detections):
    curr_gray = self._to_change_gray(frame)
    curr_sig = self._build_detection_signature(detections, frame_w, frame_h)

    if self._last_uploaded_gray_small is None:
        return True, curr_gray, curr_sig  # 第一帧，必须传

    boxes_changed = curr_sig != self._last_uploaded_signature
    if self.upload_change_method == 'boxes':
        return boxes_changed, curr_gray, curr_sig  # 纯框签名模式

    ssim = self._compute_ssim(self._last_uploaded_gray_small, curr_gray)
    changed = boxes_changed or (ssim < 0.985)
    return changed, curr_gray, curr_sig

def _enqueue_upload(self, payload):
    try:
        self._upload_queue.put_nowait(payload)
        return
    except queue.Full:
        pass

    if self._upload_drop_oldest:
        try:
            self._upload_queue.get_nowait()  # 丢弃最旧的
            self._upload_queue.task_done()
        except queue.Empty:
            pass
        try:
            self._upload_queue.put_nowait(payload)
        except queue.Full:
            pass

def _download_file(self, uri, target_path, label):
    temp = target.with_suffix(target.suffix + '.tmp')
    resp = requests.get(uri, timeout=20)
    temp.write_bytes(resp.content)
    temp.replace(target)  # 原子替换
    return True

def _rebuild_engine(self, model_path, conf_threshold, iou_threshold):
    engine = YoloOnnxEngine(model_path=model_path, ...)
    self.engine = engine

# bringup.launch.py
lifecycle_nodes = [
    'controller_server',
    'planner_server',
    'behavior_server',
    # 'bt_navigator',  ← 注释掉，不归 lifecycle_manager 管
    'waypoint_follower',
    'smoother_server',
]

ACTION_SERVERS = {
    'back_up': BackUp,
    'spin': Spin,
    'wait': Wait,
    'drive_on_heading': DriveOnHeading,
    'assisted_teleop': AssistedTeleop,
    'follow_path': FollowPath,
    'compute_path_to_pose': ComputePathToPose,
    'compute_path_through_poses': ComputePathThroughPoses,
    'smooth_path': SmoothPath,
}

class ActivateBtNavigator(Node):
    def __init__(self):
        self._action_clients = {}
        for name, action_type in ACTION_SERVERS.items():
            self._action_clients[name] = ActionClient(self, action_type, name)

        self._phase = 'waiting'
        self._timer = self.create_timer(2.0, self._tick)

    def _check_servers(self):
        ready, not_ready = [], []
        for name, client in self._action_clients.items():
            if client.server_is_ready():
                ready.append(name)
            else:
                not_ready.append(name)

        if not not_ready:
            self._phase = 'configuring'

def _do_configure(self):
    req = ChangeState.Request()
    req.transition.id = Transition.TRANSITION_CONFIGURE
    future = self._change_state_cli.call_async(req)
    future.add_done_callback(self._on_configure_done)
    self._phase = 'configure_pending'

def _do_activate(self):
    req = ChangeState.Request()
    req.transition.id = Transition.TRANSITION_ACTIVATE
    future = self._change_state_cli.call_async(req)
    future.add_done_callback(self._on_activate_done)
    self._phase = 'activate_pending'

# navigation_base.launch.py
Node(
    package='navigation',
    executable='wait_for_action_servers',
    name='activate_bt_navigator',
    arguments=['--ros-args', ...],
),

self.declare_parameter('required_services', ['/reinitialize_global_localization'])

for service_name in self._required_services:
    self._service_clients[service_name] = self.create_client(Empty, service_name)

# 在 _check_servers 里一起检查
if (not not_ready) and (not not_ready_services):
    self._phase = 'configuring'

def _fuse_scan_ranges(self, raw, depth, transform):
    fused = list(raw.ranges)  # 以雷达扫描为底

    # 深度相机每个点：坐标变换 → 投影到雷达角度 → 取最近
    for r in depth.ranges:
        # 1. 深度相机坐标系下的点
        x_d = r * math.cos(angle)
        y_d = r * math.sin(angle)

        # 2. 变换到雷达坐标系
        x_r = c * x_d - s * y_d + tx
        y_r = s * x_d + c * y_d + ty
        range_r = math.hypot(x_r, y_r)
        angle_r = math.atan2(y_r, x_r)

        # 3. 超出前方 ±70° 丢弃
        if abs(angle_r) > self._fuse_front_half_angle_rad:
            continue

        # 4. 投射到雷达对应的角度 bin
        idx = int((angle_r - raw_angle_min) / raw_angle_inc + 0.5)

        # 5. 取最近障碍物
        current = fused[idx]
        if (not math.isfinite(current)) or (range_r < current):
            fused[idx] = float(range_r)

    return fused

try:
    transform = self._tf_buffer.lookup_transform(
        raw.header.frame_id,          # 目标：雷达坐标系
        depth.header.frame_id,        # 源：深度相机坐标系
        rclpy.time.Time(),
        timeout=rclpy.duration.Duration(seconds=0.05),
    )
except TransformException:
    # TF 不可用，退化为纯雷达模式
    self._scan_pub.publish(self._build_output_scan(raw, list(raw.ranges), ...))
    return

def _normalize_scan_payload(self, template, ranges, intensities):
    if not self._enforce_fixed_scan_size:
        return ranges, intensities

    # 确定目标点数
    if self._fixed_scan_size > 0:
        target_size = self._fixed_scan_size
    else:
        # 第一帧的点数作为标准
        if self._expected_scan_size <= 0:
            self._expected_scan_size = max(1, len(ranges))
        target_size = self._expected_scan_size

    normalized_ranges = self._resample_nearest(ranges, target_size, fill=template.range_max)
    normalized_intensities = self._resample_nearest(intensities, target_size, fill=0.0)
    return normalized_ranges, normalized_intensities

@staticmethod
def _resample_nearest(values, target_size, fill):
    src = len(values)
    if src == target_size:
        return list(values)

    scale = float(src - 1) / float(target - 1)
    out = []
    for i in range(target):
        idx = int(round(i * scale))
        out.append(float(values[idx]))
    return out

if depth is None or depth_age > self._depth_timeout_sec:  # 0.4 秒
    self._scan_pub.publish(self._build_output_scan(raw, list(raw.ranges), ...))
    return

def _on_runtime_config(self, msg):
    payload = json.loads(msg.data)
    self._apply_runtime_config_payload(payload)

def _apply_runtime_config_payload(self, payload):
    fusion_cfg = runtime_cfg.get('scan_fusion')
    self._depth_fusion_enabled = bool(fusion_cfg.get('depth_fusion_enabled', ...))
    self._fuse_front_half_angle_deg = max(0.0, float(fusion_cfg.get('fuse_front_half_angle_deg', ...)))
    ...

def _cast_ray(self, grid, start_col, start_row, angle, max_cells, width, height, resolution):
    dx = math.cos(angle)
    dy = math.sin(angle)
    for step in range(1, max_cells + 1):
        col = int(start_col + dx * step)
        row = int(start_row + dy * step)
        if col < 0 or col >= width or row < 0 or row >= height:
            return step * resolution  # 出界，返回到边界的距离
        if grid[row, col] >= self.lethal_threshold:  # 200 = 致命障碍
            return (step - 1) * resolution  # 碰到障碍，返回前一格的距离
    return max_cells * resolution  # 射线全长无障碍

for i in range(self.num_wedges):  # 12
    wedge_center = -math.pi + (i + 0.5) * wedge_angle
    ray_distances = []
    for r in range(self.rays_per_wedge):  # 5
        ray_angle = wedge_center + (r - 2) * (wedge_angle / 5)
        free_dist = self._cast_ray(grid, robot_col, robot_row, ray_angle, ...)
        ray_distances.append(free_dist)

    avg_distance = sum(ray_distances) / len(ray_distances)
    angle_diff = abs(normalize_angle(wedge_center - robot_yaw))
    penalty = 0.3 * (angle_diff / math.pi)
    score = avg_distance - penalty

if all_low:
    return EscapeVector(
        angle=normalize_angle(robot_yaw + math.pi),  # 正后方
        distance=best_distance,
        score=best_score,
        fallback=True,  # 标记为降级模式
    )

def execute_recovery(self, escape, robot_yaw, publish_twist_fn, stop_fn, is_safety_blocked_fn):
    # 第一步：原地旋转到逃逸方向
    yaw_error = normalize_angle(escape.angle - robot_yaw)
    rotate_duration = abs(yaw_error) / 0.6  # 0.6 rad/s 角速度
    if rotate_duration > 0.05:
        publish_twist_fn(0.0, rotate_dir * 0.6, min(rotate_duration, 5.0))

    # 第二步：低速前进到安全区域
    forward_duration = min(escape.distance / 0.10, 5.0)  # 0.10 m/s
    if forward_duration > 0.05:
        publish_twist_fn(0.10, 0.0, min(forward_duration, 5.0))

    stop_fn()
    return RecoveryResult.SUCCESS

def _ensure_certificate_material(self):
    cert_path = Path(self.client_cert_file)
    key_path = Path(self.client_key_file)

    if cert_path.is_file() and key_path.is_file():
        return  # 已有，跳过

    if not self.cert_auto_bootstrap:
        raise RuntimeError("巡检车证书材料缺失")

    # 生成 RSA 2048 私钥
    private_key = rsa.generate_private_key(public_exponent=65537, key_size=2048)
    key_pem = private_key.private_bytes(
        encoding=Encoding.PEM,
        format=PrivateFormat.TraditionalOpenSSL,
        encryption_algorithm=NoEncryption(),
    )
    self._write_bytes(str(key_path), key_pem)

    # 用私钥签发自签证书
    cert_pem = self._generate_bootstrap_certificate(private_key)
    self._write_text(str(cert_path), cert_pem)

def _maybe_renew_certificate(self):
    current_cert = x509.load_pem_x509_certificate(self._client_cert_pem.encode("utf-8"))
    now = datetime.datetime.now(datetime.timezone.utc)
    deadline = now + datetime.timedelta(days=15)  # 到期前 15 天
    if current_cert.not_valid_after_utc > deadline:
        return False  # 还早着呢

    # 续签：用同一个私钥，新有效期
    renewed_pem = self._generate_self_signed_certificate(current_cert)
    self._write_text(self.client_cert_file, renewed_pem)
    self._cert_sync_required = True  # 标记需要同步给后端
    return True

def _auth_headers(self, method, path, payload):
    ts = int(time.time())
    payload_hash = hashlib.sha256(self._payload_bytes(payload)).hexdigest()
    message = f"{method.upper()}\n{path}\n{payload_hash}\n{ts}\n{self.rover_code}".encode("utf-8")
    signature = base64.b64encode(self._sign_message(message)).decode("ascii")
    return {
        "X-Rover-Code": self.rover_code,
        "X-Rover-Timestamp": str(ts),
        "X-Rover-Signature": signature,
    }

def _sign_message(self, message):
    if isinstance(self._private_key, rsa.RSAPrivateKey):
        return self._private_key.sign(message, padding.PKCS1v15(), hashes.SHA256())
    if isinstance(self._private_key, ec.EllipticCurvePrivateKey):
        return self._private_key.sign(message, ec.ECDSA(hashes.SHA256()))
    if isinstance(self._private_key, ed25519.Ed25519PrivateKey):
        return self._private_key.sign(message)
    if isinstance(self._private_key, ed448.Ed448PrivateKey):
        return self._private_key.sign(message)

def _ws_loop(self):
    while not self._stop_event.is_set():
        self._run_single_ws_session()
        if self._stop_event.is_set():
            break
        self._connected.clear()
        time.sleep(2.0)  # 断了等 2 秒再连

def _ws_sender_loop(self, ws_app, session_stop_event):
    while not session_stop_event.is_set():
        payload = self._send_queue.get(timeout=0.3)
        # 如果连接断了或者换了新 session，把消息塞回队列
        if self._ws_app is not ws_app:
            self._send_queue.put_nowait(payload)
            break
        if not self._connected.is_set():
            self._send_queue.put_nowait(payload)  # 塞回去，等重连
            time.sleep(0.5)
            continue
        ws_app.send(json.dumps(payload))

def _build_ws_headers(self):
    ws_path = urlparse(self.ws_url).path or "/"
    headers = self._auth_headers("WS", ws_path, {})
    return [f"{k}: {v}" for k, v in headers.items()]

if self.security_enabled:
    self._http_client_cert = (self.client_cert_file, self.client_key_file)
    if ws_secure:
        self._ws_sslopt["certfile"] = self.client_cert_file
        self._ws_sslopt["keyfile"] = self.client_key_file

def upload_map(self, payload):
    response = self._request("POST", "/api/rover/map/upload", payload)
    if response is not None:
        return response

    last_error = (self._last_error or "").lower()
    if ("http 403" in last_error) and ("签名校验失败" in last_error or "signature" in last_error):
        retry = self._request("POST", "/api/rover/map/upload/", payload)
        if retry is not None:
            self._logger.info("地图上传重试（带尾斜杠）成功")
        return retry

unknown_u8 = unknown.astype(np.uint8)
adj_unknown = cv2.dilate(unknown_u8, np.ones((3, 3), np.uint8), iterations=1).astype(bool)
frontier_mask = free & adj_unknown

def _bfs_distance(self, free, start_x, start_y):
    dist = np.full((h, w), -1, dtype=np.int32)
    queue = deque()
    dist[start_y, start_x] = 0
    queue.append((start_x, start_y))

    while queue:
        cx, cy = queue.popleft()
        cd = dist[cy, cx]
        for dx, dy in ((-1, 0), (1, 0), (0, -1), (0, 1)):
            nx, ny = cx + dx, cy + dy
            if 0 <= nx < w and 0 <= ny < h and free[ny, nx] and dist[ny, nx] < 0:
                dist[ny, nx] = cd + 1
                queue.append((nx, ny))
    return dist

has_forward = any(
    abs(angle_diff) < π/4 for _, angle_diff in candidates
)

for goal, angle_diff in candidates:
    heading_penalty = angle_diff / π
    if not has_forward:
        heading_penalty *= 0.3  # 通道尽头，大幅降低方向惩罚
    score = (
        world_dist
        + 0.4 * heading_penalty * world_dist
        - 0.03 * goal.size
    )

def add_failed_goal(self, x, y):
    self._failed_goals.append((x, y))
    if len(self._failed_goals) > 100:
        self._failed_goals = self._failed_goals[-100:]

def _is_blacklisted(self, point):
    for fg in self._failed_goals:
        if distance(point, fg) < 0.5:  # 0.5 米半径内
            return True
    return False

margin = 1
if goal_mx <= margin or goal_mx >= (w - 1 - margin) or \
   goal_my <= margin or goal_my >= (h - 1 - margin):
    continue

pickup_pose_candidate = tilt >= 50.0 and stationary
pickup_dynamic_candidate = (
    accel_magnitude_g <= 0.70
    or accel_magnitude_g >= 1.30
    or gyro_dps >= 90.0
)
pickup_candidate = pickup_pose_candidate or pickup_dynamic_candidate
pickup_confirm_target = 0.8 if pickup_pose_candidate else 0.35

# 第一步：自由落体
if accel_magnitude_g < 0.3:  # 加速度小于 0.3g = 几乎失重
    freefall_start_t = now
    in_freefall = True

# 失重结束，检查持续时间
freefall_duration = now - freefall_start_t
if freefall_duration >= 0.08:  # 至少 80ms
    waiting_impact = True
    impact_deadline = now + 0.50  # 500ms 内等待冲击

# 第二步：冲击
if waiting_impact:
    if accel_magnitude_g >= 2.8:  # 2.8g 冲击 = 着地了
        fall_triggered = True
    elif now > impact_deadline:
        waiting_impact = False  # 没等到冲击，可能是假警报

if tilt >= 70.0 and stationary:
    tipover_start_t = now
    if now - tipover_start_t >= 1.0:
        tipover_triggered = True

if (
    is_pit
    and lidar_cross_validation_enabled
    and (not lidar_confirms_obstacle())
    and invalid_ratio < 0.90
):
    is_pit = False  # 雷达说没事，深度大概率是误报

for i in range(1, len(medians)):
    if medians[i] >= medians[i - 1]:
        return False  # 不单调递减，不是坡道

gradient = medians[0] - medians[-1]
if gradient < 0.25: return False
if abs(pitch_deg) > 25.0: return False
return True

def _update_block_state(self, blocked_candidate, reason, ...):
    if blocked_candidate:
        self._block_candidate_streak += 1
        self._clear_candidate_streak = 0
        if self._block_candidate_streak >= self.block_confirm_frames:  # 2
            self._set_blocked(True, reason, ...)
    else:
        self._clear_candidate_streak += 1
        self._block_candidate_streak = 0
        if self._clear_candidate_streak >= self.clear_confirm_frames:  # 3
            self._set_blocked(False, '', ...)

from zeroconf import Zeroconf, ServiceInfo
import socket

zeroconf = Zeroconf()
info = ServiceInfo(
    type_="_http._tcp.local.",
    name="iot-backend._http._tcp.local.",
    addresses=[socket.inet_aton("192.168.1.100")],
    port=8800,
)
zeroconf.register_service(info)

def _detect_local_ipv4():
    # 第一级：通过主机名解析
    local = _detect_interface_ipv4()
    if local:
        return local

    # 第二级：UDP 探测 8.8.8.8
    probes = [("8.8.8.8", 53), ("1.1.1.1", 53)]
    for host, port in probes:
        ip = _detect_outbound_ipv4(host, port)
        if ip:
            return ip

    return ""

def _detect_outbound_ipv4(target_host, target_port):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.settimeout(0.5)
    try:
        sock.connect((target_host, target_port))
        candidate = sock.getsockname()[0]
        return _normalize_ipv4(candidate)
    except Exception:
        return ""
    finally:
        sock.close()

def _normalize_ipv4(value):
    ip_obj = ipaddress.ip_address(value)
    if ip_obj.is_unspecified or ip_obj.is_loopback or ip_obj.is_multicast or ip_obj.is_link_local:
        return ""
    return str(ip_obj)

strategies = [
    {"label": "v4_strict",      "ip_version": IPVersion.V4Only, "strict": True},
    {"label": "v4_relaxed",     "ip_version": IPVersion.V4Only, "strict": False},
    {"label": "default_relaxed", "ip_version": None,            "strict": False},
]

for strategy in strategies:
    try:
        self.stop()
        self._zeroconf = self._create_zeroconf(strategy["ip_version"])
        self._service_info = ServiceInfo(...)
        self._register_service(self._zeroconf, self._service_info, bool(strategy.get("strict", True)))
        return True
    except Exception as exc:
        self.stop()

@staticmethod
def _create_zeroconf(ip_version):
    if ip_version is None:
        return Zeroconf()
    try:
        return Zeroconf(ip_version=ip_version)
    except TypeError:
        return Zeroconf()  # 老版本没有 ip_version 参数

@staticmethod
def _register_service(zc, info, strict):
    try:
        zc.register_service(info, strict=strict)
    except TypeError:
        zc.register_service(info)  # 老版本没有 strict 参数

def _normalize_service_label(value):
    text = str(value or "").strip().lower()
    out = []
    for ch in text:
        if ch.isalnum() or ch == "-":
            out.append(ch)
        else:
            out.append("-")  # 非法字符替换成连字符
    normalized = "".join(out).strip("-")  # 去掉首尾连字符
    return normalized[:63]  # DNS 标签长度上限

def _normalize_service_type(value):
    service_type = str(value or "").strip()
    if not service_type:
        return "_http._tcp.local."
    if not service_type.endswith("."):
        service_type += "."
    return service_type

from zeroconf import ZeroconfServiceBrowser, Zeroconf

def on_service_state_change(zeroconf, service_type, name, state_change):
    if state_change == ServiceStateChange.Added:
        info = zeroconf.get_service_info(service_type, name)
        if info:
            addresses = [socket.inet_ntoa(addr) for addr in info.addresses]
            print(f"发现 {name}: {addresses}:{info.port}")

zeroconf = Zeroconf()
browser = ZeroconfServiceBrowser(zeroconf, "_http._tcp.local.", handlers=[on_service_state_change])

def stop(self):
    if self._zeroconf and self._service_info:
        try:
            self._zeroconf.unregister_service(self._service_info)
        except Exception:
            pass
    if self._zeroconf:
        try:
            self._zeroconf.close()
        except Exception:
            pass

@asynccontextmanager
async def lifespan(app):
    mdns_advertiser.start()
    yield
    mdns_advertiser.stop()