mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-17 13:01:12 +00:00
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>
This commit is contained in:
q434343
GitHub
@@ -4,13 +4,14 @@
|
|||||||
"id": "Gripper1",
|
"id": "Gripper1",
|
||||||
"name": "假夹爪",
|
"name": "假夹爪",
|
||||||
"children": [
|
"children": [
|
||||||
|
"Plate1"
|
||||||
],
|
],
|
||||||
"parent": null,
|
"parent": null,
|
||||||
"type": "device",
|
"type": "device",
|
||||||
"class": "gripper.mock",
|
"class": "gripper.mock",
|
||||||
"position": {
|
"position": {
|
||||||
"x": 620.6111111111111,
|
"x": 0,
|
||||||
"y": 171,
|
"y": 0,
|
||||||
"z": 0
|
"z": 0
|
||||||
},
|
},
|
||||||
"config": {
|
"config": {
|
||||||
@@ -23,18 +24,120 @@
|
|||||||
"name": "Plate1",
|
"name": "Plate1",
|
||||||
"children": [
|
"children": [
|
||||||
],
|
],
|
||||||
"parent": null,
|
"parent": "Gripper1",
|
||||||
"type": "plate",
|
"type": "plate",
|
||||||
"class": "nest_96_wellplate_2ml_deep",
|
"class": "nest_96_wellplate_100ul_pcr_full_skirt",
|
||||||
"position": {
|
"position": {
|
||||||
"x": 620.6111111111111,
|
"x": 0,
|
||||||
"y": 171,
|
"y": 0,
|
||||||
"z": 0
|
"z": 69
|
||||||
},
|
},
|
||||||
"config": {
|
"config": {
|
||||||
},
|
},
|
||||||
"data": {
|
"data": {
|
||||||
}
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "ot_joint_publisher",
|
||||||
|
"name": "ot_joint_publisher",
|
||||||
|
"sample_id": null,
|
||||||
|
"children": [
|
||||||
|
|
||||||
|
],
|
||||||
|
"parent": null,
|
||||||
|
"type": "device",
|
||||||
|
"class": "lh_joint_publisher",
|
||||||
|
"position": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"config": {
|
||||||
|
"lh_id":"deck",
|
||||||
|
"joint_config":
|
||||||
|
{
|
||||||
|
"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
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"data": {}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "ot_joint_publisher",
|
||||||
|
"name": "ot_joint_publisher",
|
||||||
|
"sample_id": null,
|
||||||
|
"children": [
|
||||||
|
|
||||||
|
],
|
||||||
|
"parent": null,
|
||||||
|
"type": "device",
|
||||||
|
"class": "lh_joint_publisher",
|
||||||
|
"position": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"config": {
|
||||||
|
"lh_id":"deck",
|
||||||
|
"joint_config":
|
||||||
|
{
|
||||||
|
"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
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"data": {}
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"links": [
|
"links": [
|
||||||
|
|||||||
135
test/experiments/test_copy.json
Normal file
135
test/experiments/test_copy.json
Normal file
@@ -0,0 +1,135 @@
|
|||||||
|
{
|
||||||
|
"nodes": [
|
||||||
|
{
|
||||||
|
"id": "PLR_STATION",
|
||||||
|
"name": "PLR_LH_TEST",
|
||||||
|
"parent": null,
|
||||||
|
"type": "device",
|
||||||
|
"class": "liquid_handler",
|
||||||
|
"position": {
|
||||||
|
"x": 620.6111111111111,
|
||||||
|
"y": 171,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"config": {
|
||||||
|
"data": {
|
||||||
|
"children": [
|
||||||
|
{
|
||||||
|
"_resource_child_name": "deck",
|
||||||
|
"_resource_type": "pylabrobot.resources.opentrons.deck:OTDeck"
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"backend": {
|
||||||
|
"type": "LiquidHandlerRvizBackend"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"data": {},
|
||||||
|
"children": [
|
||||||
|
"deck"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "deck",
|
||||||
|
"name": "deck",
|
||||||
|
"sample_id": null,
|
||||||
|
"children": [
|
||||||
|
"teaching_carrier"
|
||||||
|
],
|
||||||
|
"parent": "PLR_STATION",
|
||||||
|
"type": "deck",
|
||||||
|
"class": "OTDeck",
|
||||||
|
"position": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"config": {
|
||||||
|
"type": "OTDeck",
|
||||||
|
"with_trash": false,
|
||||||
|
"rotation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0,
|
||||||
|
"type": "Rotation"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"data": {}
|
||||||
|
},
|
||||||
|
|
||||||
|
{
|
||||||
|
"id": "teaching_carrier",
|
||||||
|
"name": "teaching_carrier",
|
||||||
|
"sample_id": null,
|
||||||
|
"children": [
|
||||||
|
"teaching_carrier_A1"
|
||||||
|
],
|
||||||
|
"parent": "deck",
|
||||||
|
"type": "plate",
|
||||||
|
"class": "opentrons_96_filtertiprack_1000ul",
|
||||||
|
"position": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 69
|
||||||
|
},
|
||||||
|
"config": {
|
||||||
|
"type": "Resource",
|
||||||
|
"size_x": 127,
|
||||||
|
"size_y": 85,
|
||||||
|
"size_z": 0,
|
||||||
|
"rotation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0,
|
||||||
|
"type": "Rotation"
|
||||||
|
},
|
||||||
|
"category": null,
|
||||||
|
"model": null
|
||||||
|
},
|
||||||
|
"data": {}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "teaching_carrier_A1",
|
||||||
|
"name": "teaching_carrier_A1",
|
||||||
|
"sample_id": null,
|
||||||
|
"children": [],
|
||||||
|
"parent": "teaching_carrier",
|
||||||
|
"type": "device",
|
||||||
|
"class": "",
|
||||||
|
"position": {
|
||||||
|
"x": 10.87,
|
||||||
|
"y": 70.77,
|
||||||
|
"z": 10
|
||||||
|
},
|
||||||
|
"config": {
|
||||||
|
"type": "TipSpot",
|
||||||
|
"size_x": 6.86,
|
||||||
|
"size_y": 6.86,
|
||||||
|
"size_z": 10.67,
|
||||||
|
"rotation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0,
|
||||||
|
"type": "Rotation"
|
||||||
|
},
|
||||||
|
"category": "tip_spot",
|
||||||
|
"model": null,
|
||||||
|
"prototype_tip": {
|
||||||
|
"type": "Tip",
|
||||||
|
"total_tip_length": 39.2,
|
||||||
|
"has_filter": true,
|
||||||
|
"maximal_volume": 20.0,
|
||||||
|
"fitting_depth": 3.29
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"data": {
|
||||||
|
"liquids": [],
|
||||||
|
"pending_liquids": [],
|
||||||
|
"liquid_history": []
|
||||||
|
}
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"links": [
|
||||||
|
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -12,6 +12,8 @@ def start_backend(
|
|||||||
controllers_config: dict = {},
|
controllers_config: dict = {},
|
||||||
bridges=[],
|
bridges=[],
|
||||||
without_host: bool = False,
|
without_host: bool = False,
|
||||||
|
visual: str = "None",
|
||||||
|
resources_mesh_config: dict = {},
|
||||||
**kwargs
|
**kwargs
|
||||||
):
|
):
|
||||||
if backend == "ros":
|
if backend == "ros":
|
||||||
@@ -29,7 +31,9 @@ def start_backend(
|
|||||||
|
|
||||||
backend_thread = threading.Thread(
|
backend_thread = threading.Thread(
|
||||||
target=main if not without_host else slave,
|
target=main if not without_host else slave,
|
||||||
args=(devices_config, resources_config, graph, controllers_config, bridges)
|
args=(devices_config, resources_config, graph, controllers_config, bridges, visual, resources_mesh_config),
|
||||||
|
name="backend_thread",
|
||||||
|
daemon=True,
|
||||||
)
|
)
|
||||||
backend_thread.start()
|
backend_thread.start()
|
||||||
logger.info(f"Backend {backend} started.")
|
logger.info(f"Backend {backend} started.")
|
||||||
|
|||||||
@@ -1,10 +1,17 @@
|
|||||||
import argparse
|
import argparse
|
||||||
|
import asyncio
|
||||||
import os
|
import os
|
||||||
import signal
|
import signal
|
||||||
import sys
|
import sys
|
||||||
import json
|
import json
|
||||||
|
import time
|
||||||
|
|
||||||
import yaml
|
import yaml
|
||||||
from copy import deepcopy
|
from copy import deepcopy
|
||||||
|
import threading
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
|
||||||
|
|
||||||
# 首先添加项目根目录到路径
|
# 首先添加项目根目录到路径
|
||||||
current_dir = os.path.dirname(os.path.abspath(__file__))
|
current_dir = os.path.dirname(os.path.abspath(__file__))
|
||||||
@@ -14,6 +21,10 @@ if ilabos_dir not in sys.path:
|
|||||||
|
|
||||||
from unilabos.config.config import load_config, BasicConfig, _update_config_from_env
|
from unilabos.config.config import load_config, BasicConfig, _update_config_from_env
|
||||||
from unilabos.utils.banner_print import print_status, print_unilab_banner
|
from unilabos.utils.banner_print import print_status, print_unilab_banner
|
||||||
|
from unilabos.device_mesh.resource_visalization import ResourceVisualization
|
||||||
|
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
|
||||||
|
from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
|
||||||
|
from rclpy.executors import MultiThreadedExecutor
|
||||||
|
|
||||||
|
|
||||||
def parse_args():
|
def parse_args():
|
||||||
@@ -70,7 +81,12 @@ def parse_args():
|
|||||||
default=True,
|
default=True,
|
||||||
help="是否在启动时打开信息页",
|
help="是否在启动时打开信息页",
|
||||||
)
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--visual",
|
||||||
|
choices=["rviz", "web","None"],
|
||||||
|
default="rviz",
|
||||||
|
help="选择可视化工具: 'rviz' 或 'web' 或 'None'，默认'rviz'",
|
||||||
|
)
|
||||||
return parser.parse_args()
|
return parser.parse_args()
|
||||||
|
|
||||||
|
|
||||||
@@ -121,6 +137,7 @@ def main():
|
|||||||
# 注册表
|
# 注册表
|
||||||
build_registry(args_dict["registry_path"])
|
build_registry(args_dict["registry_path"])
|
||||||
|
|
||||||
|
|
||||||
if args_dict["graph"] is not None:
|
if args_dict["graph"] is not None:
|
||||||
import unilabos.resources.graphio as graph_res
|
import unilabos.resources.graphio as graph_res
|
||||||
graph_res.physical_setup_graph = (
|
graph_res.physical_setup_graph = (
|
||||||
@@ -132,6 +149,7 @@ def main():
|
|||||||
args_dict["resources_config"] = initialize_resources(list(deepcopy(devices_and_resources).values()))
|
args_dict["resources_config"] = initialize_resources(list(deepcopy(devices_and_resources).values()))
|
||||||
args_dict["devices_config"] = dict_to_nested_dict(deepcopy(devices_and_resources), devices_only=False)
|
args_dict["devices_config"] = dict_to_nested_dict(deepcopy(devices_and_resources), devices_only=False)
|
||||||
# args_dict["resources_config"] = dict_to_tree(devices_and_resources, devices_only=False)
|
# args_dict["resources_config"] = dict_to_tree(devices_and_resources, devices_only=False)
|
||||||
|
|
||||||
args_dict["graph"] = graph_res.physical_setup_graph
|
args_dict["graph"] = graph_res.physical_setup_graph
|
||||||
else:
|
else:
|
||||||
if args_dict["devices"] is None or args_dict["resources"] is None:
|
if args_dict["devices"] is None or args_dict["resources"] is None:
|
||||||
@@ -166,9 +184,28 @@ def main():
|
|||||||
signal.signal(signal.SIGINT, _exit)
|
signal.signal(signal.SIGINT, _exit)
|
||||||
signal.signal(signal.SIGTERM, _exit)
|
signal.signal(signal.SIGTERM, _exit)
|
||||||
mqtt_client.start()
|
mqtt_client.start()
|
||||||
|
args_dict["resources_mesh_config"] = {}
|
||||||
|
|
||||||
start_backend(**args_dict)
|
if args_dict["visual"] != "None":
|
||||||
start_server(port=args_dict.get("port", 8002), open_browser=args_dict.get("open_browser", False))
|
if args_dict["visual"] == "rviz":
|
||||||
|
enable_rviz=True
|
||||||
|
elif args_dict["visual"] == "web":
|
||||||
|
enable_rviz=False
|
||||||
|
resource_visualization = ResourceVisualization(devices_and_resources, args_dict["resources_config"] ,enable_rviz=enable_rviz)
|
||||||
|
|
||||||
|
args_dict["resources_mesh_config"] = resource_visualization.resource_model
|
||||||
|
# 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中
|
||||||
|
|
||||||
|
start_backend(**args_dict)
|
||||||
|
server_thread = threading.Thread(target=start_server)
|
||||||
|
server_thread.start()
|
||||||
|
asyncio.set_event_loop(asyncio.new_event_loop())
|
||||||
|
resource_visualization.start()
|
||||||
|
while True:
|
||||||
|
time.sleep(1)
|
||||||
|
else:
|
||||||
|
start_backend(**args_dict)
|
||||||
|
start_server()
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
|
|||||||
@@ -35,7 +35,8 @@ class MQTTClient:
|
|||||||
self.client.on_disconnect = self._on_disconnect
|
self.client.on_disconnect = self._on_disconnect
|
||||||
|
|
||||||
def _on_log(self, client, userdata, level, buf):
|
def _on_log(self, client, userdata, level, buf):
|
||||||
logger.info(f"[MQTT] log: {buf}")
|
# logger.info(f"[MQTT] log: {buf}")
|
||||||
|
pass
|
||||||
|
|
||||||
def _on_connect(self, client, userdata, flags, rc, properties=None):
|
def _on_connect(self, client, userdata, flags, rc, properties=None):
|
||||||
logger.info("[MQTT] Connected with result code " + str(rc))
|
logger.info("[MQTT] Connected with result code " + str(rc))
|
||||||
|
|||||||
@@ -0,0 +1,18 @@
|
|||||||
|
{
|
||||||
|
"first_joint": {
|
||||||
|
"child":"first_link",
|
||||||
|
"axis" : "-y"
|
||||||
|
},
|
||||||
|
"second_joint": {
|
||||||
|
"child":"second_link",
|
||||||
|
"axis" : "-x"
|
||||||
|
},
|
||||||
|
"third_joint": {
|
||||||
|
"child":"third_link",
|
||||||
|
"axis" : "-z"
|
||||||
|
},
|
||||||
|
"fourth_joint": {
|
||||||
|
"child":"fourth_link",
|
||||||
|
"axis" : "-z"
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,210 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
|
||||||
|
<robot xmlns:xacro="http://ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<xacro:macro name="opentrons_liquid_handler"
|
||||||
|
params="parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 r:=0 mesh_path:=''">
|
||||||
|
|
||||||
|
|
||||||
|
<joint name="${station_name}${device_name}base_link_joint" type="fixed">
|
||||||
|
<origin xyz="${x} ${y} ${z}" rpy="0 0 ${r}" />
|
||||||
|
<parent link="${parent_link}"/>
|
||||||
|
<child link="${station_name}${device_name}device_link"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="${station_name}${device_name}device_link"/>
|
||||||
|
<joint name="${station_name}${device_name}device_link_joint" type="fixed">
|
||||||
|
<origin xyz="-0.11565 0.496 0" rpy="0 0 0" />
|
||||||
|
<parent link="${station_name}${device_name}device_link"/>
|
||||||
|
<child link="${station_name}${device_name}main_link"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name='${station_name}${device_name}main_link'>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-0.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-0.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name='${station_name}${device_name}first_link'>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-1.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-1.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name='${station_name}${device_name}second_link'>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-2.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-2.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name='${station_name}${device_name}third_link'>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3a.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3a.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<link name='${station_name}${device_name}fourth_link'>
|
||||||
|
<visual>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3b.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3b.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<joint name='${station_name}${device_name}first_joint' type='prismatic'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}first_link"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<axis xyz="0 -1 0"/>
|
||||||
|
<limit effort="-1" lower="-0.2" upper="0.13" velocity="-1"/>
|
||||||
|
<dynamics damping="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}second_joint' type='prismatic'>
|
||||||
|
<parent link="${station_name}${device_name}first_link"/>
|
||||||
|
<child link="${station_name}${device_name}second_link"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<axis xyz="-1 0 0"/>
|
||||||
|
<limit effort="-1" lower="-0.15" upper="0.15" velocity="-1"/>
|
||||||
|
<dynamics damping="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}third_joint' type='prismatic'>
|
||||||
|
<parent link="${station_name}${device_name}second_link"/>
|
||||||
|
<child link="${station_name}${device_name}third_link"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<axis xyz="0 0 -1"/>
|
||||||
|
<limit effort="-1" lower="0" upper="0.22" velocity="-1"/>
|
||||||
|
<dynamics damping="0.1"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}fourth_joint' type='prismatic'>
|
||||||
|
<parent link="${station_name}${device_name}second_link"/>
|
||||||
|
<child link="${station_name}${device_name}fourth_link"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<axis xyz="0 0 -1"/>
|
||||||
|
<limit effort="-1" lower="0" upper="0.22" velocity="-1"/>
|
||||||
|
<dynamics damping="0.1"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name='${station_name}${device_name}socketTypeGenericSbsFootprint'/>
|
||||||
|
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_10_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.1795 -0.1825 0.07"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_7_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.1795 -0.273 0.07"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_4_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.1795 -0.3635 0.07"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_1_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.1795 -0.454 0.07"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_11_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.312 -0.1825 0.07"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_8_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.312 -0.273 0.07"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_5_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.312 -0.3635 0.07"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_2_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.312 -0.454 0.07"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_9_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.4445 -0.273 0.07"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_6_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.4445 -0.3635 0.07"/>
|
||||||
|
</joint>
|
||||||
|
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_3_60_1' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
|
||||||
|
<origin rpy="0 0 -1.57" xyz="0.4445 -0.454 0.07"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
|
||||||
|
<link name='${station_name}${device_name}socketTypeHEPAModule'/>
|
||||||
|
<joint name='${station_name}${device_name}socketTypeHEPAModule' type='fixed'>
|
||||||
|
<parent link="${station_name}${device_name}main_link"/>
|
||||||
|
<child link="${station_name}${device_name}socketTypeHEPAModule"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0.31 -0.26 0.66"/>
|
||||||
|
</joint>
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,10 @@
|
|||||||
|
{
|
||||||
|
"private_param":
|
||||||
|
{
|
||||||
|
|
||||||
|
},
|
||||||
|
"public_param":
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,6 @@
|
|||||||
|
{
|
||||||
|
"slider_joint": {
|
||||||
|
"child":"slider",
|
||||||
|
"axis" : "x"
|
||||||
|
}
|
||||||
|
}
|
||||||
136
unilabos/device_mesh/devices/slide_w140/macro_device.xacro
Normal file
136
unilabos/device_mesh/devices/slide_w140/macro_device.xacro
Normal file
@@ -0,0 +1,136 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from slide.urdf | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||||
|
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
|
||||||
|
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||||
|
<robot xmlns:xacro="http://ros.org/wiki/xacro">
|
||||||
|
|
||||||
|
<xacro:macro name="slide_w140" params="mesh_path:='' length:=0.1 min_d:=0.1 max_d:=0.1 slider_d:=0.14 parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 r:=0" >
|
||||||
|
|
||||||
|
|
||||||
|
<joint name="${station_name}${device_name}base_link_joint" type="fixed">
|
||||||
|
<origin xyz="${x} ${y} ${z}" rpy="0 0 ${r}" />
|
||||||
|
<parent link="${parent_link}"/>
|
||||||
|
<child link="${station_name}${device_name}device_link"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="${station_name}${device_name}device_link"/>
|
||||||
|
<joint name="${station_name}${device_name}device_link_joint" type="fixed">
|
||||||
|
<origin xyz="${-min_d} 0 0" rpy="0 0 0" />
|
||||||
|
<parent link="${station_name}${device_name}device_link"/>
|
||||||
|
<child link="${station_name}${device_name}base_link"/>
|
||||||
|
<axis xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
<link name="${station_name}${device_name}base_link">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.00073944 -0.070732 0.035966"/>
|
||||||
|
<mass value="0.88697"/>
|
||||||
|
<inertia ixx="0.0019162" ixy="-2.5282E-11" ixz="6.1083E-07" iyy="0.00066605" iyz="1.4627E-08" izz="0.0019573"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/slide_w140/meshes/base_link.STL"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/slide_w140/meshes/base_link.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="${station_name}${device_name}slider">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0 -4.7184E-16 0.0095496"/>
|
||||||
|
<mass value="0.27913"/>
|
||||||
|
<inertia ixx="0.00048249" ixy="2.0866E-19" ixz="2.788E-21" iyy="0.00047014" iyz="1.1542E-17" izz="0.0009242"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slider.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slider.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="${station_name}${device_name}slider_joint" type="prismatic">
|
||||||
|
<origin rpy="0 0 0" xyz="${min_d + slider_d/2} 0 0.0475"/>
|
||||||
|
<parent link="${station_name}${device_name}base_link"/>
|
||||||
|
<child link="${station_name}${device_name}slider"/>
|
||||||
|
<axis xyz="1 0 0"/>
|
||||||
|
<limit effort="100" lower="0" upper="${length}" velocity="1"/>
|
||||||
|
</joint>
|
||||||
|
<link name="${station_name}${device_name}length">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.005 -9.3044E-10 0.02803"/>
|
||||||
|
<mass value="0.050532"/>
|
||||||
|
<inertia ixx="8.098E-05" ixy="-1.2574E-19" ixz="3.1207E-20" iyy="4.4152E-06" iyz="-3.1294E-13" izz="7.7407E-05"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/slide_w140/meshes/length.STL" scale="${(length + min_d + max_d + slider_d)} 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/slide_w140/meshes/length.STL" scale="${(length + min_d + max_d + slider_d)} 1 1"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="${station_name}${device_name}length_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="${station_name}${device_name}base_link"/>
|
||||||
|
<child link="${station_name}${device_name}length"/>
|
||||||
|
</joint>
|
||||||
|
<link name="${station_name}${device_name}slide_end">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.003 8.6044E-06 0.035593"/>
|
||||||
|
<mass value="0.055452"/>
|
||||||
|
<inertia ixx="9.9729E-05" ixy="-1.0228E-21" ixz="3.8344E-22" iyy="2.3158E-05" iyz="1.5613E-08" izz="7.6904E-05"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slide_end.STL"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="">
|
||||||
|
<color rgba="1 1 1 1"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slide_end.STL"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="${station_name}${device_name}slide_end_joint" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="${length + max_d +min_d + slider_d} 0 0"/>
|
||||||
|
<parent link="${station_name}${device_name}base_link"/>
|
||||||
|
<child link="${station_name}${device_name}slide_end"/>
|
||||||
|
</joint>
|
||||||
|
|
||||||
|
</xacro:macro>
|
||||||
|
</robot>
|
||||||
BIN
unilabos/device_mesh/devices/slide_w140/meshes/base_link.STL
Executable file
BIN
unilabos/device_mesh/devices/slide_w140/meshes/base_link.STL
Executable file
Binary file not shown.
BIN
unilabos/device_mesh/devices/slide_w140/meshes/base_link.fbx
Normal file
BIN
unilabos/device_mesh/devices/slide_w140/meshes/base_link.fbx
Normal file
Binary file not shown.
BIN
unilabos/device_mesh/devices/slide_w140/meshes/length.STL
Executable file
BIN
unilabos/device_mesh/devices/slide_w140/meshes/length.STL
Executable file
Binary file not shown.
BIN
unilabos/device_mesh/devices/slide_w140/meshes/length.fbx
Normal file
BIN
unilabos/device_mesh/devices/slide_w140/meshes/length.fbx
Normal file
Binary file not shown.
BIN
unilabos/device_mesh/devices/slide_w140/meshes/slide_end.STL
Executable file
BIN
unilabos/device_mesh/devices/slide_w140/meshes/slide_end.STL
Executable file
Binary file not shown.
BIN
unilabos/device_mesh/devices/slide_w140/meshes/slide_end.fbx
Normal file
BIN
unilabos/device_mesh/devices/slide_w140/meshes/slide_end.fbx
Normal file
Binary file not shown.
BIN
unilabos/device_mesh/devices/slide_w140/meshes/slider.STL
Executable file
BIN
unilabos/device_mesh/devices/slide_w140/meshes/slider.STL
Executable file
Binary file not shown.
BIN
unilabos/device_mesh/devices/slide_w140/meshes/slider.fbx
Normal file
BIN
unilabos/device_mesh/devices/slide_w140/meshes/slider.fbx
Normal file
Binary file not shown.
12
unilabos/device_mesh/devices/slide_w140/param_config.json
Normal file
12
unilabos/device_mesh/devices/slide_w140/param_config.json
Normal file
@@ -0,0 +1,12 @@
|
|||||||
|
{
|
||||||
|
"private_param":
|
||||||
|
{
|
||||||
|
"min_d": 0.1 ,
|
||||||
|
"max_d": 0.1 ,
|
||||||
|
"slider_d": 0.14
|
||||||
|
},
|
||||||
|
"public_param":
|
||||||
|
{
|
||||||
|
"length" :0.1
|
||||||
|
}
|
||||||
|
}
|
||||||
172
unilabos/device_mesh/resource_visalization.py
Normal file
172
unilabos/device_mesh/resource_visalization.py
Normal file
@@ -0,0 +1,172 @@
|
|||||||
|
import os
|
||||||
|
from pathlib import Path
|
||||||
|
from launch import LaunchService
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch_ros.actions import Node as nd
|
||||||
|
import xacro
|
||||||
|
from lxml import etree
|
||||||
|
|
||||||
|
from unilabos.registry.registry import lab_registry
|
||||||
|
|
||||||
|
|
||||||
|
class ResourceVisualization:
|
||||||
|
def __init__(self, device: dict, resource: dict, enable_rviz: bool = True):
|
||||||
|
"""初始化资源可视化类
|
||||||
|
|
||||||
|
该类用于将设备和资源的3D模型可视化展示。通过解析设备和资源的配置信息,
|
||||||
|
从注册表中获取对应的3D模型文件,并使用ROS2和RViz进行可视化。
|
||||||
|
|
||||||
|
Args:
|
||||||
|
device (dict): 设备配置字典,包含设备的类型、位置等信息
|
||||||
|
resource (dict): 资源配置字典,包含资源的类型、位置等信息
|
||||||
|
registry (dict): 注册表字典,包含设备和资源类型的注册信息
|
||||||
|
enable_rviz (bool, optional): 是否启用RViz可视化. Defaults to True.
|
||||||
|
"""
|
||||||
|
self.launch_service = LaunchService()
|
||||||
|
self.launch_description = LaunchDescription()
|
||||||
|
self.resource_dict = resource
|
||||||
|
self.resource_model = {}
|
||||||
|
self.resource_type = ['deck', 'plate', 'container']
|
||||||
|
self.mesh_path = Path(__file__).parent.absolute()
|
||||||
|
self.enable_rviz = enable_rviz
|
||||||
|
registry = lab_registry
|
||||||
|
|
||||||
|
self.srdf_str = '''
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<robot name="minimal">
|
||||||
|
|
||||||
|
</robot>
|
||||||
|
'''
|
||||||
|
self.robot_state_str= '''<?xml version="1.0" ?>
|
||||||
|
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="full_dev">
|
||||||
|
<link name="world"/>
|
||||||
|
</robot>
|
||||||
|
'''
|
||||||
|
self.root = etree.fromstring(self.robot_state_str)
|
||||||
|
|
||||||
|
xacro_uri = self.root.nsmap["xacro"]
|
||||||
|
|
||||||
|
# 遍历设备节点
|
||||||
|
for node in device.values():
|
||||||
|
if node['type'] == 'device' and node['class'] != '':
|
||||||
|
device_class = node['class']
|
||||||
|
# 检查设备类型是否在注册表中
|
||||||
|
if device_class not in registry.device_type_registry.keys():
|
||||||
|
raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
|
||||||
|
elif node['type'] in self.resource_type:
|
||||||
|
# print(registry.resource_type_registry)
|
||||||
|
resource_class = node['class']
|
||||||
|
if resource_class not in registry.resource_type_registry.keys():
|
||||||
|
raise ValueError(f"资源类型 {resource_class} 未在注册表中注册")
|
||||||
|
elif "model" in registry.resource_type_registry[resource_class].keys():
|
||||||
|
model_config = registry.resource_type_registry[resource_class]['model']
|
||||||
|
if model_config['type'] == 'resource':
|
||||||
|
self.resource_model[node['id']] = {
|
||||||
|
'mesh': f"{str(self.mesh_path)}/resources/{model_config['mesh']}",
|
||||||
|
'mesh_tf': model_config['mesh_tf']}
|
||||||
|
if model_config['children_mesh'] is not None:
|
||||||
|
self.resource_model[f"{node['id']}_"] = {
|
||||||
|
'mesh': f"{str(self.mesh_path)}/resources/{model_config['children_mesh']}",
|
||||||
|
'mesh_tf': model_config['children_mesh_tf']
|
||||||
|
}
|
||||||
|
elif model_config['type'] == 'device':
|
||||||
|
new_include = etree.SubElement(self.root, f"{{{xacro_uri}}}include")
|
||||||
|
new_include.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/macro_device.xacro")
|
||||||
|
new_dev = etree.SubElement(self.root, f"{{{xacro_uri}}}{model_config['mesh']}")
|
||||||
|
new_dev.set("parent_link", "world")
|
||||||
|
new_dev.set("mesh_path", str(self.mesh_path))
|
||||||
|
new_dev.set("device_name", node["id"]+"_")
|
||||||
|
new_dev.set("x",str(float(node["position"]["x"])/1000))
|
||||||
|
new_dev.set("y",str(float(node["position"]["y"])/1000))
|
||||||
|
new_dev.set("z",str(float(node["position"]["z"])/1000))
|
||||||
|
if "rotation" in node["config"]:
|
||||||
|
new_dev.set("r",str(float(node["config"]["rotation"]["z"])/1000))
|
||||||
|
else:
|
||||||
|
print("错误的注册表类型！")
|
||||||
|
re = etree.tostring(self.root, encoding="unicode")
|
||||||
|
doc = xacro.parse(re)
|
||||||
|
xacro.process_doc(doc)
|
||||||
|
self.urdf_str = doc.toxml()
|
||||||
|
|
||||||
|
|
||||||
|
def create_launch_description(self, urdf_str: str) -> LaunchDescription:
|
||||||
|
"""
|
||||||
|
创建launch描述，包含robot_state_publisher和move_group节点
|
||||||
|
|
||||||
|
Args:
|
||||||
|
urdf_str: URDF文本
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
LaunchDescription: launch描述对象
|
||||||
|
"""
|
||||||
|
|
||||||
|
|
||||||
|
# 解析URDF文件
|
||||||
|
robot_description = urdf_str
|
||||||
|
|
||||||
|
# 创建robot_state_publisher节点
|
||||||
|
robot_state_publisher = nd(
|
||||||
|
package='robot_state_publisher',
|
||||||
|
executable='robot_state_publisher',
|
||||||
|
name='robot_state_publisher',
|
||||||
|
output='screen',
|
||||||
|
parameters=[{
|
||||||
|
'robot_description': robot_description,
|
||||||
|
'use_sim_time': False
|
||||||
|
}]
|
||||||
|
)
|
||||||
|
|
||||||
|
# joint_state_publisher_node = nd(
|
||||||
|
# package='joint_state_publisher_gui', # 或 joint_state_publisher
|
||||||
|
# executable='joint_state_publisher_gui',
|
||||||
|
# name='joint_state_publisher',
|
||||||
|
# output='screen'
|
||||||
|
# )
|
||||||
|
# 创建move_group节点
|
||||||
|
move_group = nd(
|
||||||
|
package='moveit_ros_move_group',
|
||||||
|
executable='move_group',
|
||||||
|
output='screen',
|
||||||
|
parameters=[{
|
||||||
|
'robot_description': robot_description,
|
||||||
|
'robot_description_semantic': self.srdf_str,
|
||||||
|
'capabilities': '',
|
||||||
|
'disable_capabilities': '',
|
||||||
|
'monitor_dynamics': False,
|
||||||
|
'publish_monitored_planning_scene': True,
|
||||||
|
'publish_robot_description_semantic': True,
|
||||||
|
'publish_planning_scene': True,
|
||||||
|
'publish_geometry_updates': True,
|
||||||
|
'publish_state_updates': True,
|
||||||
|
'publish_transforms_updates': True,
|
||||||
|
}]
|
||||||
|
)
|
||||||
|
|
||||||
|
# 将节点添加到launch描述中
|
||||||
|
self.launch_description.add_action(robot_state_publisher)
|
||||||
|
# self.launch_description.add_action(joint_state_publisher_node)
|
||||||
|
self.launch_description.add_action(move_group)
|
||||||
|
|
||||||
|
# 如果启用RViz,添加RViz节点
|
||||||
|
if self.enable_rviz:
|
||||||
|
rviz_node = nd(
|
||||||
|
package='rviz2',
|
||||||
|
executable='rviz2',
|
||||||
|
name='rviz2',
|
||||||
|
arguments=['-d', f"{str(self.mesh_path)}/view_robot.rviz"],
|
||||||
|
output='screen'
|
||||||
|
)
|
||||||
|
self.launch_description.add_action(rviz_node)
|
||||||
|
|
||||||
|
return self.launch_description
|
||||||
|
|
||||||
|
def start(self) -> None:
|
||||||
|
"""
|
||||||
|
启动可视化服务
|
||||||
|
|
||||||
|
Args:
|
||||||
|
urdf_str: URDF文件路径
|
||||||
|
"""
|
||||||
|
launch_description = self.create_launch_description(self.urdf_str)
|
||||||
|
self.launch_service.include_launch_description(launch_description)
|
||||||
|
self.launch_service.run()
|
||||||
Binary file not shown.
|
After Width: | Height: | Size: 59 KiB |
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,22 @@
|
|||||||
|
{
|
||||||
|
"fileName": "generic_labware_tube_10_75",
|
||||||
|
"related": [
|
||||||
|
"generic_labware_0.5ml_screw_cap_tube",
|
||||||
|
"generic_labware_0.5ml_tube_rack",
|
||||||
|
"generic_labware_12_well_plate",
|
||||||
|
"sarstedt_14x200mm_tube",
|
||||||
|
"sarstedt_18x200mm_tube",
|
||||||
|
"generic_labware_1ml_tube_rack",
|
||||||
|
"generic_labware_24_well_plate",
|
||||||
|
"generic_labware_2ml_screw_cap_tube",
|
||||||
|
"generic_labware_5ml_screw_cap_tube",
|
||||||
|
"generic_labware_6_well_plate",
|
||||||
|
"generic_labware_96_well_square",
|
||||||
|
"generic_labware_96_well_pcr_plate_round",
|
||||||
|
"generic_labware_framedtiprack",
|
||||||
|
"generic_labware_plate_lid",
|
||||||
|
"generic_labware_reservoir",
|
||||||
|
"generic_labware_tip_box",
|
||||||
|
"generic_labware_tube_10_75"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,11 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="generic_labware_tube_10_75">
|
||||||
|
<link name='0_base_link'>
|
||||||
|
<visual name='visual'>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/0_base.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="clay" />
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
</robot>
|
||||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,86 @@
|
|||||||
|
{
|
||||||
|
"fileName": "tecan_nested_tip_rack",
|
||||||
|
"related": [
|
||||||
|
"tecan_techrom",
|
||||||
|
"tecan_holder_transfer_tool",
|
||||||
|
"tecan_fluent_9_grid_segment_cutout",
|
||||||
|
"tecan_fluent_centric_gripper",
|
||||||
|
"tecan_fluent_eccentric_gripper",
|
||||||
|
"tecan_evo100",
|
||||||
|
"tecan_fluent_mp_diti_nest_segment",
|
||||||
|
"tecan_fluent_4x100_trough",
|
||||||
|
"tecan_fluent_1080_extended",
|
||||||
|
"tecan_fluent_1_1_1000_trough",
|
||||||
|
"tecan_fluent_50ml_tube_runner_10_v2",
|
||||||
|
"tecan_fluent_15ml_tube_runner_16_v2",
|
||||||
|
"tecan_fluent_1_16_16_tube_runner",
|
||||||
|
"tecan_fluent_1.5ml_tube_runner_v2",
|
||||||
|
"tecan_fluent_1_24_10_tube_runner",
|
||||||
|
"tecan_fluent_1_24_13_tube_runner",
|
||||||
|
"tecan_fluent_3x320_reagent_trough_v2",
|
||||||
|
"tecan_fluent_32_tube_runner_v2",
|
||||||
|
"tecan_fluent_1_4_100_trough",
|
||||||
|
"tecan_fluent_2_grid_segment",
|
||||||
|
"tecan_fluent_2_4_100_trough_waste",
|
||||||
|
"tecan_fluent_3_grid_segment",
|
||||||
|
"tecan_fluent_nest_waste_segment_v2",
|
||||||
|
"tecan_fluent_320ml_reagent_trough",
|
||||||
|
"tecan_fluent_4_landscape_61mm_nest_segment",
|
||||||
|
"tecan_fluent_4_landscape_61mm_nest_segment_waste",
|
||||||
|
"tecan_fluent_4_landscape_7mm_nest_segment",
|
||||||
|
"tecan_fluent_4_landscape_7mm_nest_segment_waste",
|
||||||
|
"tecan_fluent_hotel_deck_4",
|
||||||
|
"tecan_fluent_480_extended",
|
||||||
|
"tecan_fluent_4x100_reagent_trough_v2",
|
||||||
|
"tecan_fluent_5_landscape_61mm_nest_segment",
|
||||||
|
"tecan_fluent_5_landscape_7mm_nest_segment",
|
||||||
|
"tecan_fluent_hotel_deck_5",
|
||||||
|
"tecan_fluent_6_grid_segment",
|
||||||
|
"tecan_fluent_nest_landscape_segment_v2",
|
||||||
|
"tecan_fluent_6_landscape_7mm_nest_segment",
|
||||||
|
"tecan_fluent_deck_segment_6_v2",
|
||||||
|
"tecan_fluent_fca_diti_segment_v2",
|
||||||
|
"tecan_fluent_6_nest_incubator",
|
||||||
|
"tecan_fluent_plate_nest",
|
||||||
|
"tecan_fluent_780_extended",
|
||||||
|
"tecan_fluent_plate_holder",
|
||||||
|
"tecan_fluent_8_grid_segment",
|
||||||
|
"tecan_fluent_8_grid_segment_evo",
|
||||||
|
"tecan_fluent_hotel_deck_9",
|
||||||
|
"tecan_carousel",
|
||||||
|
"tecan_carousel_stacker_10",
|
||||||
|
"tecan_carousel_stacker_25",
|
||||||
|
"tecan_carousel_stacker_6",
|
||||||
|
"tecan_fluent_coolheat_microplate_segment_v2",
|
||||||
|
"tecan_fluent_fca_diti_tray",
|
||||||
|
"tecan_fluent_trough_waste",
|
||||||
|
"tecan_fluent_id_left",
|
||||||
|
"tecan_fluent_id_middle",
|
||||||
|
"tecan_fluent_lower_6_grid_v2",
|
||||||
|
"tecan_fluent_mc384_nest",
|
||||||
|
"tecan_fluent_mca_44mm_nest",
|
||||||
|
"tecan_fluent_deck_segment_4_v2",
|
||||||
|
"tecan_fluent_mca_base_segment_384_v2",
|
||||||
|
"tecan_fluent_waste_module",
|
||||||
|
"tecan_fluent_reagent_block",
|
||||||
|
"tecan_fluent_tube_grippers",
|
||||||
|
"tecan_fluent_washstation_waste_v2",
|
||||||
|
"tecan_carrier_additive_trough_3_pce_max_100ml",
|
||||||
|
"tecan_carrier_384_well_mp_3_pos_accessible_roma",
|
||||||
|
"tecan_carrier_rack_3_diti_width_6",
|
||||||
|
"tecan_transport_box_diti_tray_1000ul",
|
||||||
|
"tecan_transport_box_diti_tray_200ul",
|
||||||
|
"tecan_magicprep_ngs_sample_deck",
|
||||||
|
"tecan_fluent_shelf_large",
|
||||||
|
"tecan_fluent_shelf_small",
|
||||||
|
"tecan_spacer_29_9_te_chrom",
|
||||||
|
"tecan_teshake_adapter_2",
|
||||||
|
"tecan_teshake_base",
|
||||||
|
"tecan_tevacs_base",
|
||||||
|
"tecan_tevacs_plate_park",
|
||||||
|
"tecan_tevacs_spacer",
|
||||||
|
"tecan_tevacs_vacuum",
|
||||||
|
"tecan_tip_box",
|
||||||
|
"tecan_nested_tip_rack"
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,11 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="tecan_nested_tip_rack">
|
||||||
|
<link name='plate_link'>
|
||||||
|
<visual name='visual'>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="meshes/plate.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="clay" />
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
</robot>
|
||||||
BIN
unilabos/device_mesh/resources/tecan_nested_tip_rack/plate.png
Normal file
BIN
unilabos/device_mesh/resources/tecan_nested_tip_rack/plate.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 128 KiB |
387
unilabos/device_mesh/view_robot.rviz
Normal file
387
unilabos/device_mesh/view_robot.rviz
Normal file
@@ -0,0 +1,387 @@
|
|||||||
|
Panels:
|
||||||
|
- Class: rviz_common/Displays
|
||||||
|
Help Height: 138
|
||||||
|
Name: Displays
|
||||||
|
Property Tree Widget:
|
||||||
|
Expanded:
|
||||||
|
- /Global Options1
|
||||||
|
- /TF1
|
||||||
|
- /TF1/Tree1
|
||||||
|
- /RobotModel1
|
||||||
|
- /PlanningScene1
|
||||||
|
- /PlanningScene1/Scene Geometry1
|
||||||
|
- /RobotState1
|
||||||
|
- /RobotState1/Links1
|
||||||
|
- /MotionPlanning1
|
||||||
|
- /MotionPlanning1/Scene Geometry1
|
||||||
|
- /MotionPlanning1/Scene Robot1
|
||||||
|
- /MotionPlanning1/Planning Request1
|
||||||
|
Splitter Ratio: 0.5
|
||||||
|
Tree Height: 345
|
||||||
|
- Class: rviz_common/Selection
|
||||||
|
Name: Selection
|
||||||
|
- Class: rviz_common/Tool Properties
|
||||||
|
Expanded:
|
||||||
|
- /2D Goal Pose1
|
||||||
|
- /Publish Point1
|
||||||
|
Name: Tool Properties
|
||||||
|
Splitter Ratio: 0.5886790156364441
|
||||||
|
- Class: rviz_common/Views
|
||||||
|
Expanded:
|
||||||
|
- /Current View1
|
||||||
|
Name: Views
|
||||||
|
Splitter Ratio: 0.5
|
||||||
|
Visualization Manager:
|
||||||
|
Class: ""
|
||||||
|
Displays:
|
||||||
|
- Alpha: 0.5
|
||||||
|
Cell Size: 1
|
||||||
|
Class: rviz_default_plugins/Grid
|
||||||
|
Color: 160; 160; 164
|
||||||
|
Enabled: true
|
||||||
|
Line Style:
|
||||||
|
Line Width: 0.029999999329447746
|
||||||
|
Value: Lines
|
||||||
|
Name: Grid
|
||||||
|
Normal Cell Count: 0
|
||||||
|
Offset:
|
||||||
|
X: 0
|
||||||
|
Y: 0
|
||||||
|
Z: 0
|
||||||
|
Plane: XY
|
||||||
|
Plane Cell Count: 10
|
||||||
|
Reference Frame: <Fixed Frame>
|
||||||
|
Value: true
|
||||||
|
- Class: rviz_default_plugins/TF
|
||||||
|
Enabled: false
|
||||||
|
Frame Timeout: 15
|
||||||
|
Frames:
|
||||||
|
All Enabled: false
|
||||||
|
Marker Scale: 1
|
||||||
|
Name: TF
|
||||||
|
Show Arrows: true
|
||||||
|
Show Axes: true
|
||||||
|
Show Names: false
|
||||||
|
Tree:
|
||||||
|
{}
|
||||||
|
Update Interval: 0
|
||||||
|
Value: false
|
||||||
|
- Alpha: 1
|
||||||
|
Class: rviz_default_plugins/RobotModel
|
||||||
|
Collision Enabled: false
|
||||||
|
Description File: ""
|
||||||
|
Description Source: Topic
|
||||||
|
Description Topic:
|
||||||
|
Depth: 5
|
||||||
|
Durability Policy: Volatile
|
||||||
|
History Policy: Keep Last
|
||||||
|
Reliability Policy: Reliable
|
||||||
|
Value: /robot_description
|
||||||
|
Enabled: false
|
||||||
|
Links:
|
||||||
|
All Links Enabled: true
|
||||||
|
Expand Joint Details: false
|
||||||
|
Expand Link Details: false
|
||||||
|
Expand Tree: false
|
||||||
|
Link Tree Style: Links in Alphabetic Order
|
||||||
|
Mass Properties:
|
||||||
|
Inertia: false
|
||||||
|
Mass: false
|
||||||
|
Name: RobotModel
|
||||||
|
TF Prefix: ""
|
||||||
|
Update Interval: 0
|
||||||
|
Value: false
|
||||||
|
Visual Enabled: true
|
||||||
|
- Class: moveit_rviz_plugin/PlanningScene
|
||||||
|
Enabled: false
|
||||||
|
Move Group Namespace: ""
|
||||||
|
Name: PlanningScene
|
||||||
|
Planning Scene Topic: /monitored_planning_scene
|
||||||
|
Robot Description: robot_description
|
||||||
|
Scene Geometry:
|
||||||
|
Scene Alpha: 0.8999999761581421
|
||||||
|
Scene Color: 50; 230; 50
|
||||||
|
Scene Display Time: 0.009999999776482582
|
||||||
|
Show Scene Geometry: true
|
||||||
|
Voxel Coloring: Z-Axis
|
||||||
|
Voxel Rendering: Occupied Voxels
|
||||||
|
Scene Robot:
|
||||||
|
Attached Body Color: 150; 50; 150
|
||||||
|
Links:
|
||||||
|
All Links Enabled: true
|
||||||
|
Expand Joint Details: false
|
||||||
|
Expand Link Details: false
|
||||||
|
Expand Tree: false
|
||||||
|
Link Tree Style: Links in Alphabetic Order
|
||||||
|
Robot Alpha: 1
|
||||||
|
Show Robot Collision: false
|
||||||
|
Show Robot Visual: false
|
||||||
|
Value: false
|
||||||
|
- Attached Body Color: 150; 50; 150
|
||||||
|
Class: moveit_rviz_plugin/RobotState
|
||||||
|
Collision Enabled: false
|
||||||
|
Enabled: false
|
||||||
|
Links:
|
||||||
|
All Links Enabled: true
|
||||||
|
Expand Joint Details: false
|
||||||
|
Expand Link Details: false
|
||||||
|
Expand Tree: false
|
||||||
|
Link Tree Style: Links in Alphabetic Order
|
||||||
|
Name: RobotState
|
||||||
|
Robot Alpha: 1
|
||||||
|
Robot Description: robot_description
|
||||||
|
Robot State Topic: display_robot_state
|
||||||
|
Show All Links: true
|
||||||
|
Show Highlights: true
|
||||||
|
Value: false
|
||||||
|
Visual Enabled: true
|
||||||
|
- Acceleration_Scaling_Factor: 0.1
|
||||||
|
Class: moveit_rviz_plugin/MotionPlanning
|
||||||
|
Enabled: true
|
||||||
|
Move Group Namespace: ""
|
||||||
|
MoveIt_Allow_Approximate_IK: false
|
||||||
|
MoveIt_Allow_External_Program: false
|
||||||
|
MoveIt_Allow_Replanning: false
|
||||||
|
MoveIt_Allow_Sensor_Positioning: false
|
||||||
|
MoveIt_Planning_Attempts: 10
|
||||||
|
MoveIt_Planning_Time: 5
|
||||||
|
MoveIt_Use_Cartesian_Path: false
|
||||||
|
MoveIt_Use_Constraint_Aware_IK: false
|
||||||
|
MoveIt_Workspace:
|
||||||
|
Center:
|
||||||
|
X: 0
|
||||||
|
Y: 0
|
||||||
|
Z: 0
|
||||||
|
Size:
|
||||||
|
X: 2
|
||||||
|
Y: 2
|
||||||
|
Z: 2
|
||||||
|
Name: MotionPlanning
|
||||||
|
Planned Path:
|
||||||
|
Color Enabled: false
|
||||||
|
Interrupt Display: false
|
||||||
|
Links:
|
||||||
|
All Links Enabled: true
|
||||||
|
Expand Joint Details: false
|
||||||
|
Expand Link Details: false
|
||||||
|
Expand Tree: false
|
||||||
|
Link Tree Style: Links in Alphabetic Order
|
||||||
|
deck_device_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
deck_first_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
deck_fourth_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
deck_main_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
deck_second_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
deck_socketTypeGenericSbsFootprint:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
deck_socketTypeHEPAModule:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
deck_third_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
world:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Loop Animation: false
|
||||||
|
Robot Alpha: 0.5
|
||||||
|
Robot Color: 150; 50; 150
|
||||||
|
Show Robot Collision: false
|
||||||
|
Show Robot Visual: true
|
||||||
|
Show Trail: false
|
||||||
|
State Display Time: 3x
|
||||||
|
Trail Step Size: 1
|
||||||
|
Trajectory Topic: /display_planned_path
|
||||||
|
Use Sim Time: false
|
||||||
|
Planning Metrics:
|
||||||
|
Payload: 1
|
||||||
|
Show Joint Torques: false
|
||||||
|
Show Manipulability: false
|
||||||
|
Show Manipulability Index: false
|
||||||
|
Show Weight Limit: false
|
||||||
|
TextHeight: 0.07999999821186066
|
||||||
|
Planning Request:
|
||||||
|
Colliding Link Color: 255; 0; 0
|
||||||
|
Goal State Alpha: 1
|
||||||
|
Goal State Color: 250; 128; 0
|
||||||
|
Interactive Marker Size: 0
|
||||||
|
Joint Violation Color: 255; 0; 255
|
||||||
|
Planning Group: ""
|
||||||
|
Query Goal State: false
|
||||||
|
Query Start State: false
|
||||||
|
Show Workspace: false
|
||||||
|
Start State Alpha: 1
|
||||||
|
Start State Color: 0; 255; 0
|
||||||
|
Planning Scene Topic: /monitored_planning_scene
|
||||||
|
Robot Description: robot_description
|
||||||
|
Scene Geometry:
|
||||||
|
Scene Alpha: 0.8999999761581421
|
||||||
|
Scene Color: 50; 230; 50
|
||||||
|
Scene Display Time: 0.009999999776482582
|
||||||
|
Show Scene Geometry: true
|
||||||
|
Voxel Coloring: Z-Axis
|
||||||
|
Voxel Rendering: Occupied Voxels
|
||||||
|
Scene Robot:
|
||||||
|
Attached Body Color: 150; 50; 150
|
||||||
|
Links:
|
||||||
|
All Links Enabled: true
|
||||||
|
Expand Joint Details: false
|
||||||
|
Expand Link Details: false
|
||||||
|
Expand Tree: false
|
||||||
|
Link Tree Style: Links in Alphabetic Order
|
||||||
|
deck_device_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
deck_first_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
deck_fourth_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
deck_main_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
deck_second_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
deck_socketTypeGenericSbsFootprint:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
deck_socketTypeHEPAModule:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
deck_third_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
world:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Robot Alpha: 1
|
||||||
|
Show Robot Collision: false
|
||||||
|
Show Robot Visual: true
|
||||||
|
Value: true
|
||||||
|
Velocity_Scaling_Factor: 0.1
|
||||||
|
Enabled: true
|
||||||
|
Global Options:
|
||||||
|
Background Color: 48; 48; 48
|
||||||
|
Fixed Frame: world
|
||||||
|
Frame Rate: 30
|
||||||
|
Name: root
|
||||||
|
Tools:
|
||||||
|
- Class: rviz_default_plugins/Interact
|
||||||
|
Hide Inactive Objects: true
|
||||||
|
- Class: rviz_default_plugins/MoveCamera
|
||||||
|
- Class: rviz_default_plugins/Select
|
||||||
|
- Class: rviz_default_plugins/FocusCamera
|
||||||
|
- Class: rviz_default_plugins/Measure
|
||||||
|
Line color: 128; 128; 0
|
||||||
|
- Class: rviz_default_plugins/SetInitialPose
|
||||||
|
Covariance x: 0.25
|
||||||
|
Covariance y: 0.25
|
||||||
|
Covariance yaw: 0.06853891909122467
|
||||||
|
Topic:
|
||||||
|
Depth: 5
|
||||||
|
Durability Policy: Volatile
|
||||||
|
History Policy: Keep Last
|
||||||
|
Reliability Policy: Reliable
|
||||||
|
Value: /initialpose
|
||||||
|
- Class: rviz_default_plugins/SetGoal
|
||||||
|
Topic:
|
||||||
|
Depth: 5
|
||||||
|
Durability Policy: Volatile
|
||||||
|
History Policy: Keep Last
|
||||||
|
Reliability Policy: Reliable
|
||||||
|
Value: /goal_pose
|
||||||
|
- Class: rviz_default_plugins/PublishPoint
|
||||||
|
Single click: true
|
||||||
|
Topic:
|
||||||
|
Depth: 5
|
||||||
|
Durability Policy: Volatile
|
||||||
|
History Policy: Keep Last
|
||||||
|
Reliability Policy: Reliable
|
||||||
|
Value: /clicked_point
|
||||||
|
Transformation:
|
||||||
|
Current:
|
||||||
|
Class: rviz_default_plugins/TF
|
||||||
|
Value: true
|
||||||
|
Views:
|
||||||
|
Current:
|
||||||
|
Class: rviz_default_plugins/Orbit
|
||||||
|
Distance: 1.49553644657135
|
||||||
|
Enable Stereo Rendering:
|
||||||
|
Stereo Eye Separation: 0.05999999865889549
|
||||||
|
Stereo Focal Distance: 1
|
||||||
|
Swap Stereo Eyes: false
|
||||||
|
Value: false
|
||||||
|
Focal Point:
|
||||||
|
X: 1.0440783500671387
|
||||||
|
Y: 0.27775266766548157
|
||||||
|
Z: 0.42559614777565
|
||||||
|
Focal Shape Fixed Size: true
|
||||||
|
Focal Shape Size: 0.05000000074505806
|
||||||
|
Invert Z Axis: false
|
||||||
|
Name: Current View
|
||||||
|
Near Clip Distance: 0.009999999776482582
|
||||||
|
Pitch: 0.40479573607444763
|
||||||
|
Target Frame: <Fixed Frame>
|
||||||
|
Value: Orbit (rviz)
|
||||||
|
Yaw: 6.090748310089111
|
||||||
|
Saved: ~
|
||||||
|
Window Geometry:
|
||||||
|
Displays:
|
||||||
|
collapsed: false
|
||||||
|
Height: 1656
|
||||||
|
Hide Left Dock: false
|
||||||
|
Hide Right Dock: true
|
||||||
|
MotionPlanning:
|
||||||
|
collapsed: false
|
||||||
|
MotionPlanning - Trajectory Slider:
|
||||||
|
collapsed: false
|
||||||
|
QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000002510000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002cb0000037f000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000627000005dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
|
Selection:
|
||||||
|
collapsed: false
|
||||||
|
Tool Properties:
|
||||||
|
collapsed: false
|
||||||
|
Views:
|
||||||
|
collapsed: true
|
||||||
|
Width: 2518
|
||||||
|
X: 385
|
||||||
|
Y: 120
|
||||||
@@ -77,9 +77,6 @@ class UrArmTask():
|
|||||||
if n > retry:
|
if n > retry:
|
||||||
raise Exception('Can not connect to the arm info server!')
|
raise Exception('Can not connect to the arm info server!')
|
||||||
|
|
||||||
self.pose_data = {}
|
|
||||||
self.pose_file = 'C:\\auto\\unilabos\\unilabos\\devices\\agv\\pose.json'
|
|
||||||
self.reload_pose()
|
|
||||||
self.dash_c.stop()
|
self.dash_c.stop()
|
||||||
|
|
||||||
def arm_init(self):
|
def arm_init(self):
|
||||||
|
|||||||
0
unilabos/devices/ros_dev/__init__.py
Normal file
0
unilabos/devices/ros_dev/__init__.py
Normal file
208
unilabos/devices/ros_dev/liquid_handler_joint_publisher.py
Normal file
208
unilabos/devices/ros_dev/liquid_handler_joint_publisher.py
Normal file
@@ -0,0 +1,208 @@
|
|||||||
|
import copy
|
||||||
|
import rclpy
|
||||||
|
import json
|
||||||
|
import time
|
||||||
|
from rclpy.executors import MultiThreadedExecutor
|
||||||
|
from rclpy.action import ActionServer
|
||||||
|
from sensor_msgs.msg import JointState
|
||||||
|
from ilabos_msgs.action import SendCmd
|
||||||
|
from rclpy.action.server import ServerGoalHandle
|
||||||
|
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
|
||||||
|
from tf_transformations import quaternion_from_euler
|
||||||
|
from tf2_ros import TransformBroadcaster, Buffer, TransformListener
|
||||||
|
|
||||||
|
|
||||||
|
class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
|
||||||
|
def __init__(self,device_id:str, joint_config:dict, lh_id:str,resource_tracker, rate=50):
|
||||||
|
super().__init__(
|
||||||
|
driver_instance=self,
|
||||||
|
device_id=device_id,
|
||||||
|
status_types={},
|
||||||
|
action_value_mappings={},
|
||||||
|
hardware_interface={},
|
||||||
|
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']]
|
||||||
|
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.j_action = ActionServer(
|
||||||
|
self,
|
||||||
|
SendCmd,
|
||||||
|
"joint",
|
||||||
|
self.lh_joint_action_callback,
|
||||||
|
result_timeout=5000
|
||||||
|
)
|
||||||
|
|
||||||
|
def lh_joint_action_callback(self,goal_handle: ServerGoalHandle):
|
||||||
|
"""Move a single joint
|
||||||
|
|
||||||
|
Args:
|
||||||
|
command: A JSON-formatted string that includes joint_name, speed, position
|
||||||
|
|
||||||
|
joint_name (str): The name of the joint to move
|
||||||
|
speed (float): The speed of the movement, speed > 0
|
||||||
|
position (float): The position to move to
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
None
|
||||||
|
"""
|
||||||
|
result = SendCmd.Result()
|
||||||
|
cmd_str = str(goal_handle.request.command).replace('\'','\"')
|
||||||
|
# goal_handle.execute()
|
||||||
|
|
||||||
|
try:
|
||||||
|
cmd_dict = json.loads(cmd_str)
|
||||||
|
self.move_joints(**cmd_dict)
|
||||||
|
result.success = True
|
||||||
|
goal_handle.succeed()
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
print(e)
|
||||||
|
goal_handle.abort()
|
||||||
|
result.success = False
|
||||||
|
|
||||||
|
return result
|
||||||
|
def inverse_kinematics(self, x, y, z,
|
||||||
|
x_joint:dict,
|
||||||
|
y_joint:dict,
|
||||||
|
z_joint:dict ):
|
||||||
|
"""
|
||||||
|
将x、y、z坐标转换为对应关节的位置
|
||||||
|
|
||||||
|
Args:
|
||||||
|
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
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
dict: 关节名称和对应位置的字典
|
||||||
|
"""
|
||||||
|
joint_positions = copy.deepcopy(self.j_msg.position)
|
||||||
|
|
||||||
|
# 处理x轴关节
|
||||||
|
for joint_name, config in x_joint.items():
|
||||||
|
index = self.j_msg.name.index(f"{self.lh_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}")
|
||||||
|
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}")
|
||||||
|
joint_positions[index] = z * config["factor"] + config["offset"]
|
||||||
|
|
||||||
|
|
||||||
|
return joint_positions
|
||||||
|
|
||||||
|
|
||||||
|
def move_joints(self, resource_name, link_name, speed, x_joint=None, y_joint=None, z_joint=None):
|
||||||
|
|
||||||
|
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
|
||||||
|
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]
|
||||||
|
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]
|
||||||
|
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]
|
||||||
|
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)
|
||||||
|
|
||||||
|
loop_flag = 0
|
||||||
|
|
||||||
|
|
||||||
|
while loop_flag < len(self.joint_config['joint_names']):
|
||||||
|
loop_flag = 0
|
||||||
|
for i in range(len(self.joint_config['joint_names'])):
|
||||||
|
distance = joint_positions_target[i] - self.j_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
|
||||||
|
else :
|
||||||
|
self.j_msg.position[i] = joint_positions_target[i]
|
||||||
|
loop_flag += 1
|
||||||
|
|
||||||
|
|
||||||
|
# 发布关节状态
|
||||||
|
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)
|
||||||
|
|
||||||
|
def main():
|
||||||
|
|
||||||
|
pass
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
@@ -19,7 +19,9 @@ gripper.mock:
|
|||||||
result:
|
result:
|
||||||
position: position
|
position: position
|
||||||
effort: torque
|
effort: torque
|
||||||
|
model:
|
||||||
|
type: device
|
||||||
|
mesh: opentrons_liquid_handler
|
||||||
|
|
||||||
gripper.misumi_rz:
|
gripper.misumi_rz:
|
||||||
description: Misumi RZ gripper
|
description: Misumi RZ gripper
|
||||||
|
|||||||
5
unilabos/registry/devices/sim_nodes.yaml
Normal file
5
unilabos/registry/devices/sim_nodes.yaml
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
lh_joint_publisher:
|
||||||
|
class:
|
||||||
|
module: unilabos.devices.ros_dev.liquid_handler_joint_publisher:LiquidHandlerJointPublisher
|
||||||
|
type: ros2
|
||||||
|
|
||||||
@@ -3,3 +3,6 @@ OTDeck:
|
|||||||
class:
|
class:
|
||||||
module: pylabrobot.resources.opentrons.deck:OTDeck
|
module: pylabrobot.resources.opentrons.deck:OTDeck
|
||||||
type: pylabrobot
|
type: pylabrobot
|
||||||
|
model:
|
||||||
|
type: device
|
||||||
|
mesh: opentrons_liquid_handler
|
||||||
@@ -51,6 +51,12 @@ nest_96_wellplate_100ul_pcr_full_skirt:
|
|||||||
class:
|
class:
|
||||||
module: pylabrobot.resources.opentrons.plates:nest_96_wellplate_100ul_pcr_full_skirt
|
module: pylabrobot.resources.opentrons.plates:nest_96_wellplate_100ul_pcr_full_skirt
|
||||||
type: pylabrobot
|
type: pylabrobot
|
||||||
|
model:
|
||||||
|
type: resource
|
||||||
|
mesh: tecan_nested_tip_rack/meshes/plate.stl
|
||||||
|
mesh_tf: [0.064, 0.043, 0, -1.5708, 0, 1.5708]
|
||||||
|
children_mesh: generic_labware_tube_10_75/meshes/0_base.stl
|
||||||
|
children_mesh_tf: [0.0018, 0.0018, 0, -1.5708,0, 0]
|
||||||
|
|
||||||
appliedbiosystemsmicroamp_384_wellplate_40ul:
|
appliedbiosystemsmicroamp_384_wellplate_40ul:
|
||||||
description: Applied Biosystems microamp 384 wellplate 40ul
|
description: Applied Biosystems microamp 384 wellplate 40ul
|
||||||
|
|||||||
@@ -63,6 +63,12 @@ opentrons_96_filtertiprack_1000ul:
|
|||||||
class:
|
class:
|
||||||
module: pylabrobot.resources.opentrons.tip_racks:opentrons_96_filtertiprack_1000ul
|
module: pylabrobot.resources.opentrons.tip_racks:opentrons_96_filtertiprack_1000ul
|
||||||
type: pylabrobot
|
type: pylabrobot
|
||||||
|
model:
|
||||||
|
type: resource
|
||||||
|
mesh: tecan_nested_tip_rack/meshes/plate.stl
|
||||||
|
mesh_tf: [0.064, 0.043, 0, -1.5708, 0, 1.5708]
|
||||||
|
children_mesh: generic_labware_tube_10_75/meshes/0_base.stl
|
||||||
|
children_mesh_tf: [0.0018, 0.0018, 0, -1.5708,0, 0]
|
||||||
|
|
||||||
opentrons_96_filtertiprack_20ul:
|
opentrons_96_filtertiprack_20ul:
|
||||||
description: Opentrons 96 filtertiprack 20ul
|
description: Opentrons 96 filtertiprack 20ul
|
||||||
|
|||||||
@@ -2,9 +2,13 @@ import copy
|
|||||||
import json
|
import json
|
||||||
import os
|
import os
|
||||||
import threading
|
import threading
|
||||||
|
import time
|
||||||
from typing import Optional, Dict, Any, List
|
from typing import Optional, Dict, Any, List
|
||||||
|
|
||||||
import rclpy
|
import rclpy
|
||||||
|
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
|
||||||
|
from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
|
||||||
|
from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
|
||||||
from unilabos_msgs.msg import Resource # type: ignore
|
from unilabos_msgs.msg import Resource # type: ignore
|
||||||
from unilabos_msgs.srv import ResourceAdd, SerialCommand # type: ignore
|
from unilabos_msgs.srv import ResourceAdd, SerialCommand # type: ignore
|
||||||
from rclpy.executors import MultiThreadedExecutor
|
from rclpy.executors import MultiThreadedExecutor
|
||||||
@@ -44,13 +48,15 @@ def main(
|
|||||||
graph: Optional[Dict[str, Any]] = None,
|
graph: Optional[Dict[str, Any]] = None,
|
||||||
controllers_config: Dict[str, Any] = {},
|
controllers_config: Dict[str, Any] = {},
|
||||||
bridges: List[Any] = [],
|
bridges: List[Any] = [],
|
||||||
|
visual: str = "None",
|
||||||
|
resources_mesh_config: dict = {},
|
||||||
args: List[str] = ["--log-level", "debug"],
|
args: List[str] = ["--log-level", "debug"],
|
||||||
discovery_interval: float = 5.0,
|
discovery_interval: float = 5.0,
|
||||||
) -> None:
|
) -> None:
|
||||||
"""主函数"""
|
"""主函数"""
|
||||||
rclpy.init(args=args)
|
|
||||||
rclpy.__executor = executor = MultiThreadedExecutor()
|
|
||||||
|
|
||||||
|
rclpy.init(args=args)
|
||||||
|
executor = rclpy.__executor = MultiThreadedExecutor()
|
||||||
# 创建主机节点
|
# 创建主机节点
|
||||||
host_node = HostNode(
|
host_node = HostNode(
|
||||||
"host_node",
|
"host_node",
|
||||||
@@ -62,11 +68,26 @@ def main(
|
|||||||
discovery_interval,
|
discovery_interval,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
if visual != "None":
|
||||||
|
resource_mesh_manager = ResourceMeshManager(
|
||||||
|
resources_mesh_config,
|
||||||
|
resources_config,
|
||||||
|
resource_tracker= DeviceNodeResourceTracker(),
|
||||||
|
device_id = 'resource_mesh_manager',
|
||||||
|
)
|
||||||
|
joint_republisher = JointRepublisher(
|
||||||
|
'joint_republisher',
|
||||||
|
DeviceNodeResourceTracker()
|
||||||
|
)
|
||||||
|
|
||||||
|
executor.add_node(resource_mesh_manager)
|
||||||
|
executor.add_node(joint_republisher)
|
||||||
|
|
||||||
thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread")
|
thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread")
|
||||||
thread.start()
|
thread.start()
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
input()
|
time.sleep(1)
|
||||||
|
|
||||||
|
|
||||||
def slave(
|
def slave(
|
||||||
@@ -78,8 +99,11 @@ def slave(
|
|||||||
args: List[str] = ["--log-level", "debug"],
|
args: List[str] = ["--log-level", "debug"],
|
||||||
) -> None:
|
) -> None:
|
||||||
"""从节点函数"""
|
"""从节点函数"""
|
||||||
rclpy.init(args=args)
|
if not rclpy.ok():
|
||||||
rclpy.__executor = executor = MultiThreadedExecutor()
|
rclpy.init(args=args)
|
||||||
|
executor = rclpy.__executor
|
||||||
|
if not executor:
|
||||||
|
executor = rclpy.__executor = MultiThreadedExecutor()
|
||||||
devices_config_copy = copy.deepcopy(devices_config)
|
devices_config_copy = copy.deepcopy(devices_config)
|
||||||
for device_id, device_config in devices_config.items():
|
for device_id, device_config in devices_config.items():
|
||||||
d = initialize_device_from_dict(device_id, device_config)
|
d = initialize_device_from_dict(device_id, device_config)
|
||||||
@@ -120,7 +144,7 @@ def slave(
|
|||||||
logger.info(f"Slave resource added.")
|
logger.info(f"Slave resource added.")
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
input()
|
time.sleep(1)
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
main()
|
main()
|
||||||
|
|||||||
60
unilabos/ros/nodes/presets/joint_republisher.py
Normal file
60
unilabos/ros/nodes/presets/joint_republisher.py
Normal file
@@ -0,0 +1,60 @@
|
|||||||
|
import rclpy,json
|
||||||
|
from rclpy.node import Node
|
||||||
|
from sensor_msgs.msg import JointState
|
||||||
|
from std_msgs.msg import String
|
||||||
|
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||||
|
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
|
||||||
|
|
||||||
|
class JointRepublisher(BaseROS2DeviceNode):
|
||||||
|
def __init__(self,device_id,resource_tracker):
|
||||||
|
super().__init__(
|
||||||
|
driver_instance=self,
|
||||||
|
device_id=device_id,
|
||||||
|
status_types={},
|
||||||
|
action_value_mappings={},
|
||||||
|
hardware_interface={},
|
||||||
|
print_publish=False,
|
||||||
|
resource_tracker=resource_tracker,
|
||||||
|
)
|
||||||
|
|
||||||
|
# print('-'*20,device_id)
|
||||||
|
self.joint_repub = self.create_publisher(String,f'joint_state_repub',10)
|
||||||
|
# 创建订阅者
|
||||||
|
self.create_subscription(
|
||||||
|
JointState,
|
||||||
|
'/joint_states',
|
||||||
|
self.listener_callback,
|
||||||
|
10,
|
||||||
|
callback_group=ReentrantCallbackGroup()
|
||||||
|
)
|
||||||
|
self.msg = String()
|
||||||
|
|
||||||
|
def listener_callback(self, msg:JointState):
|
||||||
|
|
||||||
|
try:
|
||||||
|
json_dict = {}
|
||||||
|
json_dict["name"] = list(msg.name)
|
||||||
|
json_dict["position"] = list(msg.position)
|
||||||
|
json_dict["velocity"] = list(msg.velocity)
|
||||||
|
json_dict["effort"] = list(msg.effort)
|
||||||
|
|
||||||
|
self.msg.data = str(json_dict)
|
||||||
|
self.joint_repub.publish(self.msg)
|
||||||
|
# print('-'*20)
|
||||||
|
# print(self.msg.data)
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
print(e)
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
|
||||||
|
rclpy.init()
|
||||||
|
subscriber = JointRepublisher()
|
||||||
|
rclpy.spin(subscriber)
|
||||||
|
subscriber.destroy_node()
|
||||||
|
rclpy.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
||||||
956
unilabos/ros/nodes/presets/resource_mesh_manager.py
Normal file
956
unilabos/ros/nodes/presets/resource_mesh_manager.py
Normal file
@@ -0,0 +1,956 @@
|
|||||||
|
import time
|
||||||
|
import rclpy,json
|
||||||
|
from rclpy.node import Node
|
||||||
|
from std_msgs.msg import String,Header
|
||||||
|
import numpy as np
|
||||||
|
from moveit_msgs.srv import GetPlanningScene, ApplyPlanningScene
|
||||||
|
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||||
|
from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy
|
||||||
|
from moveit_msgs.msg import CollisionObject, AttachedCollisionObject, AllowedCollisionEntry
|
||||||
|
from shape_msgs.msg import Mesh, MeshTriangle, SolidPrimitive
|
||||||
|
from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion, TransformStamped
|
||||||
|
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||||
|
from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy, QoSHistoryPolicy
|
||||||
|
from rclpy.task import Future
|
||||||
|
import copy
|
||||||
|
from typing import Tuple, Optional, Union, Any, List
|
||||||
|
from tf_transformations import quaternion_from_euler
|
||||||
|
from tf2_ros import TransformBroadcaster, Buffer, TransformListener
|
||||||
|
from rclpy.action import ActionServer
|
||||||
|
from unilabos_msgs.action import SendCmd
|
||||||
|
from rclpy.action.server import ServerGoalHandle
|
||||||
|
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode,DeviceNodeResourceTracker
|
||||||
|
|
||||||
|
class ResourceMeshManager(BaseROS2DeviceNode):
|
||||||
|
def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager", rate=50):
|
||||||
|
"""初始化资源网格管理器节点
|
||||||
|
|
||||||
|
Args:
|
||||||
|
resource_model (dict): 资源模型字典,包含资源的3D模型信息
|
||||||
|
resource_config (dict): 资源配置字典,包含资源的配置信息
|
||||||
|
device_id (str): 节点名称
|
||||||
|
"""
|
||||||
|
super().__init__(
|
||||||
|
driver_instance=self,
|
||||||
|
device_id=device_id,
|
||||||
|
status_types={},
|
||||||
|
action_value_mappings={},
|
||||||
|
hardware_interface={},
|
||||||
|
print_publish=False,
|
||||||
|
resource_tracker=resource_tracker,
|
||||||
|
)
|
||||||
|
|
||||||
|
self.resource_model = resource_model
|
||||||
|
self.resource_config_dict = {item['id']: item for item in resource_config}
|
||||||
|
self.move_group_ready = False
|
||||||
|
self.resource_tf_dict = {}
|
||||||
|
self.tf_broadcaster = TransformBroadcaster(self)
|
||||||
|
self.tf_buffer = Buffer()
|
||||||
|
self.tf_listener = TransformListener(self.tf_buffer, self)
|
||||||
|
self.rate = rate
|
||||||
|
self.zero_count = 0
|
||||||
|
|
||||||
|
self.old_resource_pose = {}
|
||||||
|
self.__planning_scene = None
|
||||||
|
self.__old_planning_scene = None
|
||||||
|
self.__old_allowed_collision_matrix = None
|
||||||
|
|
||||||
|
|
||||||
|
callback_group = ReentrantCallbackGroup()
|
||||||
|
self._get_planning_scene_service = self.create_client(
|
||||||
|
srv_type=GetPlanningScene,
|
||||||
|
srv_name="/get_planning_scene",
|
||||||
|
qos_profile=QoSProfile(
|
||||||
|
durability=QoSDurabilityPolicy.VOLATILE,
|
||||||
|
reliability=QoSReliabilityPolicy.RELIABLE,
|
||||||
|
history=QoSHistoryPolicy.KEEP_LAST,
|
||||||
|
depth=1,
|
||||||
|
),
|
||||||
|
callback_group=callback_group,
|
||||||
|
)
|
||||||
|
|
||||||
|
# Create a service for applying the planning scene
|
||||||
|
self._apply_planning_scene_service = self.create_client(
|
||||||
|
srv_type=ApplyPlanningScene,
|
||||||
|
srv_name="/apply_planning_scene",
|
||||||
|
qos_profile=QoSProfile(
|
||||||
|
durability=QoSDurabilityPolicy.VOLATILE,
|
||||||
|
reliability=QoSReliabilityPolicy.RELIABLE,
|
||||||
|
history=QoSHistoryPolicy.KEEP_LAST,
|
||||||
|
depth=1,
|
||||||
|
),
|
||||||
|
callback_group=callback_group,
|
||||||
|
)
|
||||||
|
|
||||||
|
self.resource_pose_publisher = self.create_publisher(
|
||||||
|
String, f"resource_pose", 10
|
||||||
|
)
|
||||||
|
self.__collision_object_publisher = self.create_publisher(
|
||||||
|
CollisionObject, "/collision_object", 10
|
||||||
|
)
|
||||||
|
self.__attached_collision_object_publisher = self.create_publisher(
|
||||||
|
AttachedCollisionObject, "/attached_collision_object", 10
|
||||||
|
)
|
||||||
|
|
||||||
|
# 创建一个Action Server用于修改resource_tf_dict
|
||||||
|
self._action_server = ActionServer(
|
||||||
|
self,
|
||||||
|
SendCmd,
|
||||||
|
f"tf_update",
|
||||||
|
self.tf_update,
|
||||||
|
callback_group=callback_group
|
||||||
|
)
|
||||||
|
|
||||||
|
self.resource_mesh_setup()
|
||||||
|
self.create_timer(1/self.rate, self.publish_resource_tf)
|
||||||
|
self.create_timer(1/self.rate, self.check_resource_pose_changes)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def check_move_group_ready(self):
|
||||||
|
"""检查move_group节点是否已初始化完成"""
|
||||||
|
|
||||||
|
# 获取当前可用的节点列表
|
||||||
|
|
||||||
|
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:
|
||||||
|
if self._get_planning_scene_service.service_is_ready() and self._apply_planning_scene_service.service_is_ready() and tf_ready:
|
||||||
|
self.move_group_ready = True
|
||||||
|
self.publish_resource_tf()
|
||||||
|
self.add_resource_collision_meshes()
|
||||||
|
|
||||||
|
# time.sleep(1)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def resource_mesh_setup(self):
|
||||||
|
"""move_group初始化完成后的设置"""
|
||||||
|
self.get_logger().info('开始设置资源网格管理器')
|
||||||
|
#遍历resource_config中的资源配置，判断panent是否在resource_model中，
|
||||||
|
|
||||||
|
for resource_id, resource_config in self.resource_config_dict.items():
|
||||||
|
|
||||||
|
parent = resource_config['parent']
|
||||||
|
parent_link = 'world'
|
||||||
|
if parent in self.resource_model:
|
||||||
|
parent_link = parent
|
||||||
|
elif parent is None and resource_id in self.resource_model:
|
||||||
|
pass
|
||||||
|
elif parent not in self.resource_model and parent is not None and resource_config['class'] != '':
|
||||||
|
parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None","")
|
||||||
|
else:
|
||||||
|
continue
|
||||||
|
# 提取位置信息并转换单位
|
||||||
|
position = {
|
||||||
|
"x": float(resource_config['position']['x'])/1000,
|
||||||
|
"y": float(resource_config['position']['y'])/1000,
|
||||||
|
"z": float(resource_config['position']['z'])/1000
|
||||||
|
}
|
||||||
|
|
||||||
|
rotation_dict = {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
|
||||||
|
if 'rotation' in resource_config['config']:
|
||||||
|
rotation_dict = resource_config['config']['rotation']
|
||||||
|
|
||||||
|
# 从欧拉角转换为四元数
|
||||||
|
q = quaternion_from_euler(
|
||||||
|
float(rotation_dict['x']),
|
||||||
|
float(rotation_dict['y']),
|
||||||
|
float(rotation_dict['z'])
|
||||||
|
)
|
||||||
|
# print("-"*20)
|
||||||
|
# print(f"resource_id: {resource_id}")
|
||||||
|
# print(f"parent: {parent}")
|
||||||
|
# print(f"resource_config: {resource_config}")
|
||||||
|
# print(f"parent_link: {parent_link}")
|
||||||
|
# print("-"*20)
|
||||||
|
rotation = {
|
||||||
|
"x": q[0],
|
||||||
|
"y": q[1],
|
||||||
|
"z": q[2],
|
||||||
|
"w": q[3]
|
||||||
|
}
|
||||||
|
|
||||||
|
# 更新资源TF字典
|
||||||
|
self.resource_tf_dict[resource_id] = {
|
||||||
|
"parent": parent_link,
|
||||||
|
"position": position,
|
||||||
|
"rotation": rotation
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def publish_resource_tf(self):
|
||||||
|
"""
|
||||||
|
发布资源之间的TF关系
|
||||||
|
|
||||||
|
遍历self.resource_tf_dict中的每个元素，根据key，parent，以及position和rotation，
|
||||||
|
发布key和parent之间的tf关系
|
||||||
|
"""
|
||||||
|
|
||||||
|
transforms = []
|
||||||
|
|
||||||
|
# 遍历资源TF字典
|
||||||
|
resource_tf_dict = copy.deepcopy(self.resource_tf_dict)
|
||||||
|
for resource_id, tf_info in resource_tf_dict.items():
|
||||||
|
parent = tf_info['parent']
|
||||||
|
position = tf_info['position']
|
||||||
|
rotation = tf_info['rotation']
|
||||||
|
|
||||||
|
# 创建静态变换消息
|
||||||
|
|
||||||
|
transform = TransformStamped()
|
||||||
|
transform.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
transform.header.frame_id = parent
|
||||||
|
transform.child_frame_id = resource_id
|
||||||
|
|
||||||
|
# 设置位置
|
||||||
|
transform.transform.translation.x = float(position['x'])
|
||||||
|
transform.transform.translation.y = float(position['y'])
|
||||||
|
transform.transform.translation.z = float(position['z'])
|
||||||
|
|
||||||
|
# 设置旋转
|
||||||
|
transform.transform.rotation.x = rotation['x']
|
||||||
|
transform.transform.rotation.y = rotation['y']
|
||||||
|
transform.transform.rotation.z = rotation['z']
|
||||||
|
transform.transform.rotation.w = rotation['w']
|
||||||
|
|
||||||
|
transforms.append(transform)
|
||||||
|
|
||||||
|
# 一次性发布所有静态变换
|
||||||
|
if transforms:
|
||||||
|
self.tf_broadcaster.sendTransform(transforms)
|
||||||
|
# self.check_resource_pose_changes()
|
||||||
|
# self.get_logger().info(f'已发布 {len(transforms)} 个资源TF关系')
|
||||||
|
|
||||||
|
|
||||||
|
def check_resource_pose_changes(self):
|
||||||
|
"""
|
||||||
|
遍历资源TF字典，计算每个资源相对于world的变换，
|
||||||
|
与旧的位姿比较，记录发生变化的资源，并更新旧位姿记录。
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
dict: 包含发生位姿变化的资源ID及其新位姿
|
||||||
|
"""
|
||||||
|
if not self.move_group_ready:
|
||||||
|
self.check_move_group_ready()
|
||||||
|
return
|
||||||
|
changed_poses = {}
|
||||||
|
resource_tf_dict = copy.deepcopy(self.resource_tf_dict)
|
||||||
|
for resource_id in resource_tf_dict.keys():
|
||||||
|
try:
|
||||||
|
# 获取从resource_id到world的转换
|
||||||
|
|
||||||
|
transform = self.tf_buffer.lookup_transform(
|
||||||
|
"world",
|
||||||
|
resource_id,
|
||||||
|
rclpy.time.Time(seconds=0),
|
||||||
|
rclpy.duration.Duration(seconds=5)
|
||||||
|
)
|
||||||
|
|
||||||
|
# 提取当前位姿信息
|
||||||
|
current_pose = {
|
||||||
|
"position": {
|
||||||
|
"x": transform.transform.translation.x,
|
||||||
|
"y": transform.transform.translation.y,
|
||||||
|
"z": transform.transform.translation.z
|
||||||
|
},
|
||||||
|
"rotation": {
|
||||||
|
"x": transform.transform.rotation.x,
|
||||||
|
"y": transform.transform.rotation.y,
|
||||||
|
"z": transform.transform.rotation.z,
|
||||||
|
"w": transform.transform.rotation.w
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
# 检查是否存在旧位姿记录
|
||||||
|
if resource_id not in self.old_resource_pose:
|
||||||
|
# 如果没有旧记录，则认为是新资源，记录变化
|
||||||
|
changed_poses[resource_id] = current_pose
|
||||||
|
self.old_resource_pose[resource_id] = current_pose
|
||||||
|
else:
|
||||||
|
# 比较当前位姿与旧位姿
|
||||||
|
old_pose = self.old_resource_pose[resource_id]
|
||||||
|
if (not self._is_pose_equal(current_pose, old_pose)):
|
||||||
|
# 如果位姿发生变化，记录新位姿
|
||||||
|
changed_poses[resource_id] = current_pose
|
||||||
|
self.old_resource_pose[resource_id] = current_pose
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().warning(f"获取资源 {resource_id} 的世界坐标变换失败: {e}")
|
||||||
|
|
||||||
|
if changed_poses != {}:
|
||||||
|
self.zero_count = 0
|
||||||
|
changed_poses_msg = String()
|
||||||
|
changed_poses_msg.data = json.dumps(changed_poses)
|
||||||
|
self.resource_pose_publisher.publish(changed_poses_msg)
|
||||||
|
else:
|
||||||
|
if self.zero_count > self.rate:
|
||||||
|
self.zero_count = 0
|
||||||
|
changed_poses_msg = String()
|
||||||
|
changed_poses_msg.data = json.dumps(changed_poses)
|
||||||
|
self.resource_pose_publisher.publish(changed_poses_msg)
|
||||||
|
self.zero_count += 1
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def _is_pose_equal(self, pose1, pose2, tolerance=1e-7):
|
||||||
|
"""
|
||||||
|
比较两个位姿是否相等（考虑浮点数精度）
|
||||||
|
|
||||||
|
Args:
|
||||||
|
pose1: 第一个位姿
|
||||||
|
pose2: 第二个位姿
|
||||||
|
tolerance: 浮点数比较的容差
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
bool: 如果位姿相等返回True，否则返回False
|
||||||
|
"""
|
||||||
|
# 比较位置
|
||||||
|
pos1 = pose1["position"]
|
||||||
|
pos2 = pose2["position"]
|
||||||
|
if (abs(pos1["x"] - pos2["x"]) > tolerance or
|
||||||
|
abs(pos1["y"] - pos2["y"]) > tolerance or
|
||||||
|
abs(pos1["z"] - pos2["z"]) > tolerance):
|
||||||
|
return False
|
||||||
|
|
||||||
|
# 比较旋转
|
||||||
|
rot1 = pose1["rotation"]
|
||||||
|
rot2 = pose2["rotation"]
|
||||||
|
if (abs(rot1["x"] - rot2["x"]) > tolerance or
|
||||||
|
abs(rot1["y"] - rot2["y"]) > tolerance or
|
||||||
|
abs(rot1["z"] - rot2["z"]) > tolerance or
|
||||||
|
abs(rot1["w"] - rot2["w"]) > tolerance):
|
||||||
|
return False
|
||||||
|
|
||||||
|
return True
|
||||||
|
|
||||||
|
def tf_update(self, goal_handle : ServerGoalHandle):
|
||||||
|
tf_update_msg = goal_handle.request
|
||||||
|
|
||||||
|
try:
|
||||||
|
cmd_dict = json.loads(tf_update_msg.command.replace("'",'"'))
|
||||||
|
for resource_id, target_parent in cmd_dict.items():
|
||||||
|
|
||||||
|
# 获取从resource_id到target_parent的转换
|
||||||
|
transform = self.tf_buffer.lookup_transform(
|
||||||
|
target_parent,
|
||||||
|
resource_id,
|
||||||
|
rclpy.time.Time(seconds=0)
|
||||||
|
)
|
||||||
|
|
||||||
|
# 提取转换中的位置和旋转信息
|
||||||
|
position = {
|
||||||
|
"x": transform.transform.translation.x,
|
||||||
|
"y": transform.transform.translation.y,
|
||||||
|
"z": transform.transform.translation.z
|
||||||
|
}
|
||||||
|
|
||||||
|
rotation = {
|
||||||
|
"x": transform.transform.rotation.x,
|
||||||
|
"y": transform.transform.rotation.y,
|
||||||
|
"z": transform.transform.rotation.z,
|
||||||
|
"w": transform.transform.rotation.w
|
||||||
|
}
|
||||||
|
|
||||||
|
self.resource_tf_dict[resource_id] = {
|
||||||
|
"parent": target_parent,
|
||||||
|
"position": position,
|
||||||
|
"rotation": rotation
|
||||||
|
}
|
||||||
|
|
||||||
|
self.attach_collision_object(id=resource_id,link_name=target_parent)
|
||||||
|
|
||||||
|
self.publish_resource_tf()
|
||||||
|
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().error(f"更新资源TF字典失败: {e}")
|
||||||
|
goal_handle.abort()
|
||||||
|
return SendCmd.Result(success=False)
|
||||||
|
goal_handle.succeed()
|
||||||
|
return SendCmd.Result(success=True)
|
||||||
|
|
||||||
|
|
||||||
|
def add_resource_collision_meshes(self):
|
||||||
|
"""
|
||||||
|
遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
|
||||||
|
|
||||||
|
该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径，
|
||||||
|
如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。
|
||||||
|
"""
|
||||||
|
self.get_logger().info('开始添加资源碰撞网格')
|
||||||
|
|
||||||
|
for resource_id, tf_info in self.resource_tf_dict.items():
|
||||||
|
|
||||||
|
if resource_id in self.resource_model:
|
||||||
|
# 获取位置信息
|
||||||
|
|
||||||
|
position = [
|
||||||
|
float(self.resource_model[resource_id]['mesh_tf'][0]),
|
||||||
|
float(self.resource_model[resource_id]['mesh_tf'][1]),
|
||||||
|
float(self.resource_model[resource_id]['mesh_tf'][2])
|
||||||
|
]
|
||||||
|
|
||||||
|
# 获取旋转信息并转换为四元数
|
||||||
|
|
||||||
|
q = quaternion_from_euler(
|
||||||
|
float(self.resource_model[resource_id]['mesh_tf'][3]),
|
||||||
|
float(self.resource_model[resource_id]['mesh_tf'][4]),
|
||||||
|
float(self.resource_model[resource_id]['mesh_tf'][5])
|
||||||
|
)
|
||||||
|
|
||||||
|
# 添加碰撞网格
|
||||||
|
self.add_collision_mesh(
|
||||||
|
filepath=self.resource_model[resource_id]['mesh'],
|
||||||
|
id=resource_id,
|
||||||
|
position=position,
|
||||||
|
quat_xyzw=q,
|
||||||
|
frame_id=resource_id
|
||||||
|
)
|
||||||
|
|
||||||
|
elif f"{tf_info['parent']}_" in self.resource_model:
|
||||||
|
# 获取资源的父级框架ID
|
||||||
|
id_ = f"{tf_info['parent']}_"
|
||||||
|
|
||||||
|
# 获取位置信息
|
||||||
|
position = [
|
||||||
|
float(self.resource_model[id_]['mesh_tf'][0]),
|
||||||
|
float(self.resource_model[id_]['mesh_tf'][1]),
|
||||||
|
float(self.resource_model[id_]['mesh_tf'][2])
|
||||||
|
]
|
||||||
|
|
||||||
|
# 获取旋转信息并转换为四元数
|
||||||
|
|
||||||
|
q = quaternion_from_euler(
|
||||||
|
float(self.resource_model[id_]['mesh_tf'][3]),
|
||||||
|
float(self.resource_model[id_]['mesh_tf'][4]),
|
||||||
|
float(self.resource_model[id_]['mesh_tf'][5])
|
||||||
|
)
|
||||||
|
|
||||||
|
# 添加碰撞网格
|
||||||
|
self.add_collision_mesh(
|
||||||
|
filepath=self.resource_model[id_]['mesh'],
|
||||||
|
id=resource_id,
|
||||||
|
position=position,
|
||||||
|
quat_xyzw=q,
|
||||||
|
frame_id=resource_id
|
||||||
|
)
|
||||||
|
time.sleep(0.03)
|
||||||
|
|
||||||
|
self.get_logger().info('资源碰撞网格添加完成')
|
||||||
|
|
||||||
|
|
||||||
|
def add_collision_primitive(
|
||||||
|
self,
|
||||||
|
id: str,
|
||||||
|
primitive_type: int,
|
||||||
|
dimensions: Tuple[float, float, float],
|
||||||
|
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||||||
|
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||||||
|
quat_xyzw: Optional[
|
||||||
|
Union[Quaternion, Tuple[float, float, float, float]]
|
||||||
|
] = None,
|
||||||
|
frame_id: Optional[str] = None,
|
||||||
|
operation: int = CollisionObject.ADD,
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Add collision object with a primitive geometry specified by its dimensions.
|
||||||
|
|
||||||
|
`primitive_type` can be one of the following:
|
||||||
|
- `SolidPrimitive.BOX`
|
||||||
|
- `SolidPrimitive.SPHERE`
|
||||||
|
- `SolidPrimitive.CYLINDER`
|
||||||
|
- `SolidPrimitive.CONE`
|
||||||
|
"""
|
||||||
|
|
||||||
|
if (pose is None) and (position is None or quat_xyzw is None):
|
||||||
|
raise ValueError(
|
||||||
|
"Either `pose` or `position` and `quat_xyzw` must be specified!"
|
||||||
|
)
|
||||||
|
|
||||||
|
if isinstance(pose, PoseStamped):
|
||||||
|
pose_stamped = pose
|
||||||
|
elif isinstance(pose, Pose):
|
||||||
|
pose_stamped = PoseStamped(
|
||||||
|
header=Header(
|
||||||
|
stamp=self.get_clock().now().to_msg(),
|
||||||
|
frame_id=(
|
||||||
|
frame_id if frame_id is not None else self.__base_link_name
|
||||||
|
),
|
||||||
|
),
|
||||||
|
pose=pose,
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
if not isinstance(position, Point):
|
||||||
|
position = Point(
|
||||||
|
x=float(position[0]), y=float(position[1]), z=float(position[2])
|
||||||
|
)
|
||||||
|
if not isinstance(quat_xyzw, Quaternion):
|
||||||
|
quat_xyzw = Quaternion(
|
||||||
|
x=float(quat_xyzw[0]),
|
||||||
|
y=float(quat_xyzw[1]),
|
||||||
|
z=float(quat_xyzw[2]),
|
||||||
|
w=float(quat_xyzw[3]),
|
||||||
|
)
|
||||||
|
pose_stamped = PoseStamped(
|
||||||
|
header=Header(
|
||||||
|
stamp=self.get_clock().now().to_msg(),
|
||||||
|
frame_id=(
|
||||||
|
frame_id if frame_id is not None else self.__base_link_name
|
||||||
|
),
|
||||||
|
),
|
||||||
|
pose=Pose(position=position, orientation=quat_xyzw),
|
||||||
|
)
|
||||||
|
|
||||||
|
msg = CollisionObject(
|
||||||
|
header=pose_stamped.header,
|
||||||
|
id=id,
|
||||||
|
operation=operation,
|
||||||
|
pose=pose_stamped.pose,
|
||||||
|
)
|
||||||
|
|
||||||
|
msg.primitives.append(
|
||||||
|
SolidPrimitive(type=primitive_type, dimensions=dimensions)
|
||||||
|
)
|
||||||
|
|
||||||
|
self.__collision_object_publisher.publish(msg)
|
||||||
|
|
||||||
|
def add_collision_box(
|
||||||
|
self,
|
||||||
|
id: str,
|
||||||
|
size: Tuple[float, float, float],
|
||||||
|
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||||||
|
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||||||
|
quat_xyzw: Optional[
|
||||||
|
Union[Quaternion, Tuple[float, float, float, float]]
|
||||||
|
] = None,
|
||||||
|
frame_id: Optional[str] = None,
|
||||||
|
operation: int = CollisionObject.ADD,
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Add collision object with a box geometry specified by its size.
|
||||||
|
"""
|
||||||
|
|
||||||
|
assert len(size) == 3, "Invalid size of the box!"
|
||||||
|
|
||||||
|
self.add_collision_primitive(
|
||||||
|
id=id,
|
||||||
|
primitive_type=SolidPrimitive.BOX,
|
||||||
|
dimensions=size,
|
||||||
|
pose=pose,
|
||||||
|
position=position,
|
||||||
|
quat_xyzw=quat_xyzw,
|
||||||
|
frame_id=frame_id,
|
||||||
|
operation=operation,
|
||||||
|
)
|
||||||
|
|
||||||
|
def add_collision_sphere(
|
||||||
|
self,
|
||||||
|
id: str,
|
||||||
|
radius: float,
|
||||||
|
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||||||
|
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||||||
|
quat_xyzw: Optional[
|
||||||
|
Union[Quaternion, Tuple[float, float, float, float]]
|
||||||
|
] = None,
|
||||||
|
frame_id: Optional[str] = None,
|
||||||
|
operation: int = CollisionObject.ADD,
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Add collision object with a sphere geometry specified by its radius.
|
||||||
|
"""
|
||||||
|
|
||||||
|
if quat_xyzw is None:
|
||||||
|
quat_xyzw = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
|
||||||
|
|
||||||
|
self.add_collision_primitive(
|
||||||
|
id=id,
|
||||||
|
primitive_type=SolidPrimitive.SPHERE,
|
||||||
|
dimensions=[
|
||||||
|
radius,
|
||||||
|
],
|
||||||
|
pose=pose,
|
||||||
|
position=position,
|
||||||
|
quat_xyzw=quat_xyzw,
|
||||||
|
frame_id=frame_id,
|
||||||
|
operation=operation,
|
||||||
|
)
|
||||||
|
|
||||||
|
def add_collision_cylinder(
|
||||||
|
self,
|
||||||
|
id: str,
|
||||||
|
height: float,
|
||||||
|
radius: float,
|
||||||
|
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||||||
|
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||||||
|
quat_xyzw: Optional[
|
||||||
|
Union[Quaternion, Tuple[float, float, float, float]]
|
||||||
|
] = None,
|
||||||
|
frame_id: Optional[str] = None,
|
||||||
|
operation: int = CollisionObject.ADD,
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Add collision object with a cylinder geometry specified by its height and radius.
|
||||||
|
"""
|
||||||
|
|
||||||
|
self.add_collision_primitive(
|
||||||
|
id=id,
|
||||||
|
primitive_type=SolidPrimitive.CYLINDER,
|
||||||
|
dimensions=[height, radius],
|
||||||
|
pose=pose,
|
||||||
|
position=position,
|
||||||
|
quat_xyzw=quat_xyzw,
|
||||||
|
frame_id=frame_id,
|
||||||
|
operation=operation,
|
||||||
|
)
|
||||||
|
|
||||||
|
def add_collision_cone(
|
||||||
|
self,
|
||||||
|
id: str,
|
||||||
|
height: float,
|
||||||
|
radius: float,
|
||||||
|
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||||||
|
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||||||
|
quat_xyzw: Optional[
|
||||||
|
Union[Quaternion, Tuple[float, float, float, float]]
|
||||||
|
] = None,
|
||||||
|
frame_id: Optional[str] = None,
|
||||||
|
operation: int = CollisionObject.ADD,
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Add collision object with a cone geometry specified by its height and radius.
|
||||||
|
"""
|
||||||
|
|
||||||
|
self.add_collision_primitive(
|
||||||
|
id=id,
|
||||||
|
primitive_type=SolidPrimitive.CONE,
|
||||||
|
dimensions=[height, radius],
|
||||||
|
pose=pose,
|
||||||
|
position=position,
|
||||||
|
quat_xyzw=quat_xyzw,
|
||||||
|
frame_id=frame_id,
|
||||||
|
operation=operation,
|
||||||
|
)
|
||||||
|
|
||||||
|
def add_collision_mesh(
|
||||||
|
self,
|
||||||
|
filepath: Optional[str],
|
||||||
|
id: str,
|
||||||
|
pose: Optional[Union[PoseStamped, Pose]] = None,
|
||||||
|
position: Optional[Union[Point, Tuple[float, float, float]]] = None,
|
||||||
|
quat_xyzw: Optional[
|
||||||
|
Union[Quaternion, Tuple[float, float, float, float]]
|
||||||
|
] = None,
|
||||||
|
frame_id: Optional[str] = None,
|
||||||
|
operation: int = CollisionObject.ADD,
|
||||||
|
scale: Union[float, Tuple[float, float, float]] = 1.0,
|
||||||
|
mesh: Optional[Any] = None,
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Add collision object with a mesh geometry. Either `filepath` must be
|
||||||
|
specified or `mesh` must be provided.
|
||||||
|
Note: This function required 'trimesh' Python module to be installed.
|
||||||
|
"""
|
||||||
|
|
||||||
|
# Load the mesh
|
||||||
|
try:
|
||||||
|
import trimesh
|
||||||
|
except ImportError as err:
|
||||||
|
raise ImportError(
|
||||||
|
"Python module 'trimesh' not found! Please install it manually in order "
|
||||||
|
"to add collision objects into the MoveIt 2 planning scene."
|
||||||
|
) from err
|
||||||
|
|
||||||
|
# Check the parameters
|
||||||
|
if (pose is None) and (position is None or quat_xyzw is None):
|
||||||
|
raise ValueError(
|
||||||
|
"Either `pose` or `position` and `quat_xyzw` must be specified!"
|
||||||
|
)
|
||||||
|
if (filepath is None and mesh is None) or (
|
||||||
|
filepath is not None and mesh is not None
|
||||||
|
):
|
||||||
|
raise ValueError("Exactly one of `filepath` or `mesh` must be specified!")
|
||||||
|
if mesh is not None and not isinstance(mesh, trimesh.Trimesh):
|
||||||
|
raise ValueError("`mesh` must be an instance of `trimesh.Trimesh`!")
|
||||||
|
|
||||||
|
if isinstance(pose, PoseStamped):
|
||||||
|
pose_stamped = pose
|
||||||
|
elif isinstance(pose, Pose):
|
||||||
|
pose_stamped = PoseStamped(
|
||||||
|
header=Header(
|
||||||
|
stamp=self.get_clock().now().to_msg(),
|
||||||
|
frame_id=(
|
||||||
|
frame_id if frame_id is not None else self.__base_link_name
|
||||||
|
),
|
||||||
|
),
|
||||||
|
pose=pose,
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
if not isinstance(position, Point):
|
||||||
|
position = Point(
|
||||||
|
x=float(position[0]), y=float(position[1]), z=float(position[2])
|
||||||
|
)
|
||||||
|
if not isinstance(quat_xyzw, Quaternion):
|
||||||
|
quat_xyzw = Quaternion(
|
||||||
|
x=float(quat_xyzw[0]),
|
||||||
|
y=float(quat_xyzw[1]),
|
||||||
|
z=float(quat_xyzw[2]),
|
||||||
|
w=float(quat_xyzw[3]),
|
||||||
|
)
|
||||||
|
pose_stamped = PoseStamped(
|
||||||
|
header=Header(
|
||||||
|
stamp=self.get_clock().now().to_msg(),
|
||||||
|
frame_id=(
|
||||||
|
frame_id if frame_id is not None else self.__base_link_name
|
||||||
|
),
|
||||||
|
),
|
||||||
|
pose=Pose(position=position, orientation=quat_xyzw),
|
||||||
|
)
|
||||||
|
|
||||||
|
msg = CollisionObject(
|
||||||
|
header=pose_stamped.header,
|
||||||
|
id=id,
|
||||||
|
operation=operation,
|
||||||
|
pose=pose_stamped.pose,
|
||||||
|
)
|
||||||
|
|
||||||
|
if filepath is not None:
|
||||||
|
mesh = trimesh.load(filepath)
|
||||||
|
|
||||||
|
# Scale the mesh
|
||||||
|
if isinstance(scale, float):
|
||||||
|
scale = (scale, scale, scale)
|
||||||
|
if not (scale[0] == scale[1] == scale[2] == 1.0):
|
||||||
|
# If the mesh was passed in as a parameter, make a copy of it to
|
||||||
|
# avoid transforming the original.
|
||||||
|
if filepath is not None:
|
||||||
|
mesh = mesh.copy()
|
||||||
|
# Transform the mesh
|
||||||
|
transform = np.eye(4)
|
||||||
|
np.fill_diagonal(transform, scale)
|
||||||
|
mesh.apply_transform(transform)
|
||||||
|
|
||||||
|
msg.meshes.append(
|
||||||
|
Mesh(
|
||||||
|
triangles=[MeshTriangle(vertex_indices=face) for face in mesh.faces],
|
||||||
|
vertices=[
|
||||||
|
Point(x=vert[0], y=vert[1], z=vert[2]) for vert in mesh.vertices
|
||||||
|
],
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
self.__collision_object_publisher.publish(msg)
|
||||||
|
|
||||||
|
def remove_collision_object(self, id: str):
|
||||||
|
"""
|
||||||
|
Remove collision object specified by its `id`.
|
||||||
|
"""
|
||||||
|
|
||||||
|
msg = CollisionObject()
|
||||||
|
msg.id = id
|
||||||
|
msg.operation = CollisionObject.REMOVE
|
||||||
|
msg.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
self.__collision_object_publisher.publish(msg)
|
||||||
|
|
||||||
|
def remove_collision_mesh(self, id: str):
|
||||||
|
"""
|
||||||
|
Remove collision mesh specified by its `id`.
|
||||||
|
Identical to `remove_collision_object()`.
|
||||||
|
"""
|
||||||
|
|
||||||
|
self.remove_collision_object(id)
|
||||||
|
|
||||||
|
def attach_collision_object(
|
||||||
|
self,
|
||||||
|
id: str,
|
||||||
|
link_name: Optional[str] = None,
|
||||||
|
touch_links: List[str] = [],
|
||||||
|
weight: float = 0.0,
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
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)
|
||||||
|
)
|
||||||
|
msg.link_name = link_name
|
||||||
|
msg.touch_links = touch_links
|
||||||
|
msg.weight = weight
|
||||||
|
|
||||||
|
self.__attached_collision_object_publisher.publish(msg)
|
||||||
|
|
||||||
|
def detach_collision_object(self, id: int):
|
||||||
|
"""
|
||||||
|
Detach collision object from the robot.
|
||||||
|
"""
|
||||||
|
|
||||||
|
msg = AttachedCollisionObject(
|
||||||
|
object=CollisionObject(id=id, operation=CollisionObject.REMOVE)
|
||||||
|
)
|
||||||
|
self.__attached_collision_object_publisher.publish(msg)
|
||||||
|
|
||||||
|
def detach_all_collision_objects(self):
|
||||||
|
"""
|
||||||
|
Detach collision object from the robot.
|
||||||
|
"""
|
||||||
|
|
||||||
|
msg = AttachedCollisionObject(
|
||||||
|
object=CollisionObject(operation=CollisionObject.REMOVE)
|
||||||
|
)
|
||||||
|
self.__attached_collision_object_publisher.publish(msg)
|
||||||
|
|
||||||
|
def move_collision(
|
||||||
|
self,
|
||||||
|
id: str,
|
||||||
|
position: Union[Point, Tuple[float, float, float]],
|
||||||
|
quat_xyzw: Union[Quaternion, Tuple[float, float, float, float]],
|
||||||
|
frame_id: Optional[str] = None,
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Move collision object specified by its `id`.
|
||||||
|
"""
|
||||||
|
|
||||||
|
msg = CollisionObject()
|
||||||
|
|
||||||
|
if not isinstance(position, Point):
|
||||||
|
position = Point(
|
||||||
|
x=float(position[0]), y=float(position[1]), z=float(position[2])
|
||||||
|
)
|
||||||
|
if not isinstance(quat_xyzw, Quaternion):
|
||||||
|
quat_xyzw = Quaternion(
|
||||||
|
x=float(quat_xyzw[0]),
|
||||||
|
y=float(quat_xyzw[1]),
|
||||||
|
z=float(quat_xyzw[2]),
|
||||||
|
w=float(quat_xyzw[3]),
|
||||||
|
)
|
||||||
|
|
||||||
|
pose = Pose()
|
||||||
|
pose.position = position
|
||||||
|
pose.orientation = quat_xyzw
|
||||||
|
msg.pose = pose
|
||||||
|
msg.id = id
|
||||||
|
msg.operation = CollisionObject.MOVE
|
||||||
|
msg.header.frame_id = (
|
||||||
|
frame_id if frame_id is not None else self.__base_link_name
|
||||||
|
)
|
||||||
|
msg.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
|
||||||
|
self.__collision_object_publisher.publish(msg)
|
||||||
|
|
||||||
|
def update_planning_scene(self) -> bool:
|
||||||
|
"""
|
||||||
|
Gets the current planning scene. Returns whether the service call was
|
||||||
|
successful.
|
||||||
|
"""
|
||||||
|
|
||||||
|
if not self._get_planning_scene_service.service_is_ready():
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"Service '{self._get_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
|
||||||
|
)
|
||||||
|
return False
|
||||||
|
self.__planning_scene = self._get_planning_scene_service.call(
|
||||||
|
GetPlanningScene.Request()
|
||||||
|
).scene
|
||||||
|
return True
|
||||||
|
|
||||||
|
def allow_collisions(self, id: str, allow: bool) -> Optional[Future]:
|
||||||
|
"""
|
||||||
|
Takes in the ID of an element in the planning scene. Modifies the allowed
|
||||||
|
collision matrix to (dis)allow collisions between that object and all other
|
||||||
|
object.
|
||||||
|
|
||||||
|
If `allow` is True, a plan will succeed even if the robot collides with that object.
|
||||||
|
If `allow` is False, a plan will fail if the robot collides with that object.
|
||||||
|
Returns whether it successfully updated the allowed collision matrix.
|
||||||
|
|
||||||
|
Returns the future of the service call.
|
||||||
|
"""
|
||||||
|
# Update the planning scene
|
||||||
|
if not self.update_planning_scene():
|
||||||
|
return None
|
||||||
|
allowed_collision_matrix = self.__planning_scene.allowed_collision_matrix
|
||||||
|
self.__old_allowed_collision_matrix = copy.deepcopy(allowed_collision_matrix)
|
||||||
|
|
||||||
|
# Get the location in the allowed collision matrix of the object
|
||||||
|
j = None
|
||||||
|
if id not in allowed_collision_matrix.entry_names:
|
||||||
|
allowed_collision_matrix.entry_names.append(id)
|
||||||
|
else:
|
||||||
|
j = allowed_collision_matrix.entry_names.index(id)
|
||||||
|
# For all other objects, (dis)allow collisions with the object with `id`
|
||||||
|
for i in range(len(allowed_collision_matrix.entry_values)):
|
||||||
|
if j is None:
|
||||||
|
allowed_collision_matrix.entry_values[i].enabled.append(allow)
|
||||||
|
elif i != j:
|
||||||
|
allowed_collision_matrix.entry_values[i].enabled[j] = allow
|
||||||
|
# For the object with `id`, (dis)allow collisions with all other objects
|
||||||
|
allowed_collision_entry = AllowedCollisionEntry(
|
||||||
|
enabled=[allow for _ in range(len(allowed_collision_matrix.entry_names))]
|
||||||
|
)
|
||||||
|
if j is None:
|
||||||
|
allowed_collision_matrix.entry_values.append(allowed_collision_entry)
|
||||||
|
else:
|
||||||
|
allowed_collision_matrix.entry_values[j] = allowed_collision_entry
|
||||||
|
|
||||||
|
# Apply the new planning scene
|
||||||
|
if not self._apply_planning_scene_service.service_is_ready():
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
|
||||||
|
)
|
||||||
|
return None
|
||||||
|
return self._apply_planning_scene_service.call_async(
|
||||||
|
ApplyPlanningScene.Request(scene=self.__planning_scene)
|
||||||
|
)
|
||||||
|
|
||||||
|
def process_allow_collision_future(self, future: Future) -> bool:
|
||||||
|
"""
|
||||||
|
Return whether the allow collision service call is done and has succeeded
|
||||||
|
or not. If it failed, reset the allowed collision matrix to the old one.
|
||||||
|
"""
|
||||||
|
if not future.done():
|
||||||
|
return False
|
||||||
|
|
||||||
|
# Get response
|
||||||
|
resp = future.result()
|
||||||
|
|
||||||
|
# If it failed, restore the old planning scene
|
||||||
|
if not resp.success:
|
||||||
|
self.__planning_scene.allowed_collision_matrix = (
|
||||||
|
self.__old_allowed_collision_matrix
|
||||||
|
)
|
||||||
|
|
||||||
|
return resp.success
|
||||||
|
|
||||||
|
def clear_all_collision_objects(self) -> Optional[Future]:
|
||||||
|
"""
|
||||||
|
Removes all attached and un-attached collision objects from the planning scene.
|
||||||
|
|
||||||
|
Returns a future for the ApplyPlanningScene service call.
|
||||||
|
"""
|
||||||
|
# Update the planning scene
|
||||||
|
if not self.update_planning_scene():
|
||||||
|
return None
|
||||||
|
self.__old_planning_scene = copy.deepcopy(self.__planning_scene)
|
||||||
|
|
||||||
|
# Remove all collision objects from the planning scene
|
||||||
|
self.__planning_scene.world.collision_objects = []
|
||||||
|
self.__planning_scene.robot_state.attached_collision_objects = []
|
||||||
|
|
||||||
|
# Apply the new planning scene
|
||||||
|
if not self._apply_planning_scene_service.service_is_ready():
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
|
||||||
|
)
|
||||||
|
return None
|
||||||
|
return self._apply_planning_scene_service.call_async(
|
||||||
|
ApplyPlanningScene.Request(scene=self.__planning_scene)
|
||||||
|
)
|
||||||
@@ -48,6 +48,7 @@ class DeviceNodeResourceTracker:
|
|||||||
|
|
||||||
def loop_find_resource(self, resource, resource_cls_type, identifier_key, compare_value):
|
def loop_find_resource(self, resource, resource_cls_type, identifier_key, compare_value):
|
||||||
res_list = []
|
res_list = []
|
||||||
|
print(resource, resource_cls_type, identifier_key, compare_value)
|
||||||
children = getattr(resource, "children", [])
|
children = getattr(resource, "children", [])
|
||||||
for child in children:
|
for child in children:
|
||||||
res_list.extend(self.loop_find_resource(child, resource_cls_type, identifier_key, compare_value))
|
res_list.extend(self.loop_find_resource(child, resource_cls_type, identifier_key, compare_value))
|
||||||
|
|||||||
Reference in New Issue
Block a user