humanoid_ops/

├── README.md

├── requirements.txt

├── config/

│   └── config.yaml               # 机器人 / 视觉 / 安全 / 导航 / 巡检点配置

├── humanoid_ops/

│   ├── __main__.py              # 命令行入口

│   ├── config.py                 # 配置模型与加载(含内置默认值)

│   ├── models.py                # 数据结构：RobotState / TargetEstimate / 报告…

│   ├── logger.py                 # 统一日志

│   ├── safety.py                 # 安全监控(SafetyAbort)

│   ├── orchestrator.py            # 任务编排 + 生命周期 + 安全心跳

│   ├── robot/

│   │   ├── base.py                  # RobotBackend 抽象接口

│   │   ├── mock_backend.py         # 仿真后端(运动学 + 成像)

│   │   └── unitree_backend.py        # 宇树真机后端(unitree_sdk2py)

│   ├── perception/

│   │   ├── camera.py               # 相机内参与取帧

│   │   ├── detector.py              # HSV 颜色阈值检测

│   │   ├── localizer.py              # 像素→机器人坐标(偏角/距离)

│   │   └── pipeline.py              # 取帧→检测→定位 + 标注存档

│   └── tasks/

│       ├── base.py              # 任务基类(导航/巡检复用)

│       ├── patrol.py             # 巡检任务

│       └── pick_place.py         # 视觉引导抓取任务

├── scripts/

│   ├── run_demo.sh              # 一键Mock演示[linux/macos]

│   ├── setup_windows.bat         # 一键构建：创建 .venv → 激活 → 升级 pip → 安装依赖。

│   ├── run_demo.bat              # 一键Mock演示[windows]

│   ├── run_real.bat               # 真机运行[windows]

│   └── run_real.sh               # 真机运行[linux/macos]

└── tests/

    └── test_smoke.py             # 端到端冒烟测试

cd humanoid_ops

# 依赖：numpy / opencv-python / PyYAML

pip install -r requirements.txt

# 场景一：巡检

python -m humanoid_ops --backend mock --scenario patrol

# 场景二：视觉引导抓取

python -m humanoid_ops --backend mock --scenario pick_place

# 或一键跑两个场景

bash scripts/run_demo.sh

:: 1) 创建并激活虚拟环境

py -3 -m venv .venv

.venv\Scripts\activate

:: 2) 安装依赖

python -m pip install --upgrade pip

pip install -r requirements.txt

:: 3) 运行(任选其一)

python -m humanoid_ops --backend mock --scenario patrol

python run.py --backend mock --scenario pick_place

scripts\setup_windows.bat   :: 一键建环境 + 装依赖

scripts\run_demo.bat        :: 一键跑两个场景

[1/3] 前往巡检点 WP1 (x=2.00, y=0.00)

巡检点 WP1：开始视觉巡检 (x=1.82, y=0.00, yaw=0.00)

  └ 发现 red_box：偏角 0.1°，估计距离 2.64 m

[2/3] 前往巡检点 WP2 (x=4.00, y=2.00)

  └ 发现 red_box：偏角 1.8°，估计距离 2.45 m

...

任务报告 | 成功=True | 用时=26.0s | 到达巡检点=3 | 发现目标=4

python tests/test_smoke.py      # 直接跑

python -m pytest tests/ -q       # 用 pytest

"""跨平台一键入口(Windows / Linux / macOS 通用)。
用法：
    python run.py --backend mock --scenario patrol
    python run.py --backend mock --scenario pick_place
等价于 `python -m humanoid_ops ...`，但无需依赖包是否在 PATH 中，
在 Windows 上更直观。
"""
from __future__ import annotations
import os
import sys
# 保证无论从哪个工作目录启动都能找到 humanoid_ops 包
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from humanoid_ops.__main__ import main  # noqa: E402
if __name__ == "__main__":
    sys.exit(main())

"""巡检任务：依次到达各巡检点，做视觉巡检并汇总报告。"""
from __future__ import annotations
from typing import Callable
from ..models import InspectionRecord, MissionResult, Waypoint, now
from .base import BaseTask
class PatrolTask(BaseTask):
    @property
    def name(self) -> str:
        return "patrol"
    def __init__(self, robot, vision, cfg, log) -> None:
        super().__init__(robot, vision, cfg, log)
        self.waypoints = [Waypoint(**_normalize_wp(w)) for w in cfg.waypoints]
    def run(self, tick: Callable[[float], None]) -> MissionResult:
        self.log.info("巡检任务开始，共 %d 个巡检点", len(self.waypoints))
        for i, wp in enumerate(self.waypoints, 1):
            self.log.info("[%d/%d] 前往巡检点 %s (x=%.2f, y=%.2f)",
                          i, len(self.waypoints), wp.name, wp.x, wp.y)
            reached, elapsed = self.navigate_to(wp, tick)
            rec = InspectionRecord(waypoint=wp.name, reached=reached,
                                   elapsed_s=round(elapsed, 2), pose=self._pose())
            if reached and wp.inspect:
                rec.targets = self.inspect(wp, tick)
            elif not reached:
                rec.note = "未到达(超时)"
            self._records.append(rec)
            tick(0.0)
        success = all(r.reached for r in self._records)
        self.log.info("巡检任务结束，成功=%s，用时 %.1fs", success, now() - self._started)
        return MissionResult( mission=self.name, backend=self.robot.name,
            started_at=self._started, finished_at=now(),
            success=success, records=self._records, )
def _normalize_wp(raw: dict) -> dict:
    """兼容 YAML 中省略 yaw/inspect 的写法。"""
    return { "name": raw.get("name", "WP"),
        "x": float(raw.get("x", 0.0)),
        "y": float(raw.get("y", 0.0)),
        "yaw": float(raw.get("yaw", 0.0)),
        "inspect": bool(raw.get("inspect", True)), }

"""视觉引导抓取任务：发现目标 → 对准 → 靠近 → 抓取。"""
from __future__ import annotations
import math
from typing import Callable
from ..models import InspectionRecord, MissionResult, TargetEstimate, Waypoint, clamp, now
from .base import BaseTask
from .patrol import _normalize_wp
class PickPlaceTask(BaseTask):
    @property
    def name(self) -> str:
        return "pick_place"
    def __init__(self, robot, vision, cfg, log) -> None:
        super().__init__(robot, vision, cfg, log)
        self.waypoints = [Waypoint(**_normalize_wp(w)) for w in cfg.waypoints]
    def run(self, tick: Callable[[float], None]) -> MissionResult:
        self.log.info("视觉引导抓取任务开始"); wp = self.waypoints[0]
        reached, elapsed = self.navigate_to(wp, tick)
        rec = InspectionRecord(waypoint=wp.name, reached=reached,
                               elapsed_s=round(elapsed, 2), pose=self._pose())
        if not reached:
            rec.note = "未到达作业点"
        else:
            rec.targets = self.inspect(wp, tick)
            if rec.targets:
                target = min(rec.targets, key=lambda t: t.distance_m)
                ok = self.approach_and_grasp(target, tick)
                rec.note = "抓取成功" if ok else "抓取失败"
            else:
                rec.note = "未发现可抓取目标"
        self._records.append(rec); self.robot.stop()
        return MissionResult( mission=self.name, backend=self.robot.name,
            started_at=self._started, finished_at=now(),
            success=(rec.note == "抓取成功"), records=self._records, )
    def approach_and_grasp(self, target: TargetEstimate, tick,
                           grasp_dist: float = 1.0, timeout: float = 15.0) -> bool:
        """基于实时视觉闭环对准并前进到抓取距离，然后执行抓取动作。"""
        self.log.info("目标 %s：对准并靠近至 %.2f m", target.label, grasp_dist)
        self.robot.set_arm_pose("HOME"); t0 = now()
        while now() - t0 < timeout:
            targets, _, _ = self.vision.scan()
            if not targets:
                self.log.warning("目标丢失，停止靠近"); self.robot.stop()
                return False
            t = targets[0]
            if t.distance_m <= grasp_dist:
                self.robot.stop(); self.robot.set_arm_pose("GRAB")
                self.log.info("已到位(估计距离 %.2f m)，执行抓取", t.distance_m)
                return True
            theta = -math.radians(t.bearing_deg)   # 目标在左为正，用于对准
            vyaw = clamp(1.5 * theta, -0.8, 0.8)
            vx = clamp(0.30 * max(0.0, math.cos(theta)), 0.0, 0.30)
            self.robot.move(vx, 0.0, vyaw, duration=self.cfg.nav.ctrl_period_s)
            tick(0.0)
        self.robot.stop(); self.log.warning("靠近目标超时")
        return False

"""Mock 后端：无需任何硬件即可完整跑通整条业务链。
它同时模拟了三件事：
1) 运动学积分(速度指令 -> 位姿)与电量/温度随负载变化；
2) 一个 2D 世界中的目标物；
3) 前置相机成像——把世界目标投影为图像中的彩色色块，
   从而让真实的「检测 → 定位」感知链路被真正跑起来。
"""
from __future__ import annotations
import math, time
from typing import Dict, List
import numpy as np
from .base import RobotBackend
from ..models import RobotState, now, normalize_angle as _norm
class MockRobot(RobotBackend):
    name = "mock"
    def __init__( self, world: Dict | None = None,
        frame_size: tuple[int, int] = (480, 640),
        hfov_deg: float = 70.0, seed: int = 0, ) -> None:
        self.frame_size = frame_size            # (H, W)
        self.hfov_deg = hfov_deg
        self._targets: List[dict] = list((world or {}).get("targets", []))
        self._rng = np.random.default_rng(seed)
        self._state = RobotState(timestamp=now(), mode="DAMP",
                                 battery=96.0, temperature=36.0)
        self._state.joint_positions = [0.0] * 29
        self._arm_pose = "HOME"; self._connected = False
        self._last_t = now()
    # ---------------- 生命周期 ----------------
    def connect(self) -> None:
        self._connected = True; self._last_t = now()
        self._state.timestamp = self._last_t
        self._state.mode = "DAMP"
    def disconnect(self) -> None:
        self._connected = False
    # ---------------- 物理积分 ----------------
    def _integrate(self) -> RobotState:
        t = now(); dt = max(0.0, min(0.5, t - self._last_t))
        self._last_t = t; s = self._state
        if s.mode == "MOVE":
            cy, sy = math.cos(s.yaw), math.sin(s.yaw)
            s.x += (s.vx * cy - s.vy * sy) * dt
            s.y += (s.vx * sy + s.vy * cy) * dt
            s.yaw = _norm(s.yaw + s.vyaw * dt)
        load = abs(s.vx) + abs(s.vy) + abs(s.vyaw)
        s.battery = max(0.0, s.battery - (0.01 + 0.06 * load) * dt)
        target_temp = 36.0 + 16.0 * load
        s.temperature += (target_temp - s.temperature) * min(1.0, 0.6 * dt)
        s.imu_yaw = s.yaw
        # 站姿时给一点微小姿态扰动，模拟真实抖动
        if s.mode != "DAMP":
            s.imu_roll = float(self._rng.normal(0, 0.01))
            s.imu_pitch = float(self._rng.normal(0, 0.01))
        s.timestamp = t
        return s
    def get_state(self) -> RobotState:
        return self._integrate()
    # ---------------- 动作 ----------------
    def stand_up(self) -> None:
        self._integrate(); self._state.mode = "STAND"; time.sleep(0.3)
    def squat_down(self) -> None:
        self._integrate(); self._state.mode = "STAND"
        self._state.mode = "DAMP"; time.sleep(0.3)
    def damp(self) -> None:
        self._integrate(); self._state.mode = "DAMP"
    def move(self, vx: float, vy: float, vyaw: float, duration: float) -> None:
        self._integrate(); self._state.mode = "MOVE"
        self._state.vx, self._state.vy, self._state.vyaw = vx, vy, vyaw
        t_end = now() + max(0.0, duration)
        while now() < t_end:
            self._integrate(); time.sleep(0.02)
        self._integrate()
    def stop(self) -> None:
        self._integrate()
        self._state.vx = self._state.vy = self._state.vyaw = 0.0
        if self._state.mode == "MOVE":
            self._state.mode = "STAND"
    def set_arm_pose(self, pose: str) -> None:
        self._arm_pose = pose
        # 让手臂关节位随姿态变化，便于观察
        base = {"HOME": 0.0, "GRAB": 0.7, "WAVE": -0.6}.get(pose, 0.0)
        for i in range(len(self._state.joint_positions)):
            self._state.joint_positions[i] = base
        time.sleep(0.2)
    # ---------------- 相机成像 ----------------
    def capture_frame(self) -> np.ndarray:
        H, W = self.frame_size; s = self._state
        img = np.full((H, W, 3), 28, np.uint8)
        img[H // 2:, :] = 62                     # 地面
        img[H // 2 - 2:H // 2 + 2, :] = 90       # 地平线
        hfov = math.radians(self.hfov_deg)
        fx = (W / 2.0) / math.tan(hfov / 2.0)
        for tg in self._targets:
            dx = tg["x"] - s.x; dy = tg["y"] - s.y
            dist = math.hypot(dx, dy)
            if dist < 1e-3:
                continue
            brg = _norm(math.atan2(dy, dx) - s.yaw)   # >0 目标在左
            if abs(brg) > hfov / 2.0:
                continue                               # 视场外
            u = W / 2.0 - math.tan(brg) * fx
            size = fx * float(tg.get("size", 0.30)) / dist   # 表观像素边长
            if size < 4:
                continue
            half = int(max(3, size / 2))
            u0 = int(round(u + self._rng.normal(0, 1.2)))
            v0 = int(H * 0.62)
            x1, x2 = max(0, u0 - half), min(W, u0 + half)
            y1, y2 = max(0, v0 - half), min(H, v0 + half)
            if x2 > x1 and y2 > y1:
                img[y1:y2, x1:x2] = (40, 40, 220)   # BGR 红
        noise = self._rng.normal(0, 4, img.shape).astype(np.int16)
        return np.clip(img.astype(np.int16) + noise, 0, 255).astype(np.uint8)
    # ---------------- 仅测试用 ----------------
    def ground_truth_targets(self) -> List[dict]:
        return [dict(t) for t in self._targets]

"""宇树(Unitree)G1/H1 真机后端。
基于官方 Python SDK ``unitree_sdk2py`` 的高层运动接口实现，
与 Mock 后端实现同一套 ``RobotBackend`` 接口，因此任务层无需改动。
依赖安装(真机环境)：
    pip install numpy opencv-python
    # 先装 CycloneDDS 0.10.x，再装 SDK(见仓库 README)
    git clone https://github.com/unitreerobotics/unitree_sdk2_python.git
    export CYCLONEDDS_HOME=~/cyclonedds/install
    pip install -e ./unitree_sdk2_python
运行前请确保主机与机器人处于同一网段，并用 ``ip link`` 查到对应网卡名，
以该网卡名作为 ``iface``(例如 enp3s0 / eth0)。
"""
from __future__ import annotations
import time
import numpy as np
from .base import RobotBackend
from ..models import RobotState, now
try:
    import cv2  # 可选，仅用于 USB 相机取帧
    _HAS_CV2 = True
except Exception:  # pragma: no cover
    _HAS_CV2 = False
class UnitreeRobot(RobotBackend):
    name = "unitree"
    def __init__(self, iface: str = "enp3s0", camera_index: int | None = None) -> None:
        self.iface = iface; self.camera_index = camera_index
        self._client = None           # LocoClient
        self._sub = None              # 状态订阅
        self._cam = None; self._st = RobotState(mode="DAMP")
        self._connected = False
    # ---------------- 生命周期 ----------------
    def connect(self) -> None:
        try:
            from unitree_sdk2py.core.channel import (
                ChannelFactoryInitialize, ChannelSubscriber, )
            from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_
            from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
        except Exception as exc:  # pragma: no cover - 真机环境依赖
            raise RuntimeError( "未找到 unitree_sdk2py，请先在真机环境安装(见本文件顶部说明)。"
            ) from exc
        # 1) 初始化 DDS 通道(domain_id=0，绑定网卡)
        ChannelFactoryInitialize(0, self.iface)
        # 2) 订阅高层运动状态(位姿/速度/IMU/电量)
        self._sub = ChannelSubscriber("rt/sportmodestate", SportModeState_)
        self._sub.Init(self._on_state, 10)
        # 3) 创建高层运动客户端
        self._client = LocoClient(); self._client.SetTimeout(10.0)
        self._client.Init(); self._connected = True
    def disconnect(self) -> None:
        try:
            if self._client is not None:
                self._client.StopMove()
        except Exception:
            pass
        if self._cam is not None:
            self._cam.release(); self._cam = None
        self._connected = False
    # ---------------- 状态回调 ----------------
    def _on_state(self, msg) -> None:  # pragma: no cover - 需真机
        """把 SportModeState_ 映射为统一的 RobotState。"""
        try:
            pos = getattr(msg, "position", [0.0, 0.0, 0.0])
            vel = getattr(msg, "velocity", [0.0, 0.0, 0.0])
            self._st.x = float(pos[0]); self._st.y = float(pos[1])
            self._st.yaw = float(pos[2]) if len(pos) > 2 else 0.0
            self._st.vx = float(vel[0]); self._st.vy = float(vel[1])
            self._st.vyaw = float(vel[2]) if len(vel) > 2 else 0.0
            imu = getattr(msg, "imu_state", None)
            if imu is not None:
                rpy = getattr(imu, "rpy", None) or getattr(imu, "RPY", None)
                if rpy is not None:
                    self._st.imu_roll = float(rpy[0]); self._st.imu_pitch = float(rpy[1])
                    self._st.imu_yaw = float(rpy[2])
            bat = getattr(msg, "power_v", None)
            if bat is not None:
                self._st.battery = float(bat)
            self._st.timestamp = now()
        except Exception:
            pass
    def get_state(self) -> RobotState:
        return self._st
    # ---------------- 动作 ----------------
    def stand_up(self) -> None:
        # 官方推荐：先阻尼 -> 再起立
        self._client.Damp(); time.sleep(0.5)
        self._client.Squat2StandUp()      # 或 Lie2StandUp()(仰面、硬平地面)
        time.sleep(2.0); self._client.BalanceStand()
    def squat_down(self) -> None:
        # 官方高层接口的下蹲/坐低动作(方法名随固件版本略有差异，按需替换)
        self._client.LowStand(); time.sleep(1.5)
    def damp(self) -> None:
        self._client.Damp()
    def move(self, vx: float, vy: float, vyaw: float, duration: float) -> None:
        """持续下发速度指令。
        高层接口单条指令约 1s 过期(continous_move=False)，因此需要在
        duration 内以约 20~50Hz 持续下发。
        """
        t_end = now() + max(0.0, duration)
        while now() < t_end:
            self._client.Move(vx, vy, vyaw, continous_move=False)
            time.sleep(0.05)
    def stop(self) -> None:
        self._client.StopMove()
    def set_arm_pose(self, pose: str) -> None:
        # 高层接口提供若干预设动作；可按需替换为自定义关节轨迹。
        if pose == "WAVE":
            self._client.WaveHand()
        elif pose == "GRAB":
            self._client.ShakeHand()      # 示例：抓取位可复用/自定义
        # HOME 等其它姿态交由低层关节控制实现，此处略。
    # ---------------- 相机 ----------------
    def capture_frame(self) -> np.ndarray:
        """取一帧头像。
        真机首选 SDK 前置相机接口(unitree_sdk2py.video)，此处提供一条
        更通用的 USB 相机通路作为可用实现；若未配置相机则给出明确提示。
        """
        if self.camera_index is None:
            raise NotImplementedError(
                "未配置相机。真机请使用 SDK 前置相机接口，或在构造时传入 camera_index "
                "以使用 USB 相机。" )
        if not _HAS_CV2:
            raise RuntimeError("取帧需要 opencv-python，请先 pip install opencv-python")
        if self._cam is None:
            self._cam = cv2.VideoCapture(self.camera_index)
        ok, frame = self._cam.read()
        if not ok:
            raise RuntimeError("相机取帧失败")
        return frame

"""机器人后端抽象接口。
任务层与感知层只依赖本抽象类，因此更换机型 / 切换仿真与真机时，
业务代码无需改动(依赖倒置)。
"""
from __future__ import annotations
from abc import ABC, abstractmethod
import numpy as np
from ..models import RobotState
class RobotBackend(ABC):
    """人形机器人后端统一接口。
    坐标约定：世界坐标系 x 轴为机器人初始正前方、y 轴向左、yaw 为航向角(rad)。
    """
    name: str = "abstract"
    # ------------------------------------------------ 生命周期
    @abstractmethod
    def connect(self) -> None:
        """建立连接(真机：初始化 DDS/网卡；仿真：置位)。"""
    @abstractmethod
    def disconnect(self) -> None:
        """断开连接并释放资源。"""
    # ------------------------------------------------ 状态
    @abstractmethod
    def get_state(self) -> RobotState:
        """获取一帧机器人状态。"""
    # ------------------------------------------------ 动作
    @abstractmethod
    def stand_up(self) -> None:
        """从初始姿态起立。"""
    @abstractmethod
    def squat_down(self) -> None:
        """下蹲/坐低(便于收存)。"""
    @abstractmethod
    def damp(self) -> None:
        """进入阻尼保护模式(软停)。"""
    @abstractmethod
    def move(self, vx: float, vy: float, vyaw: float, duration: float) -> None:
        """按速度指令持续移动 duration 秒(阻塞)。
        vx: 前后(+前) m/s, vy: 左右(+左) m/s, vyaw: 偏航角速度(+逆时针) rad/s
        """
    @abstractmethod
    def stop(self) -> None:
        """停止移动。"""
    @abstractmethod
    def set_arm_pose(self, pose: str) -> None:
        """设置手臂/上身姿态，如 'HOME' / 'GRAB' / 'WAVE'。"""
    # ------------------------------------------------ 感知
    @abstractmethod
    def capture_frame(self) -> np.ndarray:
        """采集一帧头部相机图像(BGR，numpy 数组)。"""

# 1) 先编译安装 CycloneDDS 0.10.x
git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x
cd cyclonedds && mkdir build install && cd build
cmake .. -DCMAKE_INSTALL_PREFIX=../install && cmake --build . --target install
# 2) 安装宇树官方Python SDK
cd ~ && git clone https://github.com/unitreerobotics/unitree_sdk2_python.git
cd unitree_sdk2_python
export CYCLONEDDS_HOME=~/cyclonedds/install
pip3 install -e .

python -m humanoid_ops --backend unitree --iface enp3s0 --scenario patrol
# 或
bash scripts/run_real.sh enp3s0

ChannelFactoryInitialize(0, iface)                           # 初始化 DDS 并绑定网卡
sub = ChannelSubscriber("rt/sportmodestate", SportModeState_)  # 订阅状态
sub.Init(callback, 10)
client = LocoClient(); client.SetTimeout(10.0); client.Init()       # 高层运动客户端
client.Damp(); client.Squat2StandUp()                       # 阻尼 → 起立
client.Move(vx, vy, vyaw, continous_move=False)              # 速度控制(需持续下发)
client.StopMove()                                        # 停止