Device Visualization & Mock Devices (#44) [37-biomek-i5i7 (#40), Device visualization (#39), Add Mock Device for Organic Synthesis\添加有机合成的虚拟仪器和Protocol (#43)]

* 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 * biomek_test.py biomek_test.py是最新的版本，运行它会生成complete_biomek_protocol.json * Update biomek.py * biomek_test.py * fix liquid_handler.biomek handles * 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 * 37-biomek-i5i7 (#40) * 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 * biomek_test.py biomek_test.py是最新的版本，运行它会生成complete_biomek_protocol.json * Update biomek.py * biomek_test.py * fix liquid_handler.biomek handles * 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 * Refine biomek * Refine copy issue * Refine --------- 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> * Device visualization (#39) * 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> * Device visualization (#41) * 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 * change arm's name * change name --------- Co-authored-by: Harvey Que <Q-Query@outlook.com> Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: q434343 <73513873+q434343@users.noreply.github.com> 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> * fix move it * fix move it * create_resource * bump ver modify slot type * 增加modbus支持调整protocol node以更好支持多种类型的read和write * 调整protocol node以更好支持多种类型的read和write * 补充日志 * Device visualization (#42) * 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，则让其解除连接 * unilab添加moveit启动 1,整合所有moveit节点到一个move_group中，并整合所有的controller依次激活 2,添加pymoveit2的节点，使用json可直接启动 3,修改机械臂规划方式，添加约束，让冗余关节不会进行过多移动 * 修改物体attach时，多次赋值当前时间导致卡顿问题， * Revert "修改物体attach时，多次赋值当前时间导致卡顿问题，" This reverts commit 56d45b94f5. * Reapply "修改物体attach时，多次赋值当前时间导致卡顿问题，" This reverts commit 07d9db20c3. * 添加缺少物料："plate_well_G12", * 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 * change arm's name * change name * fix ik error --------- Co-authored-by: Harvey Que <Q-Query@outlook.com> Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: q434343 <73513873+q434343@users.noreply.github.com> Co-authored-by: Junhan Chang <changjh@pku.edu.cn> * Add Mock Device for Organic Synthesis\添加有机合成的虚拟仪器和Protocol (#43) * Add Device MockChiller Add device MockChiller * Add Device MockFilter * Add Device MockPump * Add Device MockRotavap * Add Device MockSeparator * Add Device MockStirrer * Add Device MockHeater * Add Device MockVacuum * Add Device MockSolenoidValve * Add Device Mock \_init_.py * 规范模拟设备代码与注册表信息 * 更改Mock大写文件夹名 * 删除大写目录 * Edited Mock device json * Match mock device with action * Edit mock device yaml * Add new action * Add Virtual Device, Action, YAML, Protocol for Organic Syn * 单独分类测试的protocol文件夹 * 更名Action --------- Co-authored-by: Xuwznln <18435084+Xuwznln@users.noreply.github.com> --------- 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> Co-authored-by: q434343 <73513873+q434343@users.noreply.github.com> Co-authored-by: Harvey Que <Q-Query@outlook.com> Co-authored-by: Kongchang Feng <2100011801@stu.pku.edu.cn>
2025-12-17 04:51:10 +00:00 · 2025-06-12 21:01:04 +08:00
parent 3470a1cb69
commit aa1c67de29
264 changed files with 48914 additions and 2955 deletions
--- a/unilabos/ros/nodes/base_device_node.py
+++ b/unilabos/ros/nodes/base_device_node.py
@@ -1,5 +1,4 @@
 import copy
-import functools
 import json
 import threading
 import time
@@ -20,16 +19,29 @@ from rclpy.service import Service
 from unilabos_msgs.action import SendCmd
 from unilabos_msgs.srv._serial_command import SerialCommand_Request, SerialCommand_Response

-from unilabos.resources.graphio import convert_resources_to_type, convert_resources_from_type, resource_ulab_to_plr, \
-    initialize_resources, list_to_nested_dict, dict_to_tree, resource_plr_to_ulab, tree_to_list
+from unilabos.resources.graphio import (
+    convert_resources_to_type,
+    convert_resources_from_type,
+    resource_ulab_to_plr,
+    initialize_resources,
+    dict_to_tree,
+    resource_plr_to_ulab,
+    tree_to_list,
+)
 from unilabos.ros.msgs.message_converter import (
    convert_to_ros_msg,
    convert_from_ros_msg,
    convert_from_ros_msg_with_mapping,
-    convert_to_ros_msg_with_mapping, ros_action_to_json_schema,
+    convert_to_ros_msg_with_mapping,
 )
-from unilabos_msgs.srv import ResourceAdd, ResourceGet, ResourceDelete, ResourceUpdate, ResourceList, \
-    SerialCommand  # type: ignore
+from unilabos_msgs.srv import (
+    ResourceAdd,
+    ResourceGet,
+    ResourceDelete,
+    ResourceUpdate,
+    ResourceList,
+    SerialCommand,
+)  # type: ignore
 from unilabos_msgs.msg import Resource  # type: ignore

 from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
@@ -37,7 +49,7 @@ from unilabos.ros.x.rclpyx import get_event_loop
 from unilabos.ros.utils.driver_creator import ProtocolNodeCreator, PyLabRobotCreator, DeviceClassCreator
 from unilabos.utils.async_util import run_async_func
 from unilabos.utils.log import info, debug, warning, error, critical, logger
-from unilabos.utils.type_check import get_type_class, TypeEncoder
+from unilabos.utils.type_check import get_type_class, TypeEncoder, serialize_result_info

 T = TypeVar("T")

@@ -292,7 +304,9 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                self.create_ros_action_server(action_name, action_value_mapping)

        # 创建线程池执行器
-        self._executor = ThreadPoolExecutor(max_workers=max(len(action_value_mappings), 1), thread_name_prefix=f"ROSDevice{self.device_id}")
+        self._executor = ThreadPoolExecutor(
+            max_workers=max(len(action_value_mappings), 1), thread_name_prefix=f"ROSDevice{self.device_id}"
+        )

        # 创建资源管理客户端
        self._resource_clients: Dict[str, Client] = {
@@ -329,12 +343,14 @@ class BaseROS2DeviceNode(Node, Generic[T]):
            ADD_LIQUID_TYPE = other_calling_param.pop("ADD_LIQUID_TYPE", [])
            LIQUID_VOLUME = other_calling_param.pop("LIQUID_VOLUME", [])
            LIQUID_INPUT_SLOT = other_calling_param.pop("LIQUID_INPUT_SLOT", [])
-            slot = other_calling_param.pop("slot", -1)
-            if slot >= 0:  # slot为负数的时候采用assign方法
+            slot = other_calling_param.pop("slot", "-1")
+            if slot != "-1":  # slot为负数的时候采用assign方法
                other_calling_param["slot"] = slot
            # 本地拿到这个物料，可能需要先做初始化?
            if isinstance(resources, list):
-                if len(resources) == 1 and isinstance(resources[0], list) and not initialize_full:  # 取消，不存在的情况
+                if (
+                    len(resources) == 1 and isinstance(resources[0], list) and not initialize_full
+                ):  # 取消，不存在的情况
                    # 预先initialize过，以整组的形式传入
                    request.resources = [convert_to_ros_msg(Resource, resource_) for resource_ in resources[0]]
                elif initialize_full:
@@ -349,6 +365,25 @@ class BaseROS2DeviceNode(Node, Generic[T]):
            response = rclient.call(request)
            # 应该先add_resource了
            res.response = "OK"
+            # 如果driver自己就有assign的方法，那就使用driver自己的assign方法
+            if hasattr(self.driver_instance, "create_resource"):
+                create_resource_func = getattr(self.driver_instance, "create_resource")
+                try:
+                    ret = create_resource_func(
+                        resource_tracker=self.resource_tracker,
+                        resources=request.resources,
+                        bind_parent_id=bind_parent_id,
+                        bind_location=location,
+                        liquid_input_slot=LIQUID_INPUT_SLOT,
+                        liquid_type=ADD_LIQUID_TYPE,
+                        liquid_volume=LIQUID_VOLUME,
+                        slot_on_deck=slot,
+                    )
+                    res.response = serialize_result_info("", True, ret)
+                except Exception as e:
+                    traceback.print_exc()
+                    res.response = serialize_result_info(traceback.format_exc(), False, {})
+                return res
            # 接下来该根据bind_parent_id进行assign了，目前只有plr可以进行assign，不然没有办法输入到物料系统中
            resource = self.resource_tracker.figure_resource({"name": bind_parent_id})
            # request.resources = [convert_to_ros_msg(Resource, resources)]
@@ -359,6 +394,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                from pylabrobot.resources import Coordinate
                from pylabrobot.resources import OTDeck
                from pylabrobot.resources import Plate
+
                contain_model = not isinstance(resource, Deck)
                if isinstance(resource, ResourcePLR):
                    # resources.list()
@@ -366,25 +402,39 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                    plr_instance = resource_ulab_to_plr(resources_tree[0], contain_model)
                    if isinstance(plr_instance, Plate):
                        empty_liquid_info_in = [(None, 0)] * plr_instance.num_items
-                        for liquid_type, liquid_volume, liquid_input_slot in zip(ADD_LIQUID_TYPE, LIQUID_VOLUME, LIQUID_INPUT_SLOT):
+                        for liquid_type, liquid_volume, liquid_input_slot in zip(
+                            ADD_LIQUID_TYPE, LIQUID_VOLUME, LIQUID_INPUT_SLOT
+                        ):
                            empty_liquid_info_in[liquid_input_slot] = (liquid_type, liquid_volume)
                        plr_instance.set_well_liquids(empty_liquid_info_in)
                    if isinstance(resource, OTDeck) and "slot" in other_calling_param:
+                        other_calling_param["slot"] = int(other_calling_param["slot"])
                        resource.assign_child_at_slot(plr_instance, **other_calling_param)
                    else:
-                        _discard_slot = other_calling_param.pop("slot", -1)
-                        resource.assign_child_resource(plr_instance, Coordinate(location["x"], location["y"], location["z"]), **other_calling_param)
-                    request2.resources = [convert_to_ros_msg(Resource, r) for r in tree_to_list([resource_plr_to_ulab(resource)])]
+                        _discard_slot = other_calling_param.pop("slot", "-1")
+                        resource.assign_child_resource(
+                            plr_instance,
+                            Coordinate(location["x"], location["y"], location["z"]),
+                            **other_calling_param,
+                        )
+                    request2.resources = [
+                        convert_to_ros_msg(Resource, r) for r in tree_to_list([resource_plr_to_ulab(resource)])
+                    ]
                    rclient2.call(request2)
                # 发送给ResourceMeshManager
                action_client = ActionClient(
-                    self, SendCmd, "/devices/resource_mesh_manager/add_resource_mesh", callback_group=self.callback_group
+                    self,
+                    SendCmd,
+                    "/devices/resource_mesh_manager/add_resource_mesh",
+                    callback_group=self.callback_group,
                )
                goal = SendCmd.Goal()
-                goal.command = json.dumps({
-                    "resources": resources,
-                    "bind_parent_id": bind_parent_id,
-                })
+                goal.command = json.dumps(
+                    {
+                        "resources": resources,
+                        "bind_parent_id": bind_parent_id,
+                    }
+                )
                future = action_client.send_goal_async(goal, goal_uuid=uuid.uuid4())

                def done_cb(*args):
@@ -401,10 +451,16 @@ class BaseROS2DeviceNode(Node, Generic[T]):
        # noinspection PyTypeChecker
        self._service_server: Dict[str, Service] = {
            "query_host_name": self.create_service(
-                SerialCommand, f"/srv{self.namespace}/query_host_name", query_host_name_cb, callback_group=self.callback_group
+                SerialCommand,
+                f"/srv{self.namespace}/query_host_name",
+                query_host_name_cb,
+                callback_group=self.callback_group,
            ),
            "append_resource": self.create_service(
-                SerialCommand, f"/srv{self.namespace}/append_resource", append_resource, callback_group=self.callback_group
+                SerialCommand,
+                f"/srv{self.namespace}/append_resource",
+                append_resource,
+                callback_group=self.callback_group,
            ),
        }

@@ -433,6 +489,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
        registered_devices[self.device_id] = device_info
        from unilabos.config.config import BasicConfig
        from unilabos.ros.nodes.presets.host_node import HostNode
+
        if not BasicConfig.is_host_mode:
            sclient = self.create_client(SerialCommand, "/node_info_update")
            # 启动线程执行发送任务
@@ -440,7 +497,7 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                target=self.send_slave_node_info,
                args=(sclient,),
                daemon=True,
-                name=f"ROSDevice{self.device_id}_send_slave_node_info"
+                name=f"ROSDevice{self.device_id}_send_slave_node_info",
            ).start()
        else:
            host_node = HostNode.get_instance(0)
@@ -451,12 +508,18 @@ class BaseROS2DeviceNode(Node, Generic[T]):
        sclient.wait_for_service()
        request = SerialCommand.Request()
        from unilabos.config.config import BasicConfig
-        request.command = json.dumps({
-            "SYNC_SLAVE_NODE_INFO": {
-                "machine_name": BasicConfig.machine_name,
-                "type": "slave",
-                "edge_device_id": self.device_id
-            }}, ensure_ascii=False, cls=TypeEncoder)
+
+        request.command = json.dumps(
+            {
+                "SYNC_SLAVE_NODE_INFO": {
+                    "machine_name": BasicConfig.machine_name,
+                    "type": "slave",
+                    "edge_device_id": self.device_id,
+                }
+            },
+            ensure_ascii=False,
+            cls=TypeEncoder,
+        )

        # 发送异步请求并等待结果
        future = sclient.call_async(request)
@@ -529,6 +592,11 @@ class BaseROS2DeviceNode(Node, Generic[T]):
        """创建动作执行回调函数"""

        async def execute_callback(goal_handle: ServerGoalHandle):
+            # 初始化结果信息变量
+            execution_error = ""
+            execution_success = False
+            action_return_value = None
+
            self.lab_logger().info(f"执行动作: {action_name}")
            goal = goal_handle.request

@@ -568,7 +636,11 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                                    current_resources.extend(response.resources)
                            else:
                                r = ResourceGet.Request()
-                                r.id = action_kwargs[k]["id"] if v == "unilabos_msgs/Resource" else action_kwargs[k][0]["id"]
+                                r.id = (
+                                    action_kwargs[k]["id"]
+                                    if v == "unilabos_msgs/Resource"
+                                    else action_kwargs[k][0]["id"]
+                                )
                                r.with_children = True
                                response = await self._resource_clients["resource_get"].call_async(r)
                                current_resources.extend(response.resources)
@@ -591,7 +663,19 @@ class BaseROS2DeviceNode(Node, Generic[T]):
            if asyncio.iscoroutinefunction(ACTION):
                try:
                    self.lab_logger().info(f"异步执行动作 {ACTION}")
-                    future = ROS2DeviceNode.run_async_func(ACTION, **action_kwargs)
+                    future = ROS2DeviceNode.run_async_func(ACTION, trace_error=False, **action_kwargs)
+
+                    def _handle_future_exception(fut):
+                        nonlocal execution_error, execution_success, action_return_value
+                        try:
+                            action_return_value = fut.result()
+                            execution_success = True
+                        except Exception as e:
+                            execution_error = traceback.format_exc()
+                            error(f"异步任务 {ACTION.__name__} 报错了")
+                            error(traceback.format_exc())
+
+                    future.add_done_callback(_handle_future_exception)
                except Exception as e:
                    self.lab_logger().error(f"创建异步任务失败: {traceback.format_exc()}")
                    raise e
@@ -600,9 +684,12 @@ class BaseROS2DeviceNode(Node, Generic[T]):
                future = self._executor.submit(ACTION, **action_kwargs)

                def _handle_future_exception(fut):
+                    nonlocal execution_error, execution_success, action_return_value
                    try:
-                        fut.result()
+                        action_return_value = fut.result()
+                        execution_success = True
                    except Exception as e:
+                        execution_error = traceback.format_exc()
                        error(f"同步任务 {ACTION.__name__} 报错了")
                        error(traceback.format_exc())

@@ -659,16 +746,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:
@@ -693,6 +787,8 @@ class BaseROS2DeviceNode(Node, Generic[T]):
            for attr_name in result_msg_types.keys():
                if attr_name in ["success", "reached_goal"]:
                    setattr(result_msg, attr_name, True)
+                elif attr_name == "return_info":
+                    setattr(result_msg, attr_name, serialize_result_info(execution_error, execution_success, action_return_value))

            self.lab_logger().info(f"动作 {action_name} 完成并返回结果")
            return result_msg
@@ -738,8 +834,8 @@ class ROS2DeviceNode:
        return cls._loop

    @classmethod
-    def run_async_func(cls, func, **kwargs):
-        return run_async_func(func, loop=cls._loop, **kwargs)
+    def run_async_func(cls, func, trace_error=True, **kwargs):
+        return run_async_func(func, loop=cls._loop, trace_error=trace_error, **kwargs)

    @property
    def driver_instance(self):
@@ -791,7 +887,11 @@ class ROS2DeviceNode:
        self.resource_tracker = DeviceNodeResourceTracker()

        # use_pylabrobot_creator 使用 cls的包路径检测
-        use_pylabrobot_creator = driver_class.__module__.startswith("pylabrobot") or driver_class.__name__ == "LiquidHandlerAbstract"
+        use_pylabrobot_creator = (
+            driver_class.__module__.startswith("pylabrobot")
+            or driver_class.__name__ == "LiquidHandlerAbstract"
+            or driver_class.__name__ == "LiquidHandlerBiomek"
+        )

        # TODO: 要在创建之前预先请求服务器是否有当前id的物料，放到resource_tracker中，让pylabrobot进行创建
        # 创建设备类实例
@@ -830,6 +930,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():
--- a/unilabos/ros/nodes/presets/host_node.py
+++ b/unilabos/ros/nodes/presets/host_node.py
@@ -151,7 +151,7 @@ class HostNode(BaseROS2DeviceNode):
            mqtt_client.publish_registry(device_info["id"], device_info)
        for resource_info in lab_registry.obtain_registry_resource_info():
            mqtt_client.publish_registry(resource_info["id"], resource_info)
-
+        time.sleep(1) # 等待MQTT连接稳定
        # 首次发现网络中的设备
        self._discover_devices()

@@ -203,8 +203,12 @@ class HostNode(BaseROS2DeviceNode):
        try:
            for bridge in self.bridges:
                if hasattr(bridge, "resource_add"):
-                    self.lab_logger().info("[Host Node-Resource] Adding resources to bridge.")
-                    resource_add_res = bridge.resource_add(add_schema(resource_with_parent_name))
+                    resource_start_time = time.time()
+                    resource_add_res = bridge.resource_add(add_schema(resource_with_parent_name), True)
+                    resource_end_time = time.time()
+                    self.lab_logger().info(
+                        f"[Host Node-Resource] 物料上传 {round(resource_end_time - resource_start_time, 5) * 1000} ms"
+                    )
        except Exception as ex:
            self.lab_logger().error("[Host Node-Resource] 添加物料出错！")
            self.lab_logger().error(traceback.format_exc())
@@ -338,6 +342,7 @@ class HostNode(BaseROS2DeviceNode):
        bind_locations: list[Point],
        other_calling_params: list[str],
    ):
+        responses = []
        for resource, device_id, bind_parent_id, bind_location, other_calling_param in zip(
            resources, device_ids, bind_parent_ids, bind_locations, other_calling_params
        ):
@@ -363,8 +368,8 @@ class HostNode(BaseROS2DeviceNode):
                ensure_ascii=False,
            )
            response = sclient.call(request)
-            pass
-        pass
+            responses.append(response)
+        return responses

    def create_resource(
        self,
@@ -376,7 +381,7 @@ class HostNode(BaseROS2DeviceNode):
        liquid_input_slot: list[int],
        liquid_type: list[str],
        liquid_volume: list[int],
-        slot_on_deck: int,
+        slot_on_deck: str,
    ):
        init_new_res = initialize_resource(
            {
@@ -610,13 +615,21 @@ class HostNode(BaseROS2DeviceNode):
        """获取结果回调"""
        result_msg = future.result().result
        result_data = convert_from_ros_msg(result_msg)
+        status = "success"
+        try:
+            ret = json.loads(result_data.get("return_info", "{}"))  # 确保返回信息是有效的JSON
+            suc = ret.get("suc", False)
+            if not suc:
+                status = "failed"
+        except json.JSONDecodeError:
+            status = "failed"
        self.lab_logger().info(f"[Host Node] Result for {action_id} ({uuid_str}): success")
        self.lab_logger().debug(f"[Host Node] Result data: {result_data}")

        if uuid_str:
            for bridge in self.bridges:
                if hasattr(bridge, "publish_job_status"):
-                    bridge.publish_job_status(result_data, uuid_str, "success")
+                    bridge.publish_job_status(result_data, uuid_str, status, result_data.get("return_info", "{}"))

    def cancel_goal(self, goal_uuid: str) -> None:
        """取消目标"""
@@ -856,7 +869,6 @@ class HostNode(BaseROS2DeviceNode):
        测试网络延迟的action实现
        通过5次ping-pong机制校对时间误差并计算实际延迟
        """
-        import time
        import uuid as uuid_module

        self.lab_logger().info("=" * 60)
--- a/unilabos/ros/nodes/presets/protocol_node.py
+++ b/unilabos/ros/nodes/presets/protocol_node.py
@@ -1,6 +1,7 @@
 import time
 import asyncio
 import traceback
+from types import MethodType
 from typing import Union

 import rclpy
@@ -87,7 +88,10 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):

                    # 如果硬件接口是字符串，通过通信设备提供
                    if isinstance(name, str) and name in self.sub_devices:
+                        communicate_device = self.sub_devices[name]
+                        communicate_hardware_info = communicate_device.ros_node_instance._hardware_interface
                        self._setup_hardware_proxy(d, self.sub_devices[name], read, write)
+                        self.lab_logger().info(f"\n通信代理：为子设备{device_id}\n    添加了{read}方法(来源：{name} {communicate_hardware_info['write']}) \n    添加了{write}方法(来源：{name} {communicate_hardware_info['read']})")

    def _setup_protocol_names(self, protocol_type):
        # 处理协议类型
@@ -241,20 +245,23 @@ class ROS2ProtocolNode(BaseROS2DeviceNode):

    def _setup_hardware_proxy(self, device: ROS2DeviceNode, communication_device: ROS2DeviceNode, read_method, write_method):
        """为设备设置硬件接口代理"""
-        extra_info = [getattr(device.driver_instance, info) for info in communication_device.ros_node_instance._hardware_interface.get("extra_info", [])]
-        write_func = getattr(communication_device.ros_node_instance, communication_device.ros_node_instance._hardware_interface["write"])
-        read_func = getattr(communication_device.ros_node_instance, communication_device.ros_node_instance._hardware_interface["read"])
+        # extra_info = [getattr(device.driver_instance, info) for info in communication_device.ros_node_instance._hardware_interface.get("extra_info", [])]
+        write_func = getattr(communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["write"])
+        read_func = getattr(communication_device.driver_instance, communication_device.ros_node_instance._hardware_interface["read"])

-        def _read():
-            return read_func(*extra_info)
+        def _read(*args, **kwargs):
+            return read_func(*args, **kwargs)

-        def _write(command):
-            return write_func(*extra_info, command)
+        def _write(*args, **kwargs):
+            return write_func(*args, **kwargs)

        if read_method:
-            setattr(device.driver_instance, read_method, _read)
+            bound_read = MethodType(_read, device.driver_instance)
+            setattr(device.driver_instance, read_method, bound_read)
+
        if write_method:
-            setattr(device.driver_instance, write_method, _write)
+            bound_write = MethodType(_write, device.driver_instance)
+            setattr(device.driver_instance, write_method, bound_write)


    async def _update_resources(self, goal, protocol_kwargs):
--- a/unilabos/ros/nodes/presets/resource_mesh_manager.py
+++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py
@@ -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)
        )
--- a/unilabos/ros/nodes/resource_tracker.py
+++ b/unilabos/ros/nodes/resource_tracker.py
@@ -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):