主题① 错误码矩阵
1.1 一句话概述
把"出错"这件事从一段字符串 升级成结构化的四字段 ，再集中维护一张"业务码 ↔ HTTP 码 ↔ 是否可重试"的映射表。目的是：LLM / 前端 / 运维看同一个错误，得到同一个结论，做同一个动作。
1.2 四字段结构（与你现有 errors.py 对齐）
| 字段 | 含义 | 谁消费 | 示例 |
|---|---|---|---|
code |
稳定业务码，机器可判 | 状态机、测试、LLM 逻辑分支 | OUT_OF_WORKSPACE |
message |
人读的一句话 | 前端展示、日志 | "位姿越界: radius=0.35m > 0.28m" |
retryable |
重试是否有意义 | Agent 循环（决定重试 or 交状态机） | true / false |
details |
附加上下文（字典） | 调试、审计 | {"target": "红色杯子", "confidence": 0.93} |
1.3 为什么业务码和 HTTP 码要"分离"
- HTTP 码只有几十个，表达力不够：同一个
409下面可能是ARM_BUSY（可重试）也可能是ESTOP_ACTIVE（不可重试）。 - 业务码是稳定的语义标识 ，HTTP 码是传输层速览 。客户端判断逻辑只认
code，HTTP 码只是给人快速定位的。 - 结论：映射表单独维护 （
code/message/retryable/http一行一个），不要散落在各函数里。
1.4 retryable 判定三问（核心原则）
只有同时满足"瞬态 + 重试能成功 + 没超次数"，才是
retryable=true。
-
是瞬态问题吗？ 机器忙、网络抖动、目标暂时没检测到 → 是。急停按下、权限不足、输入越界 → 不是。
-
同样的输入重试一次能成功吗？ 能 → 可重试；不能（输入本身就是错的）→ 不可重试。
-
次数用完了吗？ 按你的 Agent 约定：连续失败 2 次 → 交状态机，重试上限 2。
retryable 判定示例（中文注释）
def should_retry(err_code: str, attempt: int, max_attempts: int = 2) -> bool:
"""只有 瞬态 + 重试可成功 + 未超次数 三个条件同时满足才重试"""
retryable_codes = {
"ARM_BUSY", # 手臂忙：等一拍再试，很可能成功 → 可重试
"TRAJECTORY_TIMEOUT", # 运动超时：重试 1 次有概率成功 → 可重试
"OBJECT_NOT_FOUND", # 重新 detect 一次，目标可能再次出现 → 可重试
}
if err_code not in retryable_codes:
return False # 不可重试：输入问题/权限/急停/内部错误
if attempt >= max_attempts:
return False # 超过次数：交给状态机兜底（你的 max_iter 约定）
return True
1.5 代码：扩展版 ErrorCode + HTTP 映射
在你现有 errors.py 基础上建议扩展的错误码（只加不改，保持兼容）：
# 错误码枚举（扩展版，中文注释）------ 与现有 errors.py 的 ErrorCode/ErrorResponse 风格一致
from enum import StrEnum
from dataclasses import dataclass, field
class ErrorCode(StrEnum):
# ------ 请求/契约类：HTTP 4xx，retryable 几乎都是 False ------
INVALID_JSON = "INVALID_JSON" # JSON 解析失败
MISSING_FIELD = "MISSING_FIELD" # 缺必填字段
INVALID_TASK = "INVALID_TASK" # 任务语义非法（非冻结目标名等）
OUT_OF_WORKSPACE = "OUT_OF_WORKSPACE" # 位姿越界：radius>0.28m 或 z 越界
UNKNOWN_TARGET = "UNKNOWN_TARGET" # 目标名不在白名单（负样本测试专用）
FORBIDDEN = "FORBIDDEN" # 权限不足（operator 解除急停）
FORBIDDEN_LOW_LEVEL_FIELD = "FORBIDDEN_LOW_LEVEL_FIELD" # 请求里带了底层字段
# ------ 机器状态类：HTTP 409，retryable 视情况 ------
ARM_BUSY = "ARM_BUSY" # 手臂正在执行其他任务 → 瞬态，可重试
ESTOP_ACTIVE = "ESTOP_ACTIVE" # 急停激活 → 必须人工解除，不可重试
DISABLED = "DISABLED" # 未使能
GRIPPER_FAULT = "GRIPPER_FAULT" # 夹爪故障
# ------ 执行类 ------
TRAJECTORY_TIMEOUT = "TRAJECTORY_TIMEOUT" # 运动/规划超时 → 可重试 1 次
COLLISION_RISK = "COLLISION_RISK" # 碰撞风险
# ------ 系统类 ------
INTERNAL_ERROR = "INTERNAL_ERROR" # 未知内部错误
LLM_UNAVAILABLE = "LLM_UNAVAILABLE" # 本地 LLM 服务不可达
@dataclass
class ErrorResponse:
"""四字段错误响应（与现有 errors.py 保持一致）"""
code: str
message: str
retryable: bool
details: dict = field(default_factory=dict)
def to_http(self) -> tuple[int, dict]:
"""业务码 → HTTP 码：映射集中一处，避免散落各函数"""
http_map = {
ErrorCode.INVALID_JSON: 400,
ErrorCode.MISSING_FIELD: 400,
ErrorCode.INVALID_TASK: 400,
ErrorCode.OUT_OF_WORKSPACE: 422, # 语义正确但不可执行
ErrorCode.UNKNOWN_TARGET: 404, # 资源不存在
ErrorCode.FORBIDDEN: 403, # 权限不足
ErrorCode.FORBIDDEN_LOW_LEVEL_FIELD: 403,
ErrorCode.ARM_BUSY: 409,
ErrorCode.ESTOP_ACTIVE: 409,
ErrorCode.DISABLED: 409,
ErrorCode.GRIPPER_FAULT: 409,
ErrorCode.TRAJECTORY_TIMEOUT: 504,
ErrorCode.COLLISION_RISK: 409,
ErrorCode.INTERNAL_ERROR: 500,
ErrorCode.LLM_UNAVAILABLE: 503,
}
status = http_map.get(self.code, 500)
return status, {
"code": self.code,
"message": self.message,
"retryable": self.retryable,
"details": self.details,
}1.6 错误码矩阵表（≥8 行，这就是 docs/error-code-matrix.md 的雏形）
| 业务码 | 含义 | HTTP | retryable | 典型场景 | 谁来修 |
|---|---|---|---|---|---|
INVALID_JSON |
JSON 无法解析 | 400 | ❌ | LLM/客户端输出坏 JSON | 模型/客户端重发 |
MISSING_FIELD |
缺必填字段 | 400 | ❌ | 请求缺 target |
调用方补字段 |
INVALID_TASK |
任务语义非法 | 400 | ❌ | 说"抓月亮" | 换说法 |
OUT_OF_WORKSPACE |
位姿越界 | 422 | ❌ | radius=0.35m / z=0.6m | 改位姿（系统生成，不靠 LLM 手写） |
UNKNOWN_TARGET |
目标不在白名单 | 404 | ❌ | LLM 输出"蓝色杯子"（表里没有） | 询问用户确认 |
FORBIDDEN |
权限不足 | 403 | ❌ | operator 尝试解除急停 | 换 admin |
ARM_BUSY |
手臂忙 | 409 | ✅ | 正在执行任务时又来新指令 | 等待后重试 |
ESTOP_ACTIVE |
急停未解除 | 409 | ❌ | 急停按下 | 人工解除 |
TRAJECTORY_TIMEOUT |
运动超时 | 504 | ✅ | 规划/执行超时 | 重试 1 次，再失败交状态机 |
INTERNAL_ERROR |
未知内部错误 | 500 | ❌ | 未捕获异常 | 运维介入 |
记忆口诀："4xx 是请求的错，5xx 是机器的错；能不能重试，看是不是瞬态、重试能不能赢、次数够不够。"
主题② 实机兼容映射表（下午 2/2）
2.1 三层架构（你项目的核心分层）
ROS2 真机/仿真 (srv/msg) ←→ 包装节点(HTTP DTO) ←→ 工具层(LLM schema)
底层字段/单位/错误 隐藏细节/统一单位/ 10 个原子工具
mm/度、scaling、 语义化重命名 m/rad + base_link
motor_faults...原则：LLM 永远只接触最右层。 底层字段不出现在 tools= schema 里，单位换算只发生在左→中 或 中→左 的边界。
2.2 已核实的 Panthera 真实接口（去 panthera_interfaces/ 对着看）
srv/MoveToPose.srv：x/y/z/roll/pitch/yaw+cartesian_path+velocity_scaling+acceleration_scaling→bool success + string messagesrv/MoveToJoint.srv：float64[6] joint_angles+ 两个 scaling →success/messagesrv/GripperControl.srv：string action（open/close/half_open）→success/messagesrv/Enable.srv：bool enable_request→bool enable_responsesrv/GoZero.srv：bool use_mit_mode→int64 code + bool statusmsg/ArmStatus.msg：arm_enabled、motion_status(0/1/2)、error_message、motor_modes[6]、motor_faults[6]、joint_at_limit[6]、gripper_position、gripper_faultmsg/EndPoseEuler.msg：x/y/z/roll/pitch/yaw（float64）
2.3 映射表（docs/panthera-mapping.md 的雏形）
| ROS 接口 | ROS 字段 | HTTP DTO | 工具层 | 说明 |
|---|---|---|---|---|
MoveToPose.srv |
x/y/z/roll/pitch/yaw + 3 个隐藏字段 | POST /arm/move_to_pose {position, orientation} |
move_to(pose) |
隐藏 cartesian_path/scaling；m/rad 透传 |
MoveToJoint.srv |
joint_angles[6] |
POST /arm/move_to_joint（仅内部/调试） |
不暴露给 LLM | 关节角不进 LLM schema |
GripperControl.srv |
action(open/close/half_open) |
POST /arm/gripper {action} |
grasp()→close、place()→open |
动作名语义化 |
Enable.srv |
enable_request |
POST /arm/enable |
状态机内部 | 使能不进 LLM |
GoZero.srv |
use_mit_mode → code/status |
POST /arm/home |
home() |
隐藏 use_mit_mode |
ArmStatus.msg |
motion_status(0/1/2) 等 8 字段 | GET /arm/status |
get_status() |
裁剪：只留 joints/pose/gripper/mode；0/1/2→idle/moving/error |
EndPoseEuler.msg |
x/y/z/roll/pitch/yaw | status.pose |
get_pose(target) |
与 scene.json 目标绑定 |
2.4 四条映射原则（面试也能讲）
- 只暴露"LLM 需要决策的语义" ：
cartesian_path、velocity_scaling、motor_faults、use_mit_mode这类执行细节，LLM 永远看不到。 - 单位换算只在 ROS 边界 ：
mm/度 ↔ m/rad只发生在包装节点；LLM 只见 m/rad + base_link。 - 字段语义化重命名 ：
motion_status的 0/1/2 →mode: "idle"/"moving"/"error"；gripper_position→"open"/"closed"。 - 错误对齐 ：ROS
success=false + message→ 映射到主题①的错误码矩阵，message原文保留进details供审计。
2.5 代码：边界转换示例
# 包装节点边界转换：DTO(m/rad) → ROS srv（中文注释）
def dto_to_move_pose(dto: dict) -> MoveToPoseRequest:
"""LLM 的 m/rad 位姿 → MoveToPose.srv 请求"""
req = MoveToPoseRequest()
req.x, req.y, req.z = dto["position"]["x"], dto["position"]["y"], dto["position"]["z"]
req.roll, req.pitch, req.yaw = (
dto["orientation"]["roll"],
dto["orientation"]["pitch"],
dto["orientation"]["yaw"],
)
# 隐藏字段：固定默认值，LLM 永远不接触
req.cartesian_path = False
req.velocity_scaling = 1.0
req.acceleration_scaling = 1.0
return req
def ros_status_to_dto(arm_status: ArmStatus) -> dict:
"""ArmStatus.msg → 工具层 get_status() 的 {joints, pose, gripper, mode}"""
mode_map = {0: "idle", 1: "moving", 2: "error"}
return {
"arm_enabled": arm_status.arm_enabled,
"mode": mode_map.get(arm_status.motion_status, "unknown"),
"joints": list(arm_status.motor_modes), # 示例：简化处理
"pose": {
"x": arm_status.pose.x, "y": arm_status.pose.y, "z": arm_status.pose.z,
"roll": arm_status.pose.roll, "pitch": arm_status.pose.pitch, "yaw": arm_status.pose.yaw,
},
"gripper": "open" if arm_status.gripper_position > 0.5 else "closed",
"fault": arm_status.error_message or None,
}主题③ 契约变更规则（晚上）
3.1 兼容 vs 破坏性判定表（docs/contract-change-rules.md 的核心）
| 变更类型 | 判定 | 示例 |
|---|---|---|
| 新增可选字段 | ✅ 兼容 | DTO 加 notes?: string |
| 新增枚举值 | ⚠️ 兼容（谨慎） | mode 增加 "paused"（消费方要能容忍未知值） |
| 新增必填字段 | ❌ 破坏性 | 请求必须带 task_id |
| 删除字段 | ❌ 破坏性 | 删除 position.z |
| 修改类型 | ❌ 破坏性 | int → string |
| 修改单位/语义 | ❌ 破坏性 | m → mm、roll 从度改弧度 |
| 修改取值范围 | ❌ 破坏性（保守判定） | radius 0.28 → 0.30 |
| 修改/删除错误码 | ❌ 破坏性 | 重命名 OBJECT_NOT_FOUND |
保守原则：拿不准就算破坏性。LLM 的 schema 一旦变化，老 prompt 缓存、已部署的 Agent、测试集都可能受影响。
3.2 代码：把判定规则"可执行化"
# 简易契约变更判定器（中文注释）------ 用 OpenAPI/JSON Schema 的 diff 驱动
def classify_change(old: dict, new: dict) -> str:
"""返回 'compatible' 或 'breaking'（保守判定）"""
# 1) 删了必填字段 → 破坏性
for name in old.get("required", []):
if name not in new.get("required", []):
return "breaking"
# 2) 字段被删除 / 类型变了 / 枚举收缩 → 破坏性
for name, old_prop in old.get("properties", {}).items():
new_prop = new.get("properties", {}).get(name)
if new_prop is None:
return "breaking" # 删除字段
if new_prop.get("type") != old_prop.get("type"):
return "breaking" # 类型变更
if old_prop.get("enum") and old_prop.get("enum") != new_prop.get("enum"):
return "breaking" # 枚举变更
# 3) 新增字段：可选 → 兼容；必填 → 破坏性
for name, new_prop in new.get("properties", {}).items():
if name not in old.get("properties", {}):
if name in new.get("required", []):
return "breaking"
return "compatible"3.3 五步变更流程
1. 变更提案 写清「改什么 / 为什么 / 影响面」（一条 PR 描述即可）
2. 影响分析 扫所有消费方：LLM schema、前端、测试集、状态机、其他服务
3. 版本管理 破坏性 → 大版本；兼容 → 小版本（见 3.4）
4. 迁移计划 兼容期双写 / 废弃期 / 下线时间表
5. 回归验证 test_arm_contract.py 全绿 + demo_contract_validate.py + 端到端 20 任务3.4 语义化版本策略（SemVer）
| 版本位 | 何时升 | 例子 |
|---|---|---|
| MAJOR（大版本） | 破坏性变更 | 1.x → 2.0（删除字段/改单位/改错误码） |
| MINOR（小版本） | 向后兼容的新功能 | 1.0 → 1.1（新增可选字段、新端点） |
| PATCH（补丁） | 修 bug | 1.1.0 → 1.1.1 |
配合你的 contracts/ 目录：schema 文件用 git 管理，每次变更过 git diff 评审 ，docs/contract-change-rules.md 里记录每一次"兼容/破坏性"判定结论。
10个Mock工具 + 防呆校验 + JSONL日志
纯仿真 Mock 工具层，上层 Agent Factory 调用的工具的软件模拟，完全不依赖 ROS、真机硬件 IQ‑9075，用于单元测试、Agent 逻辑调试，在不上实物机械臂的情况下验证大模型 Agent 任务编排逻辑。
python
"""mock_tools ------ 10 个原子工具的纯软件 Mock 实现（不接真机/ROS）。
统一约定（对齐《上层Demo实现方案 V1.0》§4.1/§4.4）：
- 所有位姿参数单位 m/rad，坐标系 base_link；
- 单位转换(mm/度↔m/rad)只在真实包装节点做，Mock 阶段直接以 m/rad 输入；
- 工具层内建防呆校验：单位数量级(unit_guard)、目标名白名单、工作空间 radius<=0.28m / z 范围；
- 非法输入一律返回 {"success": False, "message": "原因"}，绝不抛异常。
"""
from .logger import ToolCallLogger
from .config import (
ROOT,
load_json,
load_tools_schema,
load_scene,
load_scripted_inputs,
load_llm_config,
FROZEN_CATEGORIES,
)
from .arm_state import MockArm
from .scene import SceneManager
from .tools import ToolLayer
from . import tool_dispatch
__all__ = [
"ToolCallLogger", "ROOT", "load_json", "load_tools_schema", "load_scene",
"load_scripted_inputs", "load_llm_config", "FROZEN_CATEGORIES",
"MockArm", "SceneManager", "ToolLayer", "tool_dispatch",
]纯内存的机械臂状态模拟器，属于 mock_tools 的核心数据模型 ，不调用 ROS、不跟 IQ‑9075 硬件通信。只在内存维护机械臂全部运行状态，供上层 Agent 工具调用做仿真测试。 对应架构图：Agent Factory → Tools/Skills内部的虚拟硬件状态。
python
"""Mock 机械臂状态（纯软件模拟，不接真机/ROS）。
维护：末端位姿(base_link, m/rad)、6 关节角、夹爪开合、模式、夹持目标、播报记录。
"""
from __future__ import annotations
from copy import deepcopy
from typing import Any, Optional
class MockArm:
"""模拟机械臂的共享状态对象，工具层通过它读写「世界状态」。"""
def __init__(self, scene_cfg: dict) -> None:
self.home_pose: dict = deepcopy(scene_cfg["home_pose"])
self.workspace: dict = deepcopy(scene_cfg["workspace"])
self.table_z: float = float(scene_cfg["table_z"])
self.pose: dict = deepcopy(self.home_pose) # 当前末端位姿 (m/rad, base_link)
self.joints: list[float] = [0.0] * 6 # 关节角(rad)，Mock 恒为 0
self.gripper: str = "open" # open | closed
self.mode: str = "idle" # idle | moving | stopped | home
self.last_action: Optional[str] = None
self.held_target: Optional[str] = None # 当前夹持的目标类别名
self.utterances: list[str] = [] # speak 播报记录（Mock 不发声）
self.motion_events: list[dict] = [] # 运动轨迹事件（审计用）
def reset(self) -> None:
"""回到出厂状态（每个测试用例开始前调用，保证可复现）。"""
self.pose = deepcopy(self.home_pose)
self.joints = [0.0] * 6
self.gripper = "open"
self.mode = "idle"
self.last_action = None
self.held_target = None
self.utterances = []
self.motion_events = []
def set_pose(self, pose: dict) -> None:
self.pose = deepcopy(pose)
self.mode = "idle"
self.motion_events.append({"event": "move", "pose": deepcopy(pose)})
def set_home(self) -> None:
self.pose = deepcopy(self.home_pose)
self.joints = [0.0] * 6
self.mode = "home"
self.motion_events.append({"event": "home", "pose": deepcopy(self.home_pose)})
def status(self) -> dict:
"""get_status 工具的返回载荷。"""
return {
"success": True,
"joints": list(self.joints), # rad
"pose": deepcopy(self.pose), # base_link, m/rad
"gripper": self.gripper, # open/closed
"mode": self.mode,
"last_action": self.last_action,
"held_target": self.held_target,
"frame_id": "base_link",
}项目统一配置入口 ，全部 JSON 配置放在工程config/文件夹；Mock 仿真环境、未来真机环境共用这一套配置读取函数。上层 Agent、MockArm、SceneManager 全部从这里读取参数。
python
"""配置加载：统一从工程根目录下的 config/ 读取 JSON 配置。"""
from __future__ import annotations
import json
from pathlib import Path
# 工程根目录（mock_tools 的上一级）
ROOT = Path(__file__).resolve().parent.parent
# 冻结类别表（目标名白名单，对齐《本地LLM选型与训练方案》§3.2 / §2.4）
FROZEN_CATEGORIES = ["红色杯子", "蓝色方块", "绿色瓶盖", "黄色纸盒"]
# 常见同义词（用于 Mock LLM 的目标名归一，真实 LLM 由提示词约束）
SYNONYMS = {
"红杯": "红色杯子", "红杯子": "红色杯子", "那个红杯子": "红色杯子",
"蓝块": "蓝色方块", "蓝方块": "蓝色方块", "蓝色块": "蓝色方块",
"绿盖": "绿色瓶盖", "绿瓶盖": "绿色瓶盖",
"黄盒": "黄色纸盒", "黄纸盒": "黄色纸盒",
}
def load_json(rel_path: str | Path) -> dict:
"""从工程根目录加载 JSON 文件（UTF-8，兼容 Windows 写入的 BOM）。"""
with (ROOT / rel_path).open("r", encoding="utf-8-sig") as f:
return json.load(f)
def load_tools_schema(rel_path: str = "config/tools_v1.json") -> list[dict]:
"""加载 §4.3 的 10 个工具 schema（OpenAI tools 格式数组）。"""
data = load_json(rel_path)
return data["tools"]
def load_scene(rel_path: str = "config/scene.json") -> dict:
return load_json(rel_path)
def load_scripted_inputs(rel_path: str = "config/scripted_inputs.json") -> dict:
return load_json(rel_path)
def load_llm_config(rel_path: str = "config/llm.json") -> dict:
return load_json(rel_path)Agent 工具调用的审计日志组件 ，属于 mock_tools 的基础设施；Mock 仿真阶段使用，真机环境完全可以复用这套日志逻辑 。 核心目的：完整留存每一次工具调用全量信息，用于调试 Agent 规划逻辑、定位工具调用错误、单元测试断言、事后回放整个分拣任务流程。 输出格式：JSON Lines(.jsonl)，一行一条日志记录，方便后续脚本解析。
python
"""工具层内建防呆校验（对齐《上层Demo实现方案 V1.0》§4.4 + §2.4(c)）。
统一返回 (ok: bool, reason: str)，供所有工具调用；非法输入返回「失败+原因」而非抛异常。
"""
from __future__ import annotations
import math
from typing import Any
# 单位防呆：把 mm 误当 m 传入会在数量级上立刻暴露（280mm=0.28m，而 280 远超 1.0）
UNIT_X_Y_RANGE = (-1.0, 1.0) # x/y 数量级
UNIT_Z_RANGE = (0.0, 0.6) # z 数量级（桌面以上）
POSE_KEYS = ("x", "y", "z", "rx", "ry", "rz")
def unit_guard(pose: dict, z_max: float = 0.6) -> bool:
"""防呆：位姿参数数量级必须是 m/rad（§2.4c）。通过返回 True。"""
try:
x, y, z = float(pose["x"]), float(pose["y"]), float(pose["z"])
except (KeyError, TypeError, ValueError):
return False
if not (UNIT_X_Y_RANGE[0] <= x <= UNIT_X_Y_RANGE[1]):
return False
if not (UNIT_X_Y_RANGE[0] <= y <= UNIT_X_Y_RANGE[1]):
return False
if not (UNIT_Z_RANGE[0] <= z <= min(UNIT_Z_RANGE[1], z_max)):
return False
# 旋转量应在合理弧度范围内（数量级校验）
for k in ("rx", "ry", "rz"):
try:
v = float(pose.get(k, 0.0))
except (TypeError, ValueError):
return False
if not (-3.2 <= v <= 3.2):
return False
return True
def check_workspace(pose: dict, workspace: dict) -> tuple[bool, str]:
"""工作空间校验：radius<=0.28m、z∈[桌面,0.45m]（§4.4 第 6 点）。"""
try:
x, y, z = float(pose["x"]), float(pose["y"]), float(pose["z"])
except (KeyError, TypeError, ValueError):
return False, f"位姿字段缺失或非数值: {pose}"
max_radius = float(workspace.get("max_radius", 0.28))
z_min = float(workspace.get("z_min", 0.0))
z_max = float(workspace.get("z_max", 0.45))
r = math.hypot(x, y)
if r > max_radius + 1e-9:
return False, f"超出可达半径: {r * 1000:.0f}mm > {max_radius * 1000:.0f}mm"
if not (z_min - 1e-9 <= z <= z_max + 1e-9):
return False, f"z 超出范围: {z * 1000:.0f}mm 不在 [{z_min * 1000:.0f}, {z_max * 1000:.0f}]mm"
return True, "ok"
def validate_pose(pose: dict, workspace: dict) -> tuple[bool, str]:
"""组合防呆：字段齐全 + 数值类型 + m/rad 数量级 + 工作空间。"""
if not isinstance(pose, dict):
return False, "位姿必须是 JSON 对象"
for k in POSE_KEYS:
v = pose.get(k)
if v is None:
return False, f"缺少位姿字段: {k}"
if not isinstance(v, (int, float)) or isinstance(v, bool):
return False, f"位姿字段 {k} 必须是 number，收到 {type(v).__name__}"
if not unit_guard(pose):
return False, "单位防呆失败: 位姿数量级不符合 m/rad（x,y∈[-1,1], z∈[0,0.6]）"
ok, reason = check_workspace(pose, workspace)
if not ok:
return False, reason
return True, "ok"
def check_target_whitelist(name: str, categories: list[str]) -> tuple[bool, str]:
"""目标名白名单校验：必须来自冻结类别表。"""
if name in categories:
return True, "ok"
return False, f"目标 '{name}' 不在冻结类别表 {categories} 中，请先向用户确认"
def normalize_location(location: Any, place_locations: dict) -> tuple[bool, str, str]:
"""放置点归一化：default/box → 收纳盒（对齐 §4.3 place 的 enum）。"""
if location is None or location in ("default", "box"):
loc = "收纳盒"
elif isinstance(location, str):
loc = location
else:
return False, "放置点必须是字符串", ""
if loc not in place_locations:
return False, f"未知放置点 '{loc}'，可用: {list(place_locations)}", ""
return True, "ok", locMock 仿真环境里的「虚拟视觉模块」 真机是 YOLO+3D 视觉输出物体类别、置信度、6D 位姿； Mock 模式下没有摄像头，SceneManager直接从scene.json读取物体真值，模拟视觉 detect 检测接口输出。 对应工具：detect_object工具底层就是调用本类；和MockArm配合：机械臂抓取物体时，同步更新物体grabbed/present状态。
python
"""场景管理：从 config/scene.json 读取目标列表，维护抓取/放置状态。
detect() 只返回「在场且未被抓取」的目标；get_pose() 从场景取位姿。
"""
from __future__ import annotations
from copy import deepcopy
from typing import Any, Optional
class SceneManager:
"""维护当前场景中的目标（视觉识别结果的 Mock 来源）。"""
def __init__(self, scene_cfg: dict) -> None:
self.cfg: dict = scene_cfg
self.categories: list[str] = list(scene_cfg["categories"])
self.place_locations: dict = deepcopy(scene_cfg["place_locations"])
self.workspace: dict = deepcopy(scene_cfg["workspace"])
self.frame_id: str = scene_cfg.get("frame_id", "base_link")
# 深拷贝，避免配置被修改；每个用例可重建
self.targets: list[dict] = [deepcopy(t) for t in scene_cfg["targets"]]
def reset(self) -> None:
self.targets = [deepcopy(t) for t in self.cfg["targets"]]
def get_target(self, class_name: str) -> Optional[dict]:
for t in self.targets:
if t["class"] == class_name:
return t
return None
def present_targets(self) -> list[dict]:
"""在场且未被抓取的目标（detect 返回列表）。"""
return [t for t in self.targets if t.get("present") and not t.get("grabbed")]
def target_pose(self, class_name: str) -> Optional[dict]:
t = self.get_target(class_name)
return deepcopy(t["pose"]) if t else None
def mark_grabbed(self, class_name: str) -> None:
t = self.get_target(class_name)
if t:
t["grabbed"] = True
def mark_placed(self, class_name: str) -> None:
t = self.get_target(class_name)
if t:
t["present"] = False # 已放置（离开桌面视野）
t["grabbed"] = False
def count_remaining(self) -> int:
return len(self.present_targets())
def detect_payload(self) -> dict:
"""detect() 的返回载荷：可配置的当前场景目标列表。"""
items = []
for t in self.present_targets():
items.append({
"class": t["class"],
"confidence": float(t["confidence"]),
"bbox": list(t["bbox"]),
"pose": deepcopy(t["pose"]), # base_link, m/rad
"frame_id": self.frame_id,
})
return {"success": True, "message": "ok", "targets": items, "count": len(items)}Agent 大模型和 ToolLayer 工具实现之间的中间转发层 Agent（Claude‑Code / DeepSeek‑v4‑flash）输出工具调用请求，不会直接调用ToolLayer内部方法；统一经过execute_tool()做分发。 价值：隔离大模型侧与工具实现，支持全局单会话模式，也支持单元测试每个用例传入独立ToolLayer实例，各个测试用例环境互不干扰
python
"""工具分发：工具名 → ToolLayer.execute（对齐《本地LLM选型与训练方案》附录 B.1）。
默认使用全局 TOOL_LAYER（由 session 装配）；也支持显式传入 layer，便于每用例独立环境。
"""
from __future__ import annotations
from typing import Optional
from .tools import ToolLayer, TOOL_NAMES
# 全局默认工具层（单会话场景）；eval_agent 每用例会显式注入独立 layer
TOOL_LAYER: Optional[ToolLayer] = None
def set_tool_layer(layer: ToolLayer) -> None:
global TOOL_LAYER
TOOL_LAYER = layer
def execute_tool(name: str, args: dict, layer: Optional[ToolLayer] = None) -> dict:
"""执行工具；未知工具名返回失败（不抛异常）。"""
layer = layer or TOOL_LAYER
if layer is None:
return {"success": False, "message": "工具层未初始化（请先创建 Session）"}
if name not in TOOL_NAMES:
return {"success": False, "message": f"未知工具: {name}"}
return layer.execute(name, args)
__all__ = ["TOOL_LAYER", "set_tool_layer", "execute_tool", "TOOL_NAMES"]Agent 大模型和 ToolLayer 工具实现之间的中间转发层 Agent（Claude‑Code / DeepSeek‑v4‑flash）输出工具调用请求，不会直接调用ToolLayer内部方法；统一经过execute_tool()做分发。 价值：隔离大模型侧与工具实现，支持全局单会话模式，也支持单元测试每个用例传入独立ToolLayer实例，各个测试用例环境互不干扰。
python
"""10 个原子工具的 Mock 实现（不接真机/ROS，行为尽量贴近真实）。
对齐《上层Demo实现方案 V1.0》§4.2 工具总表：
detect / get_pose / grasp / place / move_to / stop / home / speak / listen / get_status
要点：
- 每个工具被调用时经 `_call` 统一包装：记录调用时间/入参/结果/耗时（ToolCallLogger）+ 控制台打印；
- 越界/非法输入返回 {"success": False, "message": "原因"}，不抛异常；
- 支持「第 N 次失败」等脚本化行为（behavior: {"grasp_fail_calls": [1], ...}），便于测试重试逻辑；
- 固定 seed / 显式配置 → 行为可复现。
"""
from __future__ import annotations
import json
import math
import random
import time
from collections import defaultdict
from typing import Any, Callable, Optional
from .arm_state import MockArm
from .scene import SceneManager
from .logger import ToolCallLogger
from .safety import (
check_target_whitelist,
normalize_location,
unit_guard,
validate_pose,
)
# 工具名白名单（§4.2）
TOOL_NAMES = [
"detect", "get_pose", "grasp", "place", "move_to",
"stop", "home", "speak", "listen", "get_status",
]
# 模拟执行时延（秒）：让日志中的耗时更贴近真实（可用 --no-latency 关闭以加速测试）
DEFAULT_LATENCY = {
"detect": 0.020, "get_pose": 0.015, "grasp": 0.045, "place": 0.040,
"move_to": 0.035, "stop": 0.005, "home": 0.030, "speak": 0.010,
"listen": 0.020, "get_status": 0.005,
}
# 抓取容差：grasp 传入的位姿与场景目标位姿的距离小于该值才算「对准目标」
GRASP_MATCH_TOL = 0.03
class ToolLayer:
"""工具层：持有 Mock 机械臂/场景/脚本化语音，统一分发 + 日志。"""
def __init__(
self,
arm: MockArm,
scene: SceneManager,
scripted_inputs: Optional[dict] = None,
behavior: Optional[dict] = None,
logger: Optional[ToolCallLogger] = None,
initial_holding: Optional[str] = None,
seed: int = 0,
simulate_latency: bool = True,
event_sink: Optional[Any] = None,
) -> None:
self.arm = arm
self.scene = scene
self.behavior = behavior or {}
self.logger = logger
self.seed = seed
self.simulate_latency = simulate_latency
self.event_sink = event_sink # 实时仪表盘事件发布（可空）
self._rng = random.Random(seed)
# 脚本化语音输入队列（模拟 ASR）
self._inputs: list[str] = list((scripted_inputs or {}).get("inputs", []))
self._loop = bool((scripted_inputs or {}).get("loop", True))
self._input_idx = 0
# 每个工具被调用次数（用于「第 N 次失败」脚本）
self._counters: dict[str, int] = defaultdict(int)
if initial_holding:
self._seed_holding(initial_holding)
# ---------- 统一分发 + 日志包装 ----------
def execute(self, name: str, args: dict) -> dict:
"""按工具名分发（Agent 与状态机统一入口）。未知工具返回失败而非抛异常。"""
fn = getattr(self, name, None)
if fn is None:
return self._finish(name, args, {"success": False, "message": f"未知工具: {name}"})
if not isinstance(args, dict):
return self._finish(name, args, {"success": False, "message": "工具参数必须是 JSON 对象"})
return self._call(name, args, fn)
def _call(self, name: str, args: dict, fn: Callable[..., dict]) -> dict:
"""统一包装：计数 → 模拟时延 → 执行 → 记录日志（时间戳/入参/结果/耗时）。
若配置了 event_sink（实时仪表盘），先发布 tool_start（"正在调用"），
执行完发布 tool_end + 机械臂状态快照（"被调用的状态"实时可见）。
"""
self._counters[name] += 1
call_no = self._counters[name]
if self.event_sink:
self.event_sink.publish("tool_start", {"tool": name, "args": args, "call_no": call_no})
t0 = time.perf_counter()
if self.simulate_latency:
time.sleep(DEFAULT_LATENCY.get(name, 0.005)) # 模拟执行时延（计入耗时）
try:
result = fn(**args)
except TypeError as e: # 参数不匹配 → 失败，不抛异常
result = {"success": False, "message": f"工具 '{name}' 参数错误: {e}"}
except Exception as e: # 兜底：工具内部异常也转成失败返回
result = {"success": False, "message": f"工具 '{name}' 执行异常: {e}"}
duration_ms = (time.perf_counter() - t0) * 1000.0
if self.logger:
self.logger.log(name, args, result, duration_ms)
if self.event_sink:
self.event_sink.publish("tool_end", {
"tool": name, "args": args, "result": result,
"duration_ms": round(duration_ms, 3), "call_no": call_no,
})
self.event_sink.publish("status", self.arm.status())
return result
def _finish(self, name: str, args: dict, result: dict) -> dict:
"""未进入正常执行路径时也记录日志（保持可审计）。"""
duration_ms = 0.0
if self.logger:
self.logger.log(name, args, result, duration_ms)
if self.event_sink:
self.event_sink.publish("tool_start", {"tool": name, "args": args, "call_no": self._counters[name]})
self.event_sink.publish("tool_end", {
"tool": name, "args": args, "result": result,
"duration_ms": round(duration_ms, 3), "call_no": self._counters[name],
})
self.event_sink.publish("status", self.arm.status())
return result
def _scripted_fail(self, tool: str, what: str) -> Optional[dict]:
"""「第 N 次失败」脚本：behavior={"grasp_fail_calls": [1,3]} 表示第 1、3 次调用失败。"""
fail_calls = self.behavior.get(f"{tool}_fail_calls", [])
if self._counters[tool] in fail_calls:
return {"success": False,
"message": f"脚本化{tool}失败(第 {self._counters[tool]} 次调用，fail_calls={fail_calls})"}
return None
# ---------- 10 个原子工具 ----------
def detect(self) -> dict:
"""detect()：返回可配置的当前场景目标列表（读 scene.json）。"""
return self.scene.detect_payload()
def get_pose(self, target: str) -> dict:
"""get_pose(目标)：类别表内 → base_link 下 m/rad 位姿；不在表内 → 失败。"""
ok, reason = check_target_whitelist(target, self.scene.categories)
if not ok:
return {"success": False, "message": reason}
t = self.scene.get_target(target)
if t is None:
return {"success": False, "message": f"场景中未找到目标 '{target}'"}
if t.get("grabbed"):
return {"success": False, "message": f"目标 '{target}' 已被抓取，无法再次获取位姿"}
pose = self.scene.target_pose(target)
return {"success": True, "message": "ok", "target": target,
"pose": pose, "frame_id": "base_link"}
def grasp(self, x: float, y: float, z: float,
rx: float = 0.0, ry: float = 0.0, rz: float = 0.0) -> dict:
"""grasp(位姿)：模拟移动到目标位姿 → 闭合夹爪抓取。越界/单位异常返回失败。"""
pose = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz": rz}
ok, reason = validate_pose(pose, self.scene.workspace)
if not ok:
return {"success": False, "message": f"grasp 位姿校验失败: {reason}"}
fail = self._scripted_fail("grasp", "抓取")
if fail:
return fail
# 在目标位姿附近找最近的在场目标（模拟「对准目标再抓」）
nearest, dist = self._nearest_present_target(pose)
if nearest is None or dist > GRASP_MATCH_TOL:
return {"success": False,
"message": f"目标位姿附近({dist * 1000:.0f}mm)未检测到可抓取目标"}
self.arm.pose = dict(pose)
self.arm.gripper = "closed"
self.arm.held_target = nearest["class"]
self.arm.last_action = f"grasp:{nearest['class']}"
self.scene.mark_grabbed(nearest["class"])
return {"success": True, "message": f"已抓取 {nearest['class']}",
"target": nearest["class"], "pose": pose}
def place(self, location: str = "收纳盒") -> dict:
"""place()：把当前夹持物放置到指定放置点（默认收纳盒），先到位再张爪。"""
ok, reason, loc = normalize_location(location, self.scene.place_locations)
if not ok:
return {"success": False, "message": reason}
if self.arm.gripper != "closed" or not self.arm.held_target:
return {"success": False, "message": "夹爪未夹持物体，无法放置"}
fail = self._scripted_fail("place", "放置")
if fail:
return fail
target = self.arm.held_target
place_pose = self.scene.place_locations[loc]
self.arm.pose = dict(place_pose)
self.arm.gripper = "open"
self.arm.held_target = None
self.arm.last_action = f"place:{loc}:{target}"
self.scene.mark_placed(target)
return {"success": True, "message": f"已把 {target} 放到 {loc}", "location": loc}
def move_to(self, x: float, y: float, z: float,
rx: float = 0.0, ry: float = 0.0, rz: float = 0.0) -> dict:
"""move_to(位姿)：移动末端到位姿（m/rad, base_link）。越界返回失败。"""
pose = {"x": x, "y": y, "z": z, "rx": rx, "ry": ry, "rz": rz}
ok, reason = validate_pose(pose, self.scene.workspace)
if not ok:
return {"success": False, "message": f"move_to 位姿校验失败: {reason}"}
fail = self._scripted_fail("move_to", "移动")
if fail:
return fail
self.arm.set_pose(pose)
self.arm.last_action = "move_to"
return {"success": True, "message": "ok", "pose": pose}
def stop(self) -> dict:
"""stop()：软急停，最高优先级，记录状态。"""
self.arm.mode = "stopped"
self.arm.last_action = "stop"
return {"success": True, "message": "已急停", "mode": "stopped"}
def home(self) -> dict:
"""home()：回安全原点。"""
self.arm.set_home()
self.arm.last_action = "home"
return {"success": True, "message": "已回原点", "pose": self.arm.home_pose}
def speak(self, text: str) -> dict:
"""speak(文本)：记录播报文本（Mock 不发声）。"""
if not isinstance(text, str) or not text.strip():
return {"success": False, "message": "speak 文本不能为空"}
self.arm.utterances.append(text)
self.arm.last_action = f"speak:{text[:20]}"
return {"success": True, "message": "ok", "text": text}
def listen(self) -> dict:
"""listen()：从 scripted_inputs.json 取一条脚本化语音输入（模拟 ASR）。"""
if not self._inputs:
return {"success": False, "message": "scripted_inputs 为空，无语音输入"}
text = self._inputs[self._input_idx % len(self._inputs)]
self._input_idx += 1
self.arm.last_action = "listen"
return {"success": True, "message": "ok", "text": text, "engine": "mock_asr"}
def get_status(self) -> dict:
"""get_status()：返回 mock 的 {joints, pose, gripper, mode, ...}。"""
return self.arm.status()
# ---------- 内部辅助 ----------
def _nearest_present_target(self, pose: dict):
"""返回 (最近在场目标, 欧氏距离)。"""
best, best_d = None, float("inf")
for t in self.scene.present_targets():
p = t["pose"]
d = math.dist([p["x"], p["y"], p["z"]], [pose["x"], pose["y"], pose["z"]])
if d < best_d:
best, best_d = t, d
return best, best_d
def _seed_holding(self, class_name: str) -> None:
"""测试辅助：预置夹爪夹持某目标（用于 place 单工具用例）。"""
t = self.scene.get_target(class_name)
if t:
self.arm.gripper = "closed"
self.arm.held_target = class_name
self.scene.mark_grabbed(class_name)
def as_json(self) -> str:
"""把当前工具层状态序列化为 JSON（审计/复现用）。"""
return json.dumps({
"arm": {"pose": self.arm.pose, "gripper": self.arm.gripper,
"mode": self.arm.mode, "held_target": self.arm.held_target},
"scene": {"targets": self.scene.targets},
"counters": dict(self._counters),
}, ensure_ascii=False, indent=2)