open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 13:01:12 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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>
This commit is contained in:
Xuwznln
2025-06-12 21:01:04 +08:00
committed by GitHub
parent 3470a1cb69
commit aa1c67de29
264 changed files with 48914 additions and 2955 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
unilabos/devices/ros_dev/lh_joint_config.json Normal file
View File

@@ -0,0 +1,38 @@
{
"OTDeck":{
"joint_names":[
"first_joint",
"second_joint",
"third_joint",
"fourth_joint"
],
"link_names":[
"first_link",
"second_link",
"third_link",
"fourth_link"
],
"y":{
"first_joint":{
"factor":-0.001,
"offset":0.166
}
},
"x":{
"second_joint":{
"factor":-0.001,
"offset":0.1775
}
},
"z":{
"third_joint":{
"factor":0.001,
"offset":0.0
},
"fourth_joint":{
"factor":0.001,
"offset":0.0
}
}
}
}

346
unilabos/devices/ros_dev/liquid_handler_joint_publisher.py
View File

@@ -1,9 +1,12 @@
import asyncio
import copy
from pathlib import Path
import threading
import rclpy
import json
import time
from rclpy.executors import MultiThreadedExecutor
from rclpy.action import ActionServer
from rclpy.action import ActionServer,ActionClient
from sensor_msgs.msg import JointState
from unilabos_msgs.action import SendCmd
from rclpy.action.server import ServerGoalHandle
@@ -11,9 +14,11 @@ from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
from tf_transformations import quaternion_from_euler
from tf2_ros import TransformBroadcaster, Buffer, TransformListener
from rclpy.node import Node
import re
class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
def __init__(self,device_id:str, joint_config:dict, lh_id:str,resource_tracker, rate=50):
def __init__(self,resources_config:list, resource_tracker, rate=50, device_id:str = "lh_joint_publisher"):
super().__init__(
driver_instance=self,
device_id=device_id,
@@ -23,60 +28,118 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
print_publish=False,
resource_tracker=resource_tracker,
)
# joint_config_dict = {
# "joint_names":[
# "first_joint",
# "second_joint",
# "third_joint",
# "fourth_joint"
# ],
# "y":{
# "first_joint":{
# "factor":-1,
# "offset":0.0
# }
# },
# "x":{
# "second_joint":{
# "factor":-1,
# "offset":0.0
# }
# },
# "z":{
# "third_joint":{
# "factor":1,
# "offset":0.0
# },
# "fourth_joint":{
# "factor":1,
# "offset":0.0
# }
# }
# }
# 初始化参数
self.j_msg = JointState()
self.lh_id = lh_id
# self.j_msg.name = joint_names
self.joint_config = joint_config
self.j_msg.position = [0.0 for i in range(len(joint_config['joint_names']))]
self.j_msg.name = [f"{self.lh_id}_{x}" for x in joint_config['joint_names']]
# self.joint_config = joint_config_dict
# self.j_msg.position = [0.0 for i in range(len(joint_config_dict['joint_names']))]
# self.j_msg.name = [f"{self.lh_id}_{x}" for x in joint_config_dict['joint_names']]
joint_config = json.load(open(f"{Path(__file__).parent.absolute()}/lh_joint_config.json", encoding="utf-8"))
self.resources_config = {x['id']:x for x in resources_config}
self.rate = rate
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.j_pub = self.create_publisher(JointState,'/joint_states',10)
self.create_timer(0.02,self.lh_joint_pub_callback)
self.create_timer(1,self.lh_joint_pub_callback)
self.resource_action = None
while self.resource_action is None:
self.resource_action = self.check_tf_update_actions()
time.sleep(1)
self.resource_action_client = ActionClient(self, SendCmd, self.resource_action)
while not self.resource_action_client.wait_for_server(timeout_sec=1.0):
self.get_logger().info('等待 TfUpdate 服务器...')
self.deck_list = []
self.lh_devices = {}
# 初始化设备ID与config信息
for resource in resources_config:
if resource['class'] == 'liquid_handler':
deck_id = resource['config']['data']['children'][0]['_resource_child_name']
deck_class = resource['config']['data']['children'][0]['_resource_type'].split(':')[-1]
key = f'{deck_id}'
# key = f'{resource["id"]}_{deck_id}'
self.lh_devices[key] = {
'joint_msg':JointState(
name=[f'{key}_{x}' for x in joint_config[deck_class]['joint_names']],
position=[0.0 for _ in joint_config[deck_class]['joint_names']]
),
'joint_config':joint_config[deck_class]
}
self.deck_list.append(deck_id)
print('='*20)
print(self.lh_devices)
print('='*20)
self.j_action = ActionServer(
self,
SendCmd,
"joint",
"hl_joint_action",
self.lh_joint_action_callback,
result_timeout=5000
)
def check_tf_update_actions(self):
topics = self.get_topic_names_and_types()
for topic_item in topics:
topic_name, topic_types = topic_item
if 'action_msgs/msg/GoalStatusArray' in topic_types:
# 删除 /_action/status 部分
base_name = topic_name.replace('/_action/status', '')
# 检查最后一个部分是否为 tf_update
parts = base_name.split('/')
if parts and parts[-1] == 'tf_update':
return base_name
return None
def find_resource_parent(self, resource_id:str):
# 遍历父辈，找到父辈的父辈，直到找到设备ID
parent_id = self.resources_config[resource_id]['parent']
try:
if parent_id in self.deck_list:
p_ = self.resources_config[parent_id]['parent']
str_ = f'{parent_id}'
return str(str_)
else:
return self.find_resource_parent(parent_id)
except Exception as e:
return None
def send_resource_action(self, resource_id_list:list[str], link_name:str):
goal_msg = SendCmd.Goal()
str_dict = {}
for resource in resource_id_list:
str_dict[resource] = link_name
goal_msg.command = json.dumps(str_dict)
self.resource_action_client.send_goal(goal_msg)
def resource_move(self, resource_id:str, link_name:str, channels:list[int]):
resource = resource_id.rsplit("_",1)
channel_list = ['A','B','C','D','E','F','G','H']
resource_list = []
match = re.match(r'([a-zA-Z_]+)(\d+)', resource[1])
if match:
number = match.group(2)
for channel in channels:
resource_list.append(f"{resource[0]}_{channel_list[channel]}{number}")
if len(resource_list) > 0:
self.send_resource_action(resource_list, link_name)
def lh_joint_action_callback(self,goal_handle: ServerGoalHandle):
"""Move a single joint
@@ -101,12 +164,13 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
goal_handle.succeed()
except Exception as e:
print(e)
print(f'Liquid handler action error: \n{e}')
goal_handle.abort()
result.success = False
return result
def inverse_kinematics(self, x, y, z,
parent_id,
x_joint:dict,
y_joint:dict,
z_joint:dict ):
@@ -117,77 +181,102 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
x (float): x坐标
y (float): y坐标
z (float): z坐标
x_joint (dict): x轴关节配置，包含plus和offset
y_joint (dict): y轴关节配置，包含plus和offset
z_joint (dict): z轴关节配置，包含plus和offset
x_joint (dict): x轴关节配置，包含factor和offset
y_joint (dict): y轴关节配置，包含factor和offset
z_joint (dict): z轴关节配置，包含factor和offset
Returns:
dict: 关节名称和对应位置的字典
"""
joint_positions = copy.deepcopy(self.j_msg.position)
joint_positions = copy.deepcopy(self.lh_devices[parent_id]['joint_msg'].position)
z_index = 0
# 处理x轴关节
for joint_name, config in x_joint.items():
index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}")
joint_positions[index] = x * config["factor"] + config["offset"]
# 处理y轴关节
for joint_name, config in y_joint.items():
index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}")
joint_positions[index] = y * config["factor"] + config["offset"]
# 处理z轴关节
for joint_name, config in z_joint.items():
index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}")
joint_positions[index] = z * config["factor"] + config["offset"]
return joint_positions
z_index = index
return joint_positions ,z_index
def move_joints(self, resource_name, link_name, speed, x_joint=None, y_joint=None, z_joint=None):
def move_joints(self, resource_names, x, y, z, option, speed = 0.1 ,x_joint=None, y_joint=None, z_joint=None):
if isinstance(resource_names, list):
resource_name_ = resource_names[0]
else:
resource_name_ = resource_names
transform = self.tf_buffer.lookup_transform(
link_name,
resource_name,
rclpy.time.Time()
)
x,y,z = transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z
parent_id = self.find_resource_parent(resource_name_)
print('!'*20)
print(parent_id)
print('!'*20)
if x_joint is None:
x_joint_config = next(iter(self.joint_config['x'].items()))
elif x_joint in self.joint_config['x']:
x_joint_config = self.joint_config['x'][x_joint]
xa,xb = next(iter(self.lh_devices[parent_id]['joint_config']['x'].items()))
x_joint_config = {xa:xb}
elif x_joint in self.lh_devices[parent_id]['joint_config']['x']:
x_joint_config = self.lh_devices[parent_id]['joint_config']['x'][x_joint]
else:
raise ValueError(f"x_joint {x_joint} not in joint_config['x']")
if y_joint is None:
y_joint_config = next(iter(self.joint_config['y'].items()))
elif y_joint in self.joint_config['y']:
y_joint_config = self.joint_config['y'][y_joint]
ya,yb = next(iter(self.lh_devices[parent_id]['joint_config']['y'].items()))
y_joint_config = {ya:yb}
elif y_joint in self.lh_devices[parent_id]['joint_config']['y']:
y_joint_config = self.lh_devices[parent_id]['joint_config']['y'][y_joint]
else:
raise ValueError(f"y_joint {y_joint} not in joint_config['y']")
if z_joint is None:
z_joint_config = next(iter(self.joint_config['z'].items()))
elif z_joint in self.joint_config['z']:
z_joint_config = self.joint_config['z'][z_joint]
za, zb = next(iter(self.lh_devices[parent_id]['joint_config']['z'].items()))
z_joint_config = {za :zb}
elif z_joint in self.lh_devices[parent_id]['joint_config']['z']:
z_joint_config = self.lh_devices[parent_id]['joint_config']['z'][z_joint]
else:
raise ValueError(f"z_joint {z_joint} not in joint_config['z']")
joint_positions_target = self.inverse_kinematics(x,y,z,x_joint_config,y_joint_config,z_joint_config)
joint_positions_target, z_index = self.inverse_kinematics(x,y,z,parent_id,x_joint_config,y_joint_config,z_joint_config)
joint_positions_target_zero = copy.deepcopy(joint_positions_target)
joint_positions_target_zero[z_index] = 0
self.move_to(joint_positions_target_zero, speed, parent_id)
self.move_to(joint_positions_target, speed, parent_id)
time.sleep(1)
if option == "pick":
link_name = self.lh_devices[parent_id]['joint_config']['link_names'][z_index]
link_name = f'{parent_id}_{link_name}'
self.resource_move(resource_name_, link_name, [0,1,2,3,4,5,6,7])
elif option == "drop_trash":
self.resource_move(resource_name_, "__trash", [0,1,2,3,4,5,6,7])
elif option == "drop":
self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7])
self.move_to(joint_positions_target_zero, speed, parent_id)
def move_to(self, joint_positions ,speed, parent_id):
loop_flag = 0
while loop_flag < len(self.joint_config['joint_names']):
while loop_flag < len(joint_positions):
loop_flag = 0
for i in range(len(self.joint_config['joint_names'])):
distance = joint_positions_target[i] - self.j_msg.position[i]
for i in range(len(joint_positions)):
distance = joint_positions[i] - self.lh_devices[parent_id]['joint_msg'].position[i]
if distance == 0:
loop_flag += 1
continue
minus_flag = distance/abs(distance)
if abs(distance) > speed/self.rate:
self.j_msg.position[i] += minus_flag * speed/self.rate
self.lh_devices[parent_id]['joint_msg'].position[i] += minus_flag * speed/self.rate
else :
self.j_msg.position[i] = joint_positions_target[i]
self.lh_devices[parent_id]['joint_msg'].position[i] = joint_positions[i]
loop_flag += 1
@@ -195,10 +284,103 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
self.lh_joint_pub_callback()
time.sleep(1/self.rate)
def lh_joint_pub_callback(self):
self.j_msg.header.stamp = self.get_clock().now().to_msg()
self.j_pub.publish(self.j_msg)
for id, config in self.lh_devices.items():
config['joint_msg'].header.stamp = self.get_clock().now().to_msg()
self.j_pub.publish(config['joint_msg'])
class JointStatePublisher(Node):
def __init__(self):
super().__init__('joint_state_publisher')
self.lh_action = None
while self.lh_action is None:
self.lh_action = self.check_hl_joint_actions()
time.sleep(1)
self.lh_action_client = ActionClient(self, SendCmd, self.lh_action)
while not self.lh_action_client.wait_for_server(timeout_sec=1.0):
self.get_logger().info('等待 TfUpdate 服务器...')
def check_hl_joint_actions(self):
topics = self.get_topic_names_and_types()
for topic_item in topics:
topic_name, topic_types = topic_item
if 'action_msgs/msg/GoalStatusArray' in topic_types:
# 删除 /_action/status 部分
base_name = topic_name.replace('/_action/status', '')
# 检查最后一个部分是否为 tf_update
parts = base_name.split('/')
if parts and parts[-1] == 'hl_joint_action':
return base_name
return None
def send_resource_action(self, resource_name, x,y,z,option, speed = 0.1,x_joint=None, y_joint=None, z_joint=None):
goal_msg = SendCmd.Goal()
str_dict = {
'resource_names':resource_name,
'x':x,
'y':y,
'z':z,
'option':option,
'speed':speed,
'x_joint':x_joint,
'y_joint':y_joint,
'z_joint':z_joint
}
goal_msg.command = json.dumps(str_dict)
if not self.lh_action_client.wait_for_server(timeout_sec=5.0):
self.get_logger().error('Action server not available')
return None
try:
# 创建新的executor
executor = rclpy.executors.MultiThreadedExecutor()
executor.add_node(self)
# 发送目标
future = self.lh_action_client.send_goal_async(goal_msg)
# 使用executor等待结果
while not future.done():
executor.spin_once(timeout_sec=0.1)
handle = future.result()
if not handle.accepted:
self.get_logger().error('Goal was rejected')
return None
# 等待最终结果
result_future = handle.get_result_async()
while not result_future.done():
executor.spin_once(timeout_sec=0.1)
result = result_future.result()
return result
except Exception as e:
self.get_logger().error(f'Error during action execution: {str(e)}')
return None
finally:
# 清理executor
executor.remove_node(self)
def main():

2442
unilabos/devices/ros_dev/moveit2.py Normal file
View File

File diff suppressed because it is too large Load Diff

384
unilabos/devices/ros_dev/moveit_interface.py Normal file
View File

@@ -0,0 +1,384 @@
import json
import time
from copy import deepcopy
from pathlib import Path
from moveit_msgs.msg import JointConstraint, Constraints
from rclpy.action import ActionClient
from tf2_ros import Buffer, TransformListener
from unilabos_msgs.action import SendCmd
from unilabos.devices.ros_dev.moveit2 import MoveIt2
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
class MoveitInterface:
_ros_node: BaseROS2DeviceNode
tf_buffer: Buffer
tf_listener: TransformListener
def __init__(self, moveit_type, joint_poses, rotation=None, device_config=None):
self.device_config = device_config
self.rotation = rotation
self.data_config = json.load(
open(
f"{Path(__file__).parent.parent.parent.absolute()}/device_mesh/devices/{moveit_type}/config/move_group.json",
encoding="utf-8",
)
)
self.arm_move_flag = False
self.move_option = ["pick", "place", "side_pick", "side_place"]
self.joint_poses = joint_poses
self.cartesian_flag = False
self.mesh_group = ["reactor", "sample", "beaker"]
self.moveit2 = {}
self.resource_action = None
self.resource_client = None
self.resource_action_ok = False
def post_init(self, ros_node: BaseROS2DeviceNode):
self._ros_node = ros_node
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self._ros_node)
for move_group, config in self.data_config.items():
base_link_name = f"{self._ros_node.device_id}_{config['base_link_name']}"
end_effector_name = f"{self._ros_node.device_id}_{config['end_effector_name']}"
joint_names = [f"{self._ros_node.device_id}_{name}" for name in config["joint_names"]]
self.moveit2[f"{move_group}"] = MoveIt2(
node=self._ros_node,
joint_names=joint_names,
base_link_name=base_link_name,
end_effector_name=end_effector_name,
group_name=f"{self._ros_node.device_id}_{move_group}",
callback_group=self._ros_node.callback_group,
use_move_group_action=True,
ignore_new_calls_while_executing=True,
)
self.moveit2[f"{move_group}"].allowed_planning_time = 3.0
self._ros_node.create_timer(1, self.wait_for_resource_action, callback_group=self._ros_node.callback_group)
def wait_for_resource_action(self):
if not self.resource_action_ok:
while self.resource_action is None:
self.resource_action = self.check_tf_update_actions()
time.sleep(1)
self.resource_client = ActionClient(self._ros_node, SendCmd, self.resource_action)
self.resource_action_ok = True
while not self.resource_client.wait_for_server(timeout_sec=5.0):
self._ros_node.lab_logger().info("等待 TfUpdate 服务器...")
def check_tf_update_actions(self):
topics = self._ros_node.get_topic_names_and_types()
for topic_item in topics:
topic_name, topic_types = topic_item
if "action_msgs/msg/GoalStatusArray" in topic_types:
# 删除 /_action/status 部分
base_name = topic_name.replace("/_action/status", "")
# 检查最后一个部分是否为 tf_update
parts = base_name.split("/")
if parts and parts[-1] == "tf_update":
return base_name
return None
def set_position(self, command):
"""使用moveit 移动到指定点
Args:
command: A JSON-formatted string that includes quaternion, speed, position
position (list) : 点的位置 [x,y,z]
quaternion (list) : 点的姿态(四元数) [x,y,z,w]
move_group (string) : The move group moveit will plan
speed (float) : The speed of the movement, speed > 0
retry (int) : Retry times when moveit plan fails
Returns:
None
"""
result = SendCmd.Result()
cmd_str = command.replace("'", '"')
cmd_dict = json.loads(cmd_str)
self.moveit_task(**cmd_dict)
return result
def moveit_task(
self, move_group, position, quaternion, speed=1, retry=10, cartesian=False, target_link=None, offsets=[0, 0, 0]
):
speed_ = float(max(0.1, min(speed, 1)))
self.moveit2[move_group].max_velocity = speed_
self.moveit2[move_group].max_acceleration = speed_
re_ = False
pose_result = [x + y for x, y in zip(position, offsets)]
# print(pose_result)
while retry > -1 and not re_:
self.moveit2[move_group].move_to_pose(
target_link=target_link,
position=pose_result,
quat_xyzw=quaternion,
cartesian=cartesian,
# cartesian_fraction_threshold=0.0,
cartesian_max_step=0.01,
weight_position=1.0,
)
re_ = self.moveit2[move_group].wait_until_executed()
retry += -1
return re_
def moveit_joint_task(self, move_group, joint_positions, joint_names=None, speed=1, retry=10):
re_ = False
joint_positions_ = [float(x) for x in joint_positions]
speed_ = float(max(0.1, min(speed, 1)))
self.moveit2[move_group].max_velocity = speed_
self.moveit2[move_group].max_acceleration = speed_
while retry > -1 and not re_:
self.moveit2[move_group].move_to_configuration(joint_positions=joint_positions_, joint_names=joint_names)
re_ = self.moveit2[move_group].wait_until_executed()
retry += -1
print(self.moveit2[move_group].compute_fk(joint_positions))
return re_
def resource_manager(self, resource, parent_link):
goal_msg = SendCmd.Goal()
str_dict = {}
str_dict[resource] = parent_link
goal_msg.command = json.dumps(str_dict)
assert self.resource_client is not None
self.resource_client.send_goal(goal_msg)
return True
def pick_and_place(self, command: str):
"""
Using MoveIt to make the robotic arm pick or place materials to a target point.
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
"""
result = SendCmd.Result()
try:
cmd_str = str(command).replace("'", '"')
cmd_dict = json.loads(cmd_str)
if cmd_dict["option"] in self.move_option:
option_index = self.move_option.index(cmd_dict["option"])
place_flag = option_index % 2
config = {}
function_list = []
status = cmd_dict["status"]
joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
config.update({k: cmd_dict[k] for k in ["speed", "retry", "move_group"] if k in cmd_dict})
# 夹取
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
)
)
else:
function_list.append(lambda: self.resource_manager(cmd_dict["resource"], "world"))
constraints = []
if "constraints" in cmd_dict.keys():
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,
)
)
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,
]
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
pose[2] += float(cmd_dict["lift_height"])
function_list.append(
lambda: self.moveit_task(
position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
)
)
end_pose = 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"])
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
else:
function_list = [
lambda: self.moveit_joint_task(**config, joint_positions=joint_positions_)
] + function_list
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
except Exception as e:
print(e)
self.cartesian_flag = False
result.success = False
return result
def set_status(self, command: str):
"""
Goto home position
Args:
command: A JSON-formatted string that includes speed
*status (string) : The joint status moveit will plan
*move_group (string): The move group moveit will plan
separate (list) : The joint index to be separated
lift_height (float) : The height at which the material should be lifted(meters)
x_distance (float) : The distance to the target in x direction(meters)
y_distance (float) : The distance to the target in y direction(meters)
speed (float) : The speed of the movement, speed > 0
retry (float) : Retry times when moveit plan fails
Returns:
None
"""
result = SendCmd.Result()
try:
cmd_str = command.replace("'", '"')
cmd_dict = json.loads(cmd_str)
config = {}
config["move_group"] = cmd_dict["move_group"]
if "speed" in cmd_dict.keys():
config["speed"] = cmd_dict["speed"]
if "retry" in cmd_dict.keys():
config["retry"] = cmd_dict["retry"]
status = cmd_dict["status"]
joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
re = self.moveit_joint_task(**config, joint_positions=joint_positions_)
if not re:
result.success = False
return result
result.success = True
except Exception as e:
print(e)
result.success = False
return result