mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-17 21:11:12 +00:00
a52133b7d0
* 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>
69 lines
2.6 KiB
Python
69 lines
2.6 KiB
Python
from unilabos.utils.log import logger
|
||
|
||
|
||
class DeviceNodeResourceTracker:
|
||
|
||
def __init__(self):
|
||
self.resources = []
|
||
self.root_resource2resource = {}
|
||
pass
|
||
|
||
def root_resource(self, resource):
|
||
if id(resource) in self.root_resource2resource:
|
||
return self.root_resource2resource[id(resource)]
|
||
else:
|
||
return resource
|
||
|
||
def add_resource(self, resource):
|
||
# 使用内存地址跟踪是否为同一个resource
|
||
for r in self.resources:
|
||
if id(r) == id(resource):
|
||
return
|
||
# 添加资源到跟踪器
|
||
self.resources.append(resource)
|
||
|
||
def clear_resource(self):
|
||
self.resources = []
|
||
|
||
def figure_resource(self, resource):
|
||
# 使用内存地址跟踪是否为同一个resource
|
||
if isinstance(resource, list):
|
||
return [self.figure_resource(r) for r in resource]
|
||
res_id = resource.id if hasattr(resource, "id") else None
|
||
res_name = resource.name if hasattr(resource, "name") else None
|
||
res_identifier = res_id if res_id else res_name
|
||
identifier_key = "id" if res_id else "name"
|
||
resource_cls_type = type(resource)
|
||
if res_identifier is None:
|
||
logger.warning(f"resource {resource} 没有id或name，暂不能对应figure")
|
||
res_list = []
|
||
for r in self.resources:
|
||
res_list.extend(
|
||
self.loop_find_resource(r, resource_cls_type, identifier_key, getattr(resource, identifier_key))
|
||
)
|
||
assert len(res_list) == 1, f"找到多个资源，请检查资源是否唯一: {res_list}"
|
||
self.root_resource2resource[id(resource)] = res_list[0]
|
||
# 后续加入其他对比方式
|
||
return res_list[0]
|
||
|
||
def loop_find_resource(self, resource, resource_cls_type, identifier_key, compare_value):
|
||
res_list = []
|
||
print(resource, resource_cls_type, identifier_key, compare_value)
|
||
children = getattr(resource, "children", [])
|
||
for child in children:
|
||
res_list.extend(self.loop_find_resource(child, resource_cls_type, identifier_key, compare_value))
|
||
if resource_cls_type == type(resource):
|
||
if hasattr(resource, identifier_key):
|
||
if getattr(resource, identifier_key) == compare_value:
|
||
res_list.append(resource)
|
||
return res_list
|
||
|
||
def filter_find_list(self, res_list, compare_std_dict):
|
||
new_list = []
|
||
for res in res_list:
|
||
for k, v in compare_std_dict.items():
|
||
if hasattr(res, k):
|
||
if getattr(res, k) == v:
|
||
new_list.append(res)
|
||
return new_list
|