diff --git a/docs/moveit2_integration_summary.md b/docs/moveit2_integration_summary.md index e645f6af..b008226e 100644 --- a/docs/moveit2_integration_summary.md +++ b/docs/moveit2_integration_summary.md @@ -467,43 +467,58 @@ set_status(command_json) #### 2.1.9 核心方法详解:`pick_and_place` ```python -def pick_and_place(self, command: str) +def pick_and_place( + self, + option: str, + move_group: str, + status: str, + resource: Optional[str] = None, + x_distance: Optional[float] = None, + y_distance: Optional[float] = None, + lift_height: Optional[float] = None, + retry: Optional[int] = None, + speed: Optional[float] = None, + target: Optional[str] = None, + constraints: Optional[Sequence[float]] = None, +) -> None: ``` 这是 `MoveitInterface` 最复杂的方法,实现了完整的抓取-放置工作流。它动态构建一个**有序函数列表** (`function_list`),然后顺序执行。 -**JSON 指令格式(完整参数):** +**破坏性变更(注册表 / 客户端)**:`pick_and_place` 在注册表中由 **SendCmd**(单字段 `command` JSON 字符串)改为 **UniLabJsonCommand**,goal 为与上表同名的**结构化字段**。云端与其它调用方需按 schema 提交 goal,而不再把整段 JSON 塞进 `command`。 + +**动作 goal 示例(字段与旧版 JSON 一致,现为结构化 goal):** ```json { - "option": "pick", // *必须: pick/place/side_pick/side_place - "move_group": "arm", // *必须: MoveIt2 规划组名 - "status": "pick_station_A", // *必须: 在 joint_poses 中的目标状态名 - "resource": "beaker_1", // 要操作的资源名称 - "target": "custom_link", // pick 时资源附着的目标 link (默认末端执行器) - "lift_height": 0.05, // 抬升高度 (米) - "x_distance": 0.1, // X 方向水平移动距离 (米) - "y_distance": 0.0, // Y 方向水平移动距离 (米) - "speed": 0.5, // 运动速度因子 (0.1~1.0) - "retry": 10, // 规划失败重试次数 - "constraints": [0, 0, 0, 0.5, 0, 0] // 各关节约束容差 (>0 时生效) + "option": "pick", + "move_group": "arm", + "status": "pick_station_A", + "resource": "beaker_1", + "target": "custom_link", + "lift_height": 0.05, + "x_distance": 0.1, + "y_distance": 0.0, + "speed": 0.5, + "retry": 10, + "constraints": [0, 0, 0, 0.5, 0, 0] } ``` -##### 阶段 1:指令解析与动作类型判定 +##### 阶段 1:参数与动作类型判定 ``` -pick_and_place(command_json) +pick_and_place(option, move_group, status, ...) │ - ├── JSON 解析 + ├── 校验 option ∈ move_option,否则直接 return ├── 动作类型判定: │ move_option = ["pick", "place", "side_pick", "side_place"] │ 0 1 2 3 - │ option_index = move_option.index(cmd["option"]) + │ option_index = move_option.index(option) │ place_flag = option_index % 2 ← 0=pick类, 1=place类 │ ├── 提取运动参数: - │ config = {speed, retry, move_group} ← 从 cmd_dict 中按需提取 + │ config = { move_group };若 speed/retry 非 None 则写入 config │ └── 获取目标关节位姿: joint_positions_ = joint_poses[move_group][status] @@ -515,7 +530,7 @@ pick_and_place(command_json) 根据 place_flag 决定资源 TF 操作: if pick 类 (place_flag == 0): - if "target" 已指定: + if target is not None: function_list += [resource_manager(resource, target)] ← 挂到自定义 link else: function_list += [resource_manager(resource, end_effector)] ← 挂到末端执行器 @@ -527,7 +542,7 @@ pick_and_place(command_json) ##### 阶段 3:构建关节约束 ``` -if "constraints" 存在于指令中: +if constraints is not None: for i, tolerance in enumerate(constraints): if tolerance > 0: JointConstraint( @@ -546,7 +561,7 @@ if "constraints" 存在于指令中: 这是最复杂的场景,涉及 FK/IK 计算和多段运动拼接: ``` -if "lift_height" 存在: +if lift_height is not None: │ ├── Step 1: FK 计算 → 获取目标关节配置对应的末端位姿 │ retval = compute_fk(joint_positions_) ← 可能需要重试 @@ -562,12 +577,12 @@ if "lift_height" 存在: │ function_list += [moveit_task(position=pose_lifted, ...)] │ ├── Step 4 (可选): 水平移动 - │ if "x_distance": + │ if x_distance is not None: │ deep_pose = copy(pose_lifted) │ deep_pose[0] += x_distance │ function_list = [moveit_task(pose_lifted)] + function_list │ function_list += [moveit_task(deep_pose)] - │ elif "y_distance": + │ elif y_distance is not None: │ 类似处理 Y 方向 │ ├── Step 5: IK 预计算 → 将末端位姿转换为安全的关节配置 @@ -585,7 +600,7 @@ if "lift_height" 存在: ##### 阶段 4B:无 `lift_height` 的简单流程 ``` -else (无 lift_height): +else (lift_height is None): │ └── 直接关节运动到目标位姿 function_list = [moveit_joint_task(joint_positions_)] + function_list @@ -600,10 +615,10 @@ for i, func in enumerate(function_list): │ i == 0: cartesian_flag = False ← 第一步用自由空间规划(大范围移动) │ i > 0: cartesian_flag = True ← 后续用笛卡尔直线规划(精确控制) │ - ├── result = func() ← 执行动作 + ├── re = func() ← 执行动作 │ - └── if not result: - return failure ← 任一步骤失败即中止 + └── if not re: + return(无返回值,不构造 SendCmd.Result)← 任一步骤失败即中止 ``` ##### 完整 pick 流程示例(含 lift_height + x_distance) @@ -657,10 +672,10 @@ for i, func in enumerate(function_list): | `.move_to_pose(...)` | `moveit_task` L129-137 | 笛卡尔空间运动规划与执行 | | `.wait_until_executed()` | `moveit_task` L138, `moveit_joint_task` L157 | 阻塞等待运动完成 | | `.move_to_configuration(...)` | `moveit_joint_task` L156 | 关节空间运动规划与执行 | -| `.compute_fk(...)` | `pick_and_place` L244, `moveit_joint_task` L160 | 正运动学:关节角 → 末端位姿 | -| `.compute_ik(...)` | `pick_and_place` L298-300 | 逆运动学:末端位姿 → 关节角(含约束) | -| `.end_effector_name` | `pick_and_place` L218 | 获取末端执行器 link 名 | -| `.joint_names` | `pick_and_place` L232, L308, L313 | 获取关节名列表 | +| `.compute_fk(...)` | `pick_and_place`, `moveit_joint_task` | 正运动学:关节角 → 末端位姿 | +| `.compute_ik(...)` | `pick_and_place` | 逆运动学:末端位姿 → 关节角(含约束) | +| `.end_effector_name` | `pick_and_place` | 获取末端执行器 link 名 | +| `.joint_names` | `pick_and_place` | 获取关节名列表 | --- @@ -668,11 +683,11 @@ for i, func in enumerate(function_list): | 场景 | 处理方式 | |------|---------| -| FK 计算失败 | 最多重试 `retry` 次(每次间隔 0.1s),超时返回 `result.success = False` | +| FK 计算失败 | 最多重试 `retry` 次(每次间隔 0.1s),超时则提前 `return`(无返回值) | | IK 计算失败 | 同上 | | 运动规划失败 | 在 `moveit_task` / `moveit_joint_task` 中最多重试 `retry+1` 次 | -| 动作序列中任一步失败 | `pick_and_place` 立即中止并返回 `result.success = False` | -| 未知异常 | `pick_and_place` 和 `set_status` 捕获 Exception,重置 `cartesian_flag`,返回失败 | +| 动作序列中任一步失败 | `pick_and_place` 立即中止并 `return`(不返回 `SendCmd.Result`) | +| 未知异常 | `pick_and_place` 捕获 Exception,打印并重置 `cartesian_flag`(`set_status` 仍返回 SendCmd.Result) | --- @@ -702,7 +717,8 @@ for i, func in enumerate(function_list): ``` 外部系统 (base_device_node) │ - │ JSON 指令字符串 + │ set_position/set_status: JSON 指令字符串(SendCmd.command) + │ pick_and_place: UniLabJsonCommand 结构化 goal → Python 关键字参数 ▼ ┌── MoveitInterface ──────────────────────────────────────────────────┐ │ │ @@ -710,7 +726,7 @@ for i, func in enumerate(function_list): │ │ │ set_status(cmd) ──→ moveit_joint_task() ──→ MoveIt2.move_to_config│ │ │ -│ pick_and_place(cmd) │ +│ pick_and_place(option, move_group, status, ...) │ │ │ │ │ ├─ MoveIt2.compute_fk() ─── /compute_fk service ──→ move_group │ │ ├─ MoveIt2.compute_ik() ─── /compute_ik service ──→ move_group │ @@ -963,7 +979,7 @@ robotic_arm.SCARA_with_slider.moveit.virtual: module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface type: python action_value_mappings: - pick_and_place: ... # SendCmd Action(JSON 指令) + pick_and_place: ... # UniLabJsonCommand(结构化 goal,与 Python 签名一致) set_position: ... # SendCmd Action set_status: ... # SendCmd Action auto-moveit_task: ... # 自动发现的方法(UniLabJsonCommand) diff --git a/unilabos/devices/liquid_handling/prcxi/prcxi.py b/unilabos/devices/liquid_handling/prcxi/prcxi.py index 855668e3..0e7d0c8e 100644 --- a/unilabos/devices/liquid_handling/prcxi/prcxi.py +++ b/unilabos/devices/liquid_handling/prcxi/prcxi.py @@ -1196,6 +1196,27 @@ class PRCXI9300Handler(LiquidHandlerAbstract): none_keys=none_keys, ) + @staticmethod + def _tip_rack_is_10ul_range(rack: TipRack) -> bool: + """判断 tip 盒是否为 10µL 量程(对应右头);优先用孔位上 prototype tip 的 maximal_volume。""" + children = getattr(rack, "children", None) or [] + if children: + spot = children[0] + tr = getattr(spot, "tracker", None) + tip = None + if tr is not None: + tip = getattr(tr, "_tip", None) or getattr(tr, "tip", None) + if tip is None: + tip = getattr(spot, "tip", None) + mv = getattr(tip, "maximal_volume", None) if tip is not None else None + if mv is not None: + try: + return float(mv) <= 10.0 + except (TypeError, ValueError): + pass + ident = f"{getattr(rack, 'model', '') or ''} {type(rack).__name__}".lower() + return "10ul" in ident + async def transfer_liquid( self, sources: Sequence[Container], @@ -1230,19 +1251,20 @@ class PRCXI9300Handler(LiquidHandlerAbstract): _asp_list = asp_vols if isinstance(asp_vols, list) else [asp_vols] _dis_list = dis_vols if isinstance(dis_vols, list) else [dis_vols] - if all(v <= 10.0 for v in _asp_list) and all(v <= 10.0 for v in _dis_list): - use_channels = [1] - mix_vol = max(min(mix_vol,10),0) if mix_vol is not None else None sources = await self._resolve_to_plr_resources(sources) targets = await self._resolve_to_plr_resources(targets) tip_racks = list(await self._resolve_to_plr_resources(tip_racks)) - change_slots = [] - change_slots.append(sources[0].parent) - change_slots.append(targets[0].parent) if isinstance(tip_racks[0], TipRack): tip_rack = tip_racks[0] else: tip_rack = tip_racks[0].parent + small_vols = all(v <= 10.0 for v in _asp_list) and all(v <= 10.0 for v in _dis_list) + if small_vols and self._tip_rack_is_10ul_range(tip_rack): + use_channels = [1] + mix_vol = max(min(mix_vol, 10), 0) if mix_vol is not None else None + change_slots = [] + change_slots.append(sources[0].parent) + change_slots.append(targets[0].parent) change_slots.append(tip_rack) diff --git a/unilabos/devices/ros_dev/moveit_interface.py b/unilabos/devices/ros_dev/moveit_interface.py index 81c2d112..66cba2fe 100644 --- a/unilabos/devices/ros_dev/moveit_interface.py +++ b/unilabos/devices/ros_dev/moveit_interface.py @@ -2,6 +2,7 @@ import json import time from copy import deepcopy from pathlib import Path +from typing import Optional, Sequence from moveit_msgs.msg import JointConstraint, Constraints from rclpy.action import ActionClient @@ -171,173 +172,160 @@ class MoveitInterface: return True - def pick_and_place(self, command: str): + def pick_and_place( + self, + option: str, + move_group: str, + status: str, + resource: Optional[str] = None, + x_distance: Optional[float] = None, + y_distance: Optional[float] = None, + lift_height: Optional[float] = None, + retry: Optional[int] = None, + speed: Optional[float] = None, + target: Optional[str] = None, + constraints: Optional[Sequence[float]] = None, + ) -> None: """ - Using MoveIt to make the robotic arm pick or place materials to a target point. + 使用 MoveIt 完成抓取/放置等序列(pick/place/side_pick/side_place)。 - Args: - command: A JSON-formatted string that includes option, target, speed, lift_height, mt_height - - *option (string) : Action type: pick/place/side_pick/side_place - *move_group (string): The move group moveit will plan - *status(string) : Target pose - resource(string) : The target resource - x_distance (float) : The distance to the target in x direction(meters) - y_distance (float) : The distance to the target in y direction(meters) - lift_height (float) : The height at which the material should be lifted(meters) - retry (float) : Retry times when moveit plan fails - speed (float) : The speed of the movement, speed > 0 - Returns: - None + 必选:option, move_group, status。 + 可选:resource, x_distance, y_distance, lift_height, retry, speed, target, constraints。 + 无返回值;失败时提前 return 或打印异常。 """ - result = SendCmd.Result() - try: - cmd_str = str(command).replace("'", '"') - cmd_dict = json.loads(cmd_str) + if option not in self.move_option: + raise ValueError(f"Invalid option: {option}") - if cmd_dict["option"] in self.move_option: - option_index = self.move_option.index(cmd_dict["option"]) - place_flag = option_index % 2 + option_index = self.move_option.index(option) + place_flag = option_index % 2 - config = {} - function_list = [] + config: dict = {"move_group": move_group} + if speed is not None: + config["speed"] = speed + if retry is not None: + config["retry"] = retry - status = cmd_dict["status"] - joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status] + function_list = [] + joint_positions_ = self.joint_poses[move_group][status] - config.update({k: cmd_dict[k] for k in ["speed", "retry", "move_group"] if k in cmd_dict}) + # 夹取 / 放置:绑定 resource 与 parent + if not place_flag: + if target is not None: + function_list.append(lambda r=resource, t=target: self.resource_manager(r, t)) + else: + ee = self.moveit2[move_group].end_effector_name + function_list.append(lambda r=resource: self.resource_manager(r, ee)) + else: + function_list.append(lambda r=resource: self.resource_manager(r, "world")) - # 夹取 - if not place_flag: - if "target" in cmd_dict.keys(): - function_list.append(lambda: self.resource_manager(cmd_dict["resource"], cmd_dict["target"])) - else: - function_list.append( - lambda: self.resource_manager( - cmd_dict["resource"], self.moveit2[cmd_dict["move_group"]].end_effector_name + joint_constraint_msgs: list = [] + if constraints is not None: + for i, c in enumerate(constraints): + v = float(c) + if v > 0: + joint_constraint_msgs.append( + JointConstraint( + joint_name=self.moveit2[move_group].joint_names[i], + position=joint_positions_[i], + tolerance_above=v, + tolerance_below=v, + weight=1.0, ) ) - else: - function_list.append(lambda: self.resource_manager(cmd_dict["resource"], "world")) - constraints = [] - if "constraints" in cmd_dict.keys(): + if lift_height is not None: + retval = None + attempts = config.get("retry", 10) + while retval is None and attempts > 0: + retval = self.moveit2[move_group].compute_fk(joint_positions_) + time.sleep(0.1) + attempts -= 1 + if retval is None: + raise ValueError("Failed to compute forward kinematics") + pose = [retval.pose.position.x, retval.pose.position.y, retval.pose.position.z] + quaternion = [ + retval.pose.orientation.x, + retval.pose.orientation.y, + retval.pose.orientation.z, + retval.pose.orientation.w, + ] - for i in range(len(cmd_dict["constraints"])): - v = float(cmd_dict["constraints"][i]) - if v > 0: - constraints.append( - JointConstraint( - joint_name=self.moveit2[cmd_dict["move_group"]].joint_names[i], - position=joint_positions_[i], - tolerance_above=v, - tolerance_below=v, - weight=1.0, - ) - ) + function_list = [ + lambda: self.moveit_task( + position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z], + quaternion=quaternion, + **config, + cartesian=self.cartesian_flag, + ) + ] + function_list - if "lift_height" in cmd_dict.keys(): - retval = None - retry = config.get("retry", 10) - while retval is None and retry > 0: - retval = self.moveit2[cmd_dict["move_group"]].compute_fk(joint_positions_) - time.sleep(0.1) - retry -= 1 - if retval is None: - result.success = False - return result - pose = [retval.pose.position.x, retval.pose.position.y, retval.pose.position.z] - quaternion = [ - retval.pose.orientation.x, - retval.pose.orientation.y, - retval.pose.orientation.z, - retval.pose.orientation.w, - ] + pose[2] += float(lift_height) + function_list.append( + lambda p=pose.copy(), q=quaternion, cfg=config: self.moveit_task( + position=p, quaternion=q, **cfg, cartesian=self.cartesian_flag + ) + ) + end_pose = list(pose) + + if x_distance is not None or y_distance is not None: + if x_distance is not None: + deep_pose = deepcopy(pose) + deep_pose[0] += float(x_distance) + elif y_distance is not None: + deep_pose = deepcopy(pose) + deep_pose[1] += float(y_distance) function_list = [ - lambda: self.moveit_task( - position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z], - quaternion=quaternion, - **config, - cartesian=self.cartesian_flag, + lambda p=pose.copy(), q=quaternion, cfg=config: self.moveit_task( + position=p, quaternion=q, **cfg, cartesian=self.cartesian_flag ) ] + function_list - - pose[2] += float(cmd_dict["lift_height"]) function_list.append( - lambda: self.moveit_task( - position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag + lambda dp=deep_pose.copy(), q=quaternion, cfg=config: self.moveit_task( + position=dp, quaternion=q, **cfg, cartesian=self.cartesian_flag ) ) - end_pose = pose + end_pose = list(deep_pose) - if "x_distance" in cmd_dict.keys() or "y_distance" in cmd_dict.keys(): - if "x_distance" in cmd_dict.keys(): - deep_pose = deepcopy(pose) - deep_pose[0] += float(cmd_dict["x_distance"]) - elif "y_distance" in cmd_dict.keys(): - deep_pose = deepcopy(pose) - deep_pose[1] += float(cmd_dict["y_distance"]) + retval_ik = None + attempts_ik = config.get("retry", 10) + while retval_ik is None and attempts_ik > 0: + retval_ik = self.moveit2[move_group].compute_ik( + position=end_pose, + quat_xyzw=quaternion, + constraints=Constraints(joint_constraints=joint_constraint_msgs), + ) + time.sleep(0.1) + attempts_ik -= 1 + if retval_ik is None: + raise ValueError("Failed to compute inverse kinematics") + position_ = [ + retval_ik.position[retval_ik.name.index(i)] for i in self.moveit2[move_group].joint_names + ] + jn = self.moveit2[move_group].joint_names + function_list = [ + lambda pos=position_, names=jn, cfg=config: self.moveit_joint_task( + joint_positions=pos, joint_names=names, **cfg + ) + ] + function_list + else: + function_list = [lambda cfg=config, jp=joint_positions_: self.moveit_joint_task(**cfg, joint_positions=jp)] + function_list - function_list = [ - lambda: self.moveit_task( - position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag - ) - ] + function_list - function_list.append( - lambda: self.moveit_task( - position=deep_pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag - ) - ) - end_pose = deep_pose - - retval_ik = None - retry = config.get("retry", 10) - while retval_ik is None and retry > 0: - retval_ik = self.moveit2[cmd_dict["move_group"]].compute_ik( - position=end_pose, quat_xyzw=quaternion, constraints=Constraints(joint_constraints=constraints) - ) - time.sleep(0.1) - retry -= 1 - if retval_ik is None: - result.success = False - return result - position_ = [ - retval_ik.position[retval_ik.name.index(i)] - for i in self.moveit2[cmd_dict["move_group"]].joint_names - ] - function_list = [ - lambda: self.moveit_joint_task( - joint_positions=position_, - joint_names=self.moveit2[cmd_dict["move_group"]].joint_names, - **config, - ) - ] + function_list + for i in range(len(function_list)): + if i == 0: + self.cartesian_flag = False else: - function_list = [ - lambda: self.moveit_joint_task(**config, joint_positions=joint_positions_) - ] + function_list + self.cartesian_flag = True - for i in range(len(function_list)): - if i == 0: - self.cartesian_flag = False - else: - self.cartesian_flag = True - - re = function_list[i]() - if not re: - print(i, re) - result.success = False - return result - result.success = True + re = function_list[i]() + if not re: + print(i, re) + raise ValueError(f"Failed to execute moveit task: {i}") except Exception as e: - print(e) self.cartesian_flag = False - result.success = False - - return result + raise e def set_status(self, command: str): """ diff --git a/unilabos/registry/devices/robot_arm.yaml b/unilabos/registry/devices/robot_arm.yaml index ff357ad4..a2d60573 100644 --- a/unilabos/registry/devices/robot_arm.yaml +++ b/unilabos/registry/devices/robot_arm.yaml @@ -173,48 +173,64 @@ robotic_arm.SCARA_with_slider.moveit.virtual: type: object type: UniLabJsonCommand pick_and_place: - feedback: - status: status - goal: - command: command + feedback: {} + goal: {} goal_default: - command: '' + constraints: null + lift_height: null + move_group: null + option: null + resource: null + retry: null + speed: null + status: null + target: null + x_distance: null + y_distance: null handles: {} placeholder_keys: {} - result: - return_info: return_info - success: success + result: {} schema: - description: '' + description: pick_and_place 显式参数(UniLabJsonCommand) properties: - feedback: - additionalProperties: false + feedback: {} + goal: properties: + constraints: + items: + type: number + type: array + lift_height: + type: string + move_group: + type: string + option: + type: string + resource: + type: string + retry: + type: string + speed: + type: string status: type: string - title: SendCmd_Feedback - type: object - goal: - additionalProperties: false - properties: - command: + target: type: string - title: SendCmd_Goal - type: object - result: - additionalProperties: false - properties: - return_info: + x_distance: type: string - success: - type: boolean - title: SendCmd_Result + y_distance: + type: string + required: + - option + - move_group + - status type: object + result: {} required: - goal - title: SendCmd + title: pick_and_place参数 type: object - type: SendCmd + type: UniLabJsonCommand set_position: feedback: status: status diff --git a/unilabos/registry/devices/robot_linear_motion.yaml b/unilabos/registry/devices/robot_linear_motion.yaml index 74b01e80..6a36dae0 100644 --- a/unilabos/registry/devices/robot_linear_motion.yaml +++ b/unilabos/registry/devices/robot_linear_motion.yaml @@ -684,48 +684,64 @@ linear_motion.toyo_xyz.sim: type: object type: UniLabJsonCommand pick_and_place: - feedback: - status: status - goal: - command: command + feedback: {} + goal: {} goal_default: - command: '' + constraints: null + lift_height: null + move_group: null + option: null + resource: null + retry: null + speed: null + status: null + target: null + x_distance: null + y_distance: null handles: {} placeholder_keys: {} - result: - return_info: return_info - success: success + result: {} schema: - description: '' + description: pick_and_place 显式参数(UniLabJsonCommand) properties: - feedback: - additionalProperties: false + feedback: {} + goal: properties: + constraints: + items: + type: number + type: array + lift_height: + type: string + move_group: + type: string + option: + type: string + resource: + type: string + retry: + type: string + speed: + type: string status: type: string - title: SendCmd_Feedback - type: object - goal: - additionalProperties: false - properties: - command: + target: type: string - title: SendCmd_Goal - type: object - result: - additionalProperties: false - properties: - return_info: + x_distance: type: string - success: - type: boolean - title: SendCmd_Result + y_distance: + type: string + required: + - option + - move_group + - status type: object + result: {} required: - goal - title: SendCmd + title: pick_and_place参数 type: object - type: SendCmd + type: UniLabJsonCommand set_position: feedback: status: status diff --git a/unilabos/ros/nodes/base_device_node.py b/unilabos/ros/nodes/base_device_node.py index e249bc0f..43b8ebfb 100644 --- a/unilabos/ros/nodes/base_device_node.py +++ b/unilabos/ros/nodes/base_device_node.py @@ -543,7 +543,12 @@ class BaseROS2DeviceNode(Node, Generic[T]): } ) tree_response: SerialCommand.Response = await client.call_async(request) - uuid_maps = json.loads(tree_response.response) + _raw_resp = tree_response.response if tree_response else "" + if _raw_resp: + uuid_maps = json.loads(_raw_resp) + else: + uuid_maps = {} + self._lab_logger.warning("Resource tree add 返回空响应,跳过 UUID 映射") self.resource_tracker.loop_update_uuid(input_resources, uuid_maps) self._lab_logger.info(f"Resource tree added. UUID mapping: {len(uuid_maps)} nodes") # 这里created_resources不包含parent_resource @@ -1729,10 +1734,25 @@ class BaseROS2DeviceNode(Node, Generic[T]): try: function_args[arg_name] = self._convert_resources_sync(resource_data["uuid"])[0] except Exception as e: - self.lab_logger().error( - f"转换ResourceSlot参数 {arg_name} 失败: {e}\n{traceback.format_exc()}" + # UUID 在资源树中不存在,尝试从传入的完整 dict 直接构建 PLR 资源 + self.lab_logger().warning( + f"UUID查询 {arg_name} 失败,尝试从传入数据直接构建: {e}" ) - raise JsonCommandInitError(f"ResourceSlot参数转换失败: {arg_name}") + try: + fallback_tree = ResourceTreeSet.from_raw_dict_list([resource_data]) + if len(fallback_tree.trees) == 0: + raise + plr_list = fallback_tree.to_plr_resources() + if not plr_list: + raise + plr_res = plr_list[0] + figured = self.resource_tracker.figure_resource(plr_res, try_mode=True) + function_args[arg_name] = figured[0] if figured else plr_res + except Exception: + self.lab_logger().error( + f"转换ResourceSlot参数 {arg_name} 失败(含回退): {e}\n{traceback.format_exc()}" + ) + raise JsonCommandInitError(f"ResourceSlot参数转换失败: {arg_name}") # 处理 ResourceSlot 列表 elif isinstance(arg_type, tuple) and len(arg_type) == 2: @@ -1744,10 +1764,25 @@ class BaseROS2DeviceNode(Node, Generic[T]): uuids = [r["uuid"] for r in resource_list if isinstance(r, dict) and "id" in r] function_args[arg_name] = self._convert_resources_sync(*uuids) if uuids else [] except Exception as e: - self.lab_logger().error( - f"转换ResourceSlot列表参数 {arg_name} 失败: {e}\n{traceback.format_exc()}" + self.lab_logger().warning( + f"UUID查询列表 {arg_name} 失败,尝试从传入数据直接构建: {e}" ) - raise JsonCommandInitError(f"ResourceSlot列表参数转换失败: {arg_name}") + try: + dict_items = [r for r in resource_list if isinstance(r, dict) and "id" in r] + fallback_tree = ResourceTreeSet.from_raw_dict_list(dict_items) + if len(fallback_tree.trees) == 0: + raise + plr_list = fallback_tree.to_plr_resources() + resolved = [] + for plr_res in plr_list: + figured = self.resource_tracker.figure_resource(plr_res, try_mode=True) + resolved.append(figured[0] if figured else plr_res) + function_args[arg_name] = resolved + except Exception: + self.lab_logger().error( + f"转换ResourceSlot列表参数 {arg_name} 失败(含回退): {e}\n{traceback.format_exc()}" + ) + raise JsonCommandInitError(f"ResourceSlot列表参数转换失败: {arg_name}") # todo: 默认反报送 return function(**function_args) diff --git a/unilabos/test/experiments/prcxi_9320_slim.json b/unilabos/test/experiments/prcxi_9320_slim.json index 196debf0..9af1c5bd 100644 --- a/unilabos/test/experiments/prcxi_9320_slim.json +++ b/unilabos/test/experiments/prcxi_9320_slim.json @@ -28,7 +28,7 @@ "matrix_id": "", "simulator": false, "channel_num": 2, - "step_mode": true, + "step_mode": false, "calibration_points": { "line_1": [[452.07,21.19], [313.88,21.19], [176.87,21.19], [39.08,21.19]], "line_2": [[451.37,116.68], [313.28,116.88], [176.58,116.69], [38.58,117.18]],