open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2026-02-07 23:45:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

Device visualization (#39)

Browse Source 
* Update README and MQTTClient for installation instructions and code improvements

* feat: 支持local_config启动
add: 增加对crt path的说明，为传入config.py的相对路径
move: web component

* add: registry description

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* feat: node_info_update srv
fix: OTDeck cant create

* close #12
feat: slave node registry

* feat: show machine name
fix: host node registry not uploaded

* feat: add hplc registry

* feat: add hplc registry

* fix: hplc status typo

* fix: devices/

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* fix: device.class possible null

* fix: HPLC additions with online service

* fix: slave mode spin not working

* fix: slave mode spin not working

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* feat: 多ProtocolNode 允许子设备ID相同
feat: 上报发现的ActionClient
feat: Host重启动，通过discover机制要求slaveNode重新注册，实现信息及时上报

* feat: 支持env设置config

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

* Device visualization (#14)

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>

* fix: missing hostname in devices_names
fix: upload_file for model file

* fix: missing paho-mqtt package
bump version to 0.9.0

* fix startup
add ResourceCreateFromOuter.action

* fix type hint

* update actions

* update actions

* host node add_resource_from_outer
fix cmake list

* pass device config to device class

* add: bind_parent_ids to resource create action
fix: message convert string

* fix: host node should not be re_discovered

* feat: resource tracker support dict

* feat: add more necessary params

* feat: fix boolean null in registry action data

* feat: add outer resource

* 编写mesh添加action

* feat: append resource

* add action

* feat: vis 2d for plr

* fix

* fix: browser on rviz

* fix: cloud bridge error fallback to local

* fix: salve auto run rviz

* 初始化两个plate

* Device visualization (#22)

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

* 编写mesh添加action

* add action

* fix

* fix: browser on rviz

* fix: cloud bridge error fallback to local

* fix: salve auto run rviz

* 初始化两个plate

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>

* fix: multi channel

* fix: aspirate

* fix: aspirate

* fix: aspirate

* fix: aspirate

* 提交

* fix: jobadd

* fix: jobadd

* fix: msg converter

* tijiao

* add resource creat easy action

* identify debug msg

* mq client id

* 提取lh的joint发布

* unify liquid_handler definition

* 修改物料跟随与物料添加逻辑

修改物料跟随与物料添加逻辑
将joint_publisher类移出lh的backends，但仍需要对lh的backends进行一些改写

* Revert "修改物料跟随与物料添加逻辑"

This reverts commit 498c997ad7.

* Reapply "修改物料跟随与物料添加逻辑"

This reverts commit 3a60d2ae81.

* Revert "Merge remote-tracking branch 'upstream/dev' into device_visualization"

This reverts commit fa727220af, reversing
changes made to 498c997ad7.

* 修改物料放下时的方法，如果选择

修改物料放下时的方法，
如果选择drop_trash，则删除物料显示
如果选择drop，则让其解除连接

* add biomek.py demo implementation

* 更新LiquidHandlerBiomek类，添加资源创建功能，优化协议创建方法，修复部分代码格式问题，更新YAML配置以支持新功能。

* Test

* fix biomek success type

* Convert LH action to biomek.

* Update biomek.py

* 注册表上报handle和schema (param input)

* 修复biomek缺少的字段

* delete 's'

* Remove warnings

* Update biomek.py

* Biomek test

* Update biomek.py

* 新增transfer_biomek的msg

* New transfer_biomek

* Updated transfer_biomek

* 更新transfer_biomek的msg

* 更新transfer_biomek的msg

* 支持Biomek创建

* new action

* fix key name typo

* New parameter for biomek to run.

* Refine

* Update

* new actions

* new actions

* 1

* registry

* fix biomek startup
add action handles

* fix handles not as default entry

* unilab添加moveit启动

1,整合所有moveit节点到一个move_group中，并整合所有的controller依次激活
2,添加pymoveit2的节点，使用json可直接启动
3,修改机械臂规划方式，添加约束，让冗余关节不会进行过多移动

* biomek_test.py

biomek_test.py是最新的版本，运行它会生成complete_biomek_protocol.json

* Update biomek.py

* biomek_test.py

* fix liquid_handler.biomek handles

* 修改物体attach时，多次赋值当前时间导致卡顿问题，

* Revert "修改物体attach时，多次赋值当前时间导致卡顿问题，"

This reverts commit 56d45b94f5.

* Reapply "修改物体attach时，多次赋值当前时间导致卡顿问题，"

This reverts commit 07d9db20c3.

* 添加缺少物料："plate_well_G12",

* host node新增resource add时间统计
create_resource新增handle
bump version to 0.9.2

* 修正物料上传时间
改用biomek_test
增加ResultInfoEncoder
支持返回结果上传

* 正确发送return_info结果

* 同步执行状态信息

* 取消raiseValueError提示

* Update biomek_test.py

* 0608 DONE

* 同步了Biomek.py 现在应可用

* biomek switch back to non-test

* temp disable initialize resource

* add

* fix tip resource data

* liquid states

* change to debug level

* Revert "change to debug level"

This reverts commit 5d9953c3e5.

* Reapply "change to debug level"

This reverts commit 2487bb6ffc.

* fix tip resource data

* add full device

* add moveit yaml

* 修复moveit
增加post_init阶段，给予ros_node反向

* remove necessary node

* fix moveit action client

* remove necessary imports

* Update moveit_interface.py

* fix handler_key uppercase

* json add liquids

* fix setup

* add

* change to "sources" and "targets" for lh

* bump version

* remove parent's parent link

---------

Co-authored-by: Harvey Que <Q-Query@outlook.com>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>
Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: Junhan Chang <changjh@pku.edu.cn>
Co-authored-by: Guangxin Zhang <guangxin.zhang.bio@gmail.com>
Co-authored-by: qxw138 <qxw@stu.pku.edu.cn>
This commit is contained in:
q434343
2025-06-09 17:06:04 +08:00
committed by GitHub
parent 729a0fcf0c
commit 133ffaac17
104 changed files with 20893 additions and 2712 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
unilabos/ros/main_slave_run.py
View File

@@ -6,9 +6,9 @@ import time
from typing import Optional, Dict, Any, List
import rclpy
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
from unilabos.devices.ros_dev.liquid_handler_joint_publisher import LiquidHandlerJointPublisher
from unilabos_msgs.msg import Resource # type: ignore
from unilabos_msgs.srv import ResourceAdd, SerialCommand # type: ignore
from rclpy.executors import MultiThreadedExecutor
@@ -69,19 +69,23 @@ def main(
)
if visual != "disable":
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
resource_mesh_manager = ResourceMeshManager(
resources_mesh_config,
resources_config,
resource_tracker= DeviceNodeResourceTracker(),
resource_tracker = host_node.resource_tracker,
device_id = 'resource_mesh_manager',
)
joint_republisher = JointRepublisher(
'joint_republisher',
DeviceNodeResourceTracker()
host_node.resource_tracker
)
lh_joint_pub = LiquidHandlerJointPublisher(resources_config=resources_config,
resource_tracker=host_node.resource_tracker)
executor.add_node(resource_mesh_manager)
executor.add_node(joint_republisher)
executor.add_node(lh_joint_pub)
thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread")
thread.start()
@@ -121,6 +125,7 @@ def slave(
executor.add_node(n)
if visual != "disable":
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
resource_mesh_manager = ResourceMeshManager(
resources_mesh_config,
resources_config,

4
unilabos/ros/msgs/message_converter.py
View File

@@ -131,7 +131,7 @@ _msg_converter: Dict[Type, Any] = {
Bool: lambda x: Bool(data=bool(x)),
str: str,
String: lambda x: String(data=str(x)),
Point: lambda x: Point(x=x.x, y=x.y, z=x.z) if not isinstance(x, dict) else Point(x=x.get("x", 0), y=x.get("y", 0), z=x.get("z", 0)),
Point: lambda x: Point(x=x.x, y=x.y, z=x.z) if not isinstance(x, dict) else Point(x=x.get("x", 0.0), y=x.get("y", 0.0), z=x.get("z", 0.0)),
Resource: lambda x: Resource(
id=x.get("id", ""),
name=x.get("name", ""),
@@ -141,7 +141,7 @@ _msg_converter: Dict[Type, Any] = {
type=x.get("type", ""),
category=x.get("class", "") or x.get("type", ""),
pose=(
Pose(position=Point(x=float(x.get("position", {}).get("x", 0)), y=float(x.get("position", {}).get("y", 0)), z=float(x.get("position", {}).get("z", 0))))
Pose(position=Point(x=float(x.get("position", {}).get("x", 0.0)), y=float(x.get("position", {}).get("y", 0.0)), z=float(x.get("position", {}).get("z", 0.0))))
if x.get("position", None) is not None
else Pose()
),

23
unilabos/ros/nodes/base_device_node.py
View File

@@ -740,16 +740,23 @@ class BaseROS2DeviceNode(Node, Generic[T]):
self.lab_logger().info(f"更新资源状态: {k}")
r = ResourceUpdate.Request()
# 仅当action_kwargs[k]不为None时尝试转换
akv = action_kwargs[k]
akv = action_kwargs[k] # 已经是完成转换的物料了，只需要转换成ros msg Resource了
apv = action_paramtypes[k]
final_type = get_type_class(apv)
if final_type is None:
continue
try:
r.resources = [
convert_to_ros_msg(Resource, self.resource_tracker.root_resource(rs))
for rs in convert_resources_from_type(akv, final_type) # type: ignore # FIXME # 考虑反查到最大的
]
seen = set()
unique_resources = []
for rs in akv:
res = self.resource_tracker.parent_resource(rs) # 获取 resource 对象
if id(res) not in seen:
seen.add(id(res))
converted_list = convert_resources_from_type([res], final_type)
unique_resources.extend([convert_to_ros_msg(Resource, converted) for converted in converted_list])
r.resources = unique_resources
response = await self._resource_clients["resource_update"].call_async(r)
self.lab_logger().debug(f"资源更新结果: {response}")
except Exception as e:
@@ -917,6 +924,12 @@ class ROS2DeviceNode:
)
self._ros_node: BaseROS2DeviceNode
self._ros_node.lab_logger().info(f"初始化完成 {self._ros_node.uuid} {self.driver_is_ros}")
self.driver_instance._ros_node = self._ros_node # type: ignore
if hasattr(self.driver_instance, "post_init"):
try:
self.driver_instance.post_init(self._ros_node) # type: ignore
except Exception as e:
self._ros_node.lab_logger().error(f"设备后初始化失败: {e}")
def _start_loop(self):
def run_event_loop():

128
unilabos/ros/nodes/presets/resource_mesh_manager.py
View File

@@ -91,8 +91,11 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.__collision_object_publisher = self.create_publisher(
CollisionObject, "/collision_object", 10
)
self.__planning_scene_publisher = self.create_publisher(
PlanningScene, "/planning_scene", 10
)
self.__attached_collision_object_publisher = self.create_publisher(
AttachedCollisionObject, "/attached_collision_object", 10
AttachedCollisionObject, "/attached_collision_object", 0
)
# 创建一个Action Server用于修改resource_tf_dict
@@ -121,7 +124,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"""检查move_group节点是否已初始化完成"""
# 获取当前可用的节点列表
if len(self.resource_tf_dict) == 0:
return
tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2))
# if tf_ready:
@@ -129,8 +133,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.move_group_ready = True
self.publish_resource_tf()
self.add_resource_collision_meshes(self.resource_tf_dict)
# time.sleep(1)
def add_resource_mesh_callback(self, goal_handle : ServerGoalHandle):
tf_update_msg = goal_handle.request
@@ -147,7 +150,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"""刷新资源配置"""
registry = lab_registry
resource_config = json.loads(resource_config_str)
resource_config = json.loads(resource_config_str.replace("'",'"'))
if resource_config['id'] in self.resource_config_dict:
self.get_logger().info(f'资源 {resource_config["id"]} 已存在')
@@ -158,7 +161,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.resource_model[resource_config['id']] = {
'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
'mesh_tf': model_config['mesh_tf']}
if model_config['children_mesh'] is not None:
if 'children_mesh' in model_config.keys():
self.resource_model[f"{resource_config['id']}_"] = {
'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
'mesh_tf': model_config['children_mesh_tf']
@@ -187,7 +190,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
pass
elif parent is not None and resource_id in self.resource_model:
parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None","")
# parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None_","")
parent_link = f"{parent}_device_link".replace("None_","")
else:
@@ -297,7 +301,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"world",
resource_id,
rclpy.time.Time(seconds=0),
rclpy.duration.Duration(seconds=5)
# rclpy.duration.Duration(seconds=5)
)
# 提取当前位姿信息
@@ -344,9 +348,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.resource_pose_publisher.publish(changed_poses_msg)
self.zero_count += 1
def _is_pose_equal(self, pose1, pose2, tolerance=1e-7):
"""
比较两个位姿是否相等（考虑浮点数精度）
@@ -386,14 +388,24 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.__planning_scene = self._get_planning_scene_service.call(
GetPlanningScene.Request()
).scene
for resource_id, target_parent in cmd_dict.items():
self.__planning_scene.is_diff = True
planning_scene = PlanningScene()
planning_scene.is_diff = True
planning_scene.robot_state.is_diff = True
# time_start = self.get_clock().now()
time_start = rclpy.time.Time(seconds=0)
count = 0
for resource_id, target_parent in cmd_dict.items():
parent_id = target_parent
if target_parent == '__trash':
parent_id = 'world'
# 获取从resource_id到target_parent的转换
transform = self.tf_buffer.lookup_transform(
target_parent,
parent_id,
resource_id,
rclpy.time.Time(seconds=0)
time_start,
timeout=rclpy.duration.Duration(seconds=10)
)
# 提取转换中的位置和旋转信息
@@ -411,26 +423,62 @@ class ResourceMeshManager(BaseROS2DeviceNode):
}
self.resource_tf_dict[resource_id] = {
"parent": target_parent,
"parent": parent_id,
"position": position,
"rotation": rotation
}
# self.attach_collision_object(id=resource_id,link_name=target_parent)
collision_object = AttachedCollisionObject(
# time.sleep(0.02)
operation_attach = CollisionObject.ADD
operation_world = CollisionObject.REMOVE
if target_parent == 'world':
operation_attach = CollisionObject.REMOVE
operation_world = CollisionObject.ADD
elif target_parent == '__trash':
operation_attach = CollisionObject.REMOVE
world_object = CollisionObject(
id=resource_id,
link_name=target_parent,
operation=operation_world
)
if target_parent != '__trash':
planning_scene.world.collision_objects.append(world_object)
collision_object = AttachedCollisionObject(
object=CollisionObject(
id=resource_id,
operation=CollisionObject.ADD
operation=operation_attach
)
)
self.__planning_scene.robot_state.attached_collision_objects.append(collision_object)
if target_parent != 'world' and target_parent != '__trash':
collision_object.link_name = target_parent
planning_scene.robot_state.attached_collision_objects.append(collision_object)
count += 1
if count > 30:
req = ApplyPlanningScene.Request()
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self.__planning_scene_publisher.publish(planning_scene)
count = 0
planning_scene = PlanningScene()
planning_scene.is_diff = True
planning_scene.robot_state.is_diff = True
req = ApplyPlanningScene.Request()
req.scene = self.__planning_scene
self._apply_planning_scene_service.call_async(req)
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self.__planning_scene_publisher.publish(planning_scene)
# self.__collision_object_publisher.publish(CollisionObject())
except Exception as e:
self.get_logger().error(f"更新资源TF字典失败: {e}")
@@ -440,18 +488,22 @@ class ResourceMeshManager(BaseROS2DeviceNode):
return SendCmd.Result(success=True)
def add_resource_collision_meshes(self,resource_tf_dict:dict):
"""
遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径，
如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。
"""
self.get_logger().info('开始添加资源碰撞网格')
self.__planning_scene = self._get_planning_scene_service.call(
GetPlanningScene.Request()
).scene
planning_scene = PlanningScene()
planning_scene.is_diff = True
count = 0
for resource_id, tf_info in resource_tf_dict.items():
if resource_id in self.resource_model:
@@ -479,7 +531,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
quat_xyzw=q,
frame_id=resource_id
)
self.__planning_scene.world.collision_objects.append(collision_object)
count += 1
planning_scene.world.collision_objects.append(collision_object)
elif f"{tf_info['parent']}_" in self.resource_model:
# 获取资源的父级框架ID
id_ = f"{tf_info['parent']}_"
@@ -507,14 +560,26 @@ class ResourceMeshManager(BaseROS2DeviceNode):
quat_xyzw=q,
frame_id=resource_id
)
count += 1
planning_scene.world.collision_objects.append(collision_object)
self.__planning_scene.world.collision_objects.append(collision_object)
if count > 30:
req = ApplyPlanningScene.Request()
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self.__planning_scene_publisher.publish(planning_scene)
count = 0
planning_scene = PlanningScene()
planning_scene.is_diff = True
req = ApplyPlanningScene.Request()
req.scene = self.__planning_scene
self._apply_planning_scene_service.call_async(req)
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self.__planning_scene_publisher.publish(planning_scene)
self.get_logger().info('资源碰撞网格添加完成')
@@ -959,9 +1024,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
Attach collision object to the robot.
"""
if link_name is None:
link_name = self.__end_effector_name
msg = AttachedCollisionObject(
object=CollisionObject(id=id, operation=CollisionObject.ADD)
)

21
unilabos/ros/nodes/resource_tracker.py
View File

@@ -1,3 +1,5 @@
from typing import List, Tuple, Any
from unilabos.utils.log import logger
@@ -5,12 +7,12 @@ class DeviceNodeResourceTracker(object):
def __init__(self):
self.resources = []
self.root_resource2resource = {}
self.resource2parent_resource = {}
pass
def root_resource(self, resource):
if id(resource) in self.root_resource2resource:
return self.root_resource2resource[id(resource)]
def parent_resource(self, resource):
if id(resource) in self.resource2parent_resource:
return self.resource2parent_resource[id(resource)]
else:
return resource
@@ -44,20 +46,21 @@ class DeviceNodeResourceTracker(object):
self.loop_find_resource(r, resource_cls_type, identifier_key, getattr(query_resource, identifier_key))
)
assert len(res_list) == 1, f"{query_resource} 找到多个资源，请检查资源是否唯一: {res_list}"
self.root_resource2resource[id(query_resource)] = res_list[0]
self.resource2parent_resource[id(query_resource)] = res_list[0][0]
self.resource2parent_resource[id(res_list[0][1])] = res_list[0][0]
# 后续加入其他对比方式
return res_list[0]
return res_list[0][1]
def loop_find_resource(self, resource, target_resource_cls_type, identifier_key, compare_value):
def loop_find_resource(self, resource, target_resource_cls_type, identifier_key, compare_value, parent_res=None) -> List[Tuple[Any, Any]]:
res_list = []
# print(resource, target_resource_cls_type, identifier_key, compare_value)
children = getattr(resource, "children", [])
for child in children:
res_list.extend(self.loop_find_resource(child, target_resource_cls_type, identifier_key, compare_value))
res_list.extend(self.loop_find_resource(child, target_resource_cls_type, identifier_key, compare_value, resource))
if target_resource_cls_type == type(resource) or target_resource_cls_type == dict:
if hasattr(resource, identifier_key):
if getattr(resource, identifier_key) == compare_value:
res_list.append(resource)
res_list.append((parent_res, resource))
return res_list
def filter_find_list(self, res_list, compare_std_dict):

50
unilabos/ros/utils/driver_creator.py
View File

@@ -105,40 +105,41 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
return nested_dict_to_list(resource), Resource
return resource, source_type
def _process_resource_references(self, data: Any, to_dict=False) -> Any:
def _process_resource_references(self, data: Any, to_dict=False, states=None, prefix_path="") -> Any:
"""
递归处理资源引用，替换_resource_child_name对应的资源
Args:
data: 需要处理的数据，可能是字典、列表或其他类型
to_dict: 转换成对应的实例，还是转换成对应的字典
to_dict: 是否返回字典形式的资源
states: 用于保存所有资源状态
prefix_path: 当前递归路径
Returns:
处理后的数据
"""
from pylabrobot.resources import Deck, Resource
if states is None:
states = {}
if isinstance(data, dict):
# 检查是否包含资源引用
if "_resource_child_name" in data:
child_name = data["_resource_child_name"]
if child_name in self.children:
# 找到了对应的资源
resource = self.children[child_name]
# 检查是否需要转换资源类型
if "_resource_type" in data:
type_path = data["_resource_type"]
try:
# 尝试导入指定的类型
target_type = import_manager.get_class(type_path)
contain_model = not issubclass(target_type, Deck)
resource, target_type = self._process_resource_mapping(resource, target_type)
# 在截图中格式，是deserialize，所以这里要转成plr resource可deserialize的字典
# 这样后面执行deserialize的时候能够正确反序列化对应的物料
resource_instance: Resource = resource_ulab_to_plr(resource, contain_model)
# 使用 prefix_path 作为 key 存储资源状态
if to_dict:
return resource_instance.serialize()
serialized = resource_instance.serialize()
states[prefix_path] = resource_instance.serialize_all_state()
return serialized
else:
self.resource_tracker.add_resource(resource_instance)
return resource_instance
@@ -151,18 +152,21 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
else:
logger.warning(f"找不到资源引用 '{child_name}'，保持原值不变")
# 递归处理字典的每个值
# 递归处理每个键值
result = {}
for key, value in data.items():
result[key] = self._process_resource_references(value, to_dict)
new_prefix = f"{prefix_path}.{key}" if prefix_path else key
result[key] = self._process_resource_references(value, to_dict, states, new_prefix)
return result
# 处理列表类型
elif isinstance(data, list):
return [self._process_resource_references(item, to_dict) for item in data]
return [
self._process_resource_references(item, to_dict, states, f"{prefix_path}[{i}]")
for i, item in enumerate(data)
]
# 其他类型直接返回
return data
else:
return data
def create_instance(self, data: Dict[str, Any]) -> Optional[T]:
"""
@@ -187,10 +191,18 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
logger.debug(f"自动补充 _resource_type: {data[param_name]['_resource_type']}")
# 首先处理资源引用
processed_data = self._process_resource_references(data, to_dict=True)
states = {}
processed_data = self._process_resource_references(data, to_dict=True, states=states)
try:
self.device_instance = deserialize_method(**processed_data)
all_states = self.device_instance.serialize_all_state()
for k, v in states.items():
logger.debug(f"PyLabRobot反序列化设置状态：{k}")
for kk, vv in all_states.items():
if kk not in v:
v[kk] = vv
self.device_instance.deck.load_all_state(v)
self.resource_tracker.add_resource(self.device_instance)
self.post_create()
return self.device_instance # type: ignore
@@ -225,6 +237,10 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
if hasattr(self.device_instance, "setup") and asyncio.iscoroutinefunction(getattr(self.device_instance, "setup")):
from unilabos.ros.nodes.base_device_node import ROS2DeviceNode
def done_cb(*args):
from pylabrobot.resources import set_volume_tracking
# from pylabrobot.resources import set_tip_tracking
set_volume_tracking(enabled=True)
# set_tip_tracking(enabled=True) # 序列化tip_spot has为False
logger.debug(f"PyLabRobot设备实例 {self.device_instance} 设置完成")
from unilabos.config.config import BasicConfig
if BasicConfig.vis_2d_enable: