diff --git a/test/experiments/test.json b/test/experiments/test.json index 07b802cd..ac5e5043 100644 --- a/test/experiments/test.json +++ b/test/experiments/test.json @@ -4,13 +4,14 @@ "id": "Gripper1", "name": "假夹爪", "children": [ + "Plate1" ], "parent": null, "type": "device", "class": "gripper.mock", "position": { - "x": 620.6111111111111, - "y": 171, + "x": 0, + "y": 0, "z": 0 }, "config": { @@ -23,18 +24,120 @@ "name": "Plate1", "children": [ ], - "parent": null, + "parent": "Gripper1", "type": "plate", - "class": "nest_96_wellplate_2ml_deep", + "class": "nest_96_wellplate_100ul_pcr_full_skirt", "position": { - "x": 620.6111111111111, - "y": 171, - "z": 0 + "x": 0, + "y": 0, + "z": 69 }, "config": { }, "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": [ diff --git a/test/experiments/test_copy.json b/test/experiments/test_copy.json new file mode 100644 index 00000000..9b1debed --- /dev/null +++ b/test/experiments/test_copy.json @@ -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": [ + + ] +} \ No newline at end of file diff --git a/unilabos/app/backend.py b/unilabos/app/backend.py index 19cebff0..8acc8251 100644 --- a/unilabos/app/backend.py +++ b/unilabos/app/backend.py @@ -12,6 +12,8 @@ def start_backend( controllers_config: dict = {}, bridges=[], without_host: bool = False, + visual: str = "None", + resources_mesh_config: dict = {}, **kwargs ): if backend == "ros": @@ -29,7 +31,9 @@ def start_backend( backend_thread = threading.Thread( 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() logger.info(f"Backend {backend} started.") diff --git a/unilabos/app/main.py b/unilabos/app/main.py index 89462822..16667d6c 100644 --- a/unilabos/app/main.py +++ b/unilabos/app/main.py @@ -1,10 +1,17 @@ import argparse +import asyncio import os import signal import sys import json +import time + import yaml 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__)) @@ -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.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(): @@ -70,7 +81,12 @@ def parse_args(): default=True, help="是否在启动时打开信息页", ) - + parser.add_argument( + "--visual", + choices=["rviz", "web","None"], + default="rviz", + help="选择可视化工具: 'rviz' 或 'web' 或 'None'，默认'rviz'", + ) return parser.parse_args() @@ -121,6 +137,7 @@ def main(): # 注册表 build_registry(args_dict["registry_path"]) + if args_dict["graph"] is not None: import unilabos.resources.graphio as graph_res 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["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["graph"] = graph_res.physical_setup_graph else: 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.SIGTERM, _exit) mqtt_client.start() + args_dict["resources_mesh_config"] = {} + + if args_dict["visual"] != "None": + 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) - start_backend(**args_dict) - start_server(port=args_dict.get("port", 8002), open_browser=args_dict.get("open_browser", False)) + 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__": diff --git a/unilabos/app/mq.py b/unilabos/app/mq.py index a6123fb2..d0975e91 100644 --- a/unilabos/app/mq.py +++ b/unilabos/app/mq.py @@ -35,7 +35,8 @@ class MQTTClient: self.client.on_disconnect = self._on_disconnect 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): logger.info("[MQTT] Connected with result code " + str(rc)) diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/joint_config.json b/unilabos/device_mesh/devices/opentrons_liquid_handler/joint_config.json new file mode 100644 index 00000000..e40339da --- /dev/null +++ b/unilabos/device_mesh/devices/opentrons_liquid_handler/joint_config.json @@ -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" + } +} \ No newline at end of file diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro b/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro new file mode 100644 index 00000000..4e660557 --- /dev/null +++ b/unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro @@ -0,0 +1,210 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-0.fbx b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-0.fbx new file mode 100644 index 00000000..5687a596 Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-0.fbx differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-0.stl b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-0.stl new file mode 100644 index 00000000..5a2f71e8 Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-0.stl differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-1.fbx b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-1.fbx new file mode 100644 index 00000000..201167d1 Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-1.fbx differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-1.stl b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-1.stl new file mode 100644 index 00000000..a47b9020 Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-1.stl differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-2.fbx b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-2.fbx new file mode 100644 index 00000000..4e716336 Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-2.fbx differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-2.stl b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-2.stl new file mode 100644 index 00000000..a1d118ff Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-2.stl differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3a.fbx b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3a.fbx new file mode 100644 index 00000000..32234365 Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3a.fbx differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3a.stl b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3a.stl new file mode 100644 index 00000000..7d31a8d6 Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3a.stl differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3b.fbx b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3b.fbx new file mode 100644 index 00000000..8f5fd4ff Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3b.fbx differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3b.stl b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3b.stl new file mode 100644 index 00000000..fa674839 Binary files /dev/null and b/unilabos/device_mesh/devices/opentrons_liquid_handler/meshes/ot2-3b.stl differ diff --git a/unilabos/device_mesh/devices/opentrons_liquid_handler/param_config.json b/unilabos/device_mesh/devices/opentrons_liquid_handler/param_config.json new file mode 100644 index 00000000..749feb47 --- /dev/null +++ b/unilabos/device_mesh/devices/opentrons_liquid_handler/param_config.json @@ -0,0 +1,10 @@ +{ + "private_param": + { + + }, + "public_param": + { + + } +} diff --git a/unilabos/device_mesh/devices/slide_w140/joint_config.json b/unilabos/device_mesh/devices/slide_w140/joint_config.json new file mode 100644 index 00000000..168bb0d4 --- /dev/null +++ b/unilabos/device_mesh/devices/slide_w140/joint_config.json @@ -0,0 +1,6 @@ +{ + "slider_joint": { + "child":"slider", + "axis" : "x" + } +} \ No newline at end of file diff --git a/unilabos/device_mesh/devices/slide_w140/macro_device.xacro b/unilabos/device_mesh/devices/slide_w140/macro_device.xacro new file mode 100644 index 00000000..7e43242e --- /dev/null +++ b/unilabos/device_mesh/devices/slide_w140/macro_device.xacro @@ -0,0 +1,136 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unilabos/device_mesh/devices/slide_w140/meshes/base_link.STL b/unilabos/device_mesh/devices/slide_w140/meshes/base_link.STL new file mode 100755 index 00000000..f2ae347c Binary files /dev/null and b/unilabos/device_mesh/devices/slide_w140/meshes/base_link.STL differ diff --git a/unilabos/device_mesh/devices/slide_w140/meshes/base_link.fbx b/unilabos/device_mesh/devices/slide_w140/meshes/base_link.fbx new file mode 100644 index 00000000..8439d485 Binary files /dev/null and b/unilabos/device_mesh/devices/slide_w140/meshes/base_link.fbx differ diff --git a/unilabos/device_mesh/devices/slide_w140/meshes/length.STL b/unilabos/device_mesh/devices/slide_w140/meshes/length.STL new file mode 100755 index 00000000..df420447 Binary files /dev/null and b/unilabos/device_mesh/devices/slide_w140/meshes/length.STL differ diff --git a/unilabos/device_mesh/devices/slide_w140/meshes/length.fbx b/unilabos/device_mesh/devices/slide_w140/meshes/length.fbx new file mode 100644 index 00000000..b46c2c1e Binary files /dev/null and b/unilabos/device_mesh/devices/slide_w140/meshes/length.fbx differ diff --git a/unilabos/device_mesh/devices/slide_w140/meshes/slide_end.STL b/unilabos/device_mesh/devices/slide_w140/meshes/slide_end.STL new file mode 100755 index 00000000..3eaf9e01 Binary files /dev/null and b/unilabos/device_mesh/devices/slide_w140/meshes/slide_end.STL differ diff --git a/unilabos/device_mesh/devices/slide_w140/meshes/slide_end.fbx b/unilabos/device_mesh/devices/slide_w140/meshes/slide_end.fbx new file mode 100644 index 00000000..de0331ba Binary files /dev/null and b/unilabos/device_mesh/devices/slide_w140/meshes/slide_end.fbx differ diff --git a/unilabos/device_mesh/devices/slide_w140/meshes/slider.STL b/unilabos/device_mesh/devices/slide_w140/meshes/slider.STL new file mode 100755 index 00000000..d4b74347 Binary files /dev/null and b/unilabos/device_mesh/devices/slide_w140/meshes/slider.STL differ diff --git a/unilabos/device_mesh/devices/slide_w140/meshes/slider.fbx b/unilabos/device_mesh/devices/slide_w140/meshes/slider.fbx new file mode 100644 index 00000000..38b9814b Binary files /dev/null and b/unilabos/device_mesh/devices/slide_w140/meshes/slider.fbx differ diff --git a/unilabos/device_mesh/devices/slide_w140/param_config.json b/unilabos/device_mesh/devices/slide_w140/param_config.json new file mode 100644 index 00000000..d1c9213b --- /dev/null +++ b/unilabos/device_mesh/devices/slide_w140/param_config.json @@ -0,0 +1,12 @@ +{ + "private_param": + { + "min_d": 0.1 , + "max_d": 0.1 , + "slider_d": 0.14 + }, + "public_param": + { + "length" :0.1 + } +} \ No newline at end of file diff --git a/unilabos/device_mesh/resource_visalization.py b/unilabos/device_mesh/resource_visalization.py new file mode 100644 index 00000000..2ec28b01 --- /dev/null +++ b/unilabos/device_mesh/resource_visalization.py @@ -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 = ''' + + + + + ''' + self.robot_state_str= ''' + + + + ''' + 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() \ No newline at end of file diff --git a/unilabos/device_mesh/resources/generic_labware_tube_10_75/0_base.png b/unilabos/device_mesh/resources/generic_labware_tube_10_75/0_base.png new file mode 100644 index 00000000..d9de252f Binary files /dev/null and b/unilabos/device_mesh/resources/generic_labware_tube_10_75/0_base.png differ diff --git a/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.glb b/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.glb new file mode 100644 index 00000000..1c64cc4e Binary files /dev/null and b/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.glb differ diff --git a/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.stl b/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.stl new file mode 100644 index 00000000..023cf1a2 Binary files /dev/null and b/unilabos/device_mesh/resources/generic_labware_tube_10_75/meshes/0_base.stl differ diff --git a/unilabos/device_mesh/resources/generic_labware_tube_10_75/meta.json b/unilabos/device_mesh/resources/generic_labware_tube_10_75/meta.json new file mode 100644 index 00000000..84f4657b --- /dev/null +++ b/unilabos/device_mesh/resources/generic_labware_tube_10_75/meta.json @@ -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" + ] +} diff --git a/unilabos/device_mesh/resources/generic_labware_tube_10_75/modal.xacro b/unilabos/device_mesh/resources/generic_labware_tube_10_75/modal.xacro new file mode 100644 index 00000000..65711112 --- /dev/null +++ b/unilabos/device_mesh/resources/generic_labware_tube_10_75/modal.xacro @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.glb b/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.glb new file mode 100644 index 00000000..afb6c99b Binary files /dev/null and b/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.glb differ diff --git a/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.stl b/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.stl new file mode 100644 index 00000000..2655ff94 Binary files /dev/null and b/unilabos/device_mesh/resources/tecan_nested_tip_rack/meshes/plate.stl differ diff --git a/unilabos/device_mesh/resources/tecan_nested_tip_rack/meta.json b/unilabos/device_mesh/resources/tecan_nested_tip_rack/meta.json new file mode 100644 index 00000000..1b437d1f --- /dev/null +++ b/unilabos/device_mesh/resources/tecan_nested_tip_rack/meta.json @@ -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" + ] +} diff --git a/unilabos/device_mesh/resources/tecan_nested_tip_rack/modal.xacro b/unilabos/device_mesh/resources/tecan_nested_tip_rack/modal.xacro new file mode 100644 index 00000000..e29617fa --- /dev/null +++ b/unilabos/device_mesh/resources/tecan_nested_tip_rack/modal.xacro @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/unilabos/device_mesh/resources/tecan_nested_tip_rack/plate.png b/unilabos/device_mesh/resources/tecan_nested_tip_rack/plate.png new file mode 100644 index 00000000..34d7ac68 Binary files /dev/null and b/unilabos/device_mesh/resources/tecan_nested_tip_rack/plate.png differ diff --git a/unilabos/device_mesh/view_robot.rviz b/unilabos/device_mesh/view_robot.rviz new file mode 100644 index 00000000..bf32efc3 --- /dev/null +++ b/unilabos/device_mesh/view_robot.rviz @@ -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: + 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: + 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 diff --git a/unilabos/devices/agv/ur_arm_task.py b/unilabos/devices/agv/ur_arm_task.py index 8c84c855..47a7c931 100644 --- a/unilabos/devices/agv/ur_arm_task.py +++ b/unilabos/devices/agv/ur_arm_task.py @@ -77,9 +77,6 @@ class UrArmTask(): if n > retry: 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() def arm_init(self): diff --git a/unilabos/devices/ros_dev/__init__.py b/unilabos/devices/ros_dev/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py new file mode 100644 index 00000000..7933f8df --- /dev/null +++ b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py @@ -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() \ No newline at end of file diff --git a/unilabos/registry/devices/robot_gripper.yaml b/unilabos/registry/devices/robot_gripper.yaml index 04ea338b..3cb86fed 100644 --- a/unilabos/registry/devices/robot_gripper.yaml +++ b/unilabos/registry/devices/robot_gripper.yaml @@ -19,7 +19,9 @@ gripper.mock: result: position: position effort: torque - + model: + type: device + mesh: opentrons_liquid_handler gripper.misumi_rz: description: Misumi RZ gripper @@ -35,4 +37,4 @@ gripper.misumi_rz: command: command feedback: {} result: - success: success \ No newline at end of file + success: success diff --git a/unilabos/registry/devices/sim_nodes.yaml b/unilabos/registry/devices/sim_nodes.yaml new file mode 100644 index 00000000..de4d110e --- /dev/null +++ b/unilabos/registry/devices/sim_nodes.yaml @@ -0,0 +1,5 @@ +lh_joint_publisher: + class: + module: unilabos.devices.ros_dev.liquid_handler_joint_publisher:LiquidHandlerJointPublisher + type: ros2 + diff --git a/unilabos/registry/resources/opentrons/deck.yaml b/unilabos/registry/resources/opentrons/deck.yaml index 439da452..652240a5 100644 --- a/unilabos/registry/resources/opentrons/deck.yaml +++ b/unilabos/registry/resources/opentrons/deck.yaml @@ -2,4 +2,7 @@ OTDeck: description: Opentrons deck class: module: pylabrobot.resources.opentrons.deck:OTDeck - type: pylabrobot \ No newline at end of file + type: pylabrobot + model: + type: device + mesh: opentrons_liquid_handler \ No newline at end of file diff --git a/unilabos/registry/resources/opentrons/plates.yaml b/unilabos/registry/resources/opentrons/plates.yaml index f15da8ed..c00b2744 100644 --- a/unilabos/registry/resources/opentrons/plates.yaml +++ b/unilabos/registry/resources/opentrons/plates.yaml @@ -51,6 +51,12 @@ nest_96_wellplate_100ul_pcr_full_skirt: class: module: pylabrobot.resources.opentrons.plates:nest_96_wellplate_100ul_pcr_full_skirt 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: description: Applied Biosystems microamp 384 wellplate 40ul diff --git a/unilabos/registry/resources/opentrons/tip_racks.yaml b/unilabos/registry/resources/opentrons/tip_racks.yaml index c5292e2c..a605b55f 100644 --- a/unilabos/registry/resources/opentrons/tip_racks.yaml +++ b/unilabos/registry/resources/opentrons/tip_racks.yaml @@ -63,7 +63,13 @@ opentrons_96_filtertiprack_1000ul: class: module: pylabrobot.resources.opentrons.tip_racks:opentrons_96_filtertiprack_1000ul 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: description: Opentrons 96 filtertiprack 20ul class: diff --git a/unilabos/ros/main_slave_run.py b/unilabos/ros/main_slave_run.py index 9ac96748..8560d376 100644 --- a/unilabos/ros/main_slave_run.py +++ b/unilabos/ros/main_slave_run.py @@ -2,9 +2,13 @@ import copy import json import os import threading +import time from typing import Optional, Dict, Any, List import rclpy +from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher +from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager +from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker from unilabos_msgs.msg import Resource # type: ignore from unilabos_msgs.srv import ResourceAdd, SerialCommand # type: ignore from rclpy.executors import MultiThreadedExecutor @@ -44,13 +48,15 @@ def main( graph: Optional[Dict[str, Any]] = None, controllers_config: Dict[str, Any] = {}, bridges: List[Any] = [], + visual: str = "None", + resources_mesh_config: dict = {}, args: List[str] = ["--log-level", "debug"], discovery_interval: float = 5.0, ) -> None: """主函数""" - rclpy.init(args=args) - rclpy.__executor = executor = MultiThreadedExecutor() + rclpy.init(args=args) + executor = rclpy.__executor = MultiThreadedExecutor() # 创建主机节点 host_node = HostNode( "host_node", @@ -62,11 +68,26 @@ def main( 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.start() while True: - input() + time.sleep(1) def slave( @@ -78,8 +99,11 @@ def slave( args: List[str] = ["--log-level", "debug"], ) -> None: """从节点函数""" - rclpy.init(args=args) - rclpy.__executor = executor = MultiThreadedExecutor() + if not rclpy.ok(): + rclpy.init(args=args) + executor = rclpy.__executor + if not executor: + executor = rclpy.__executor = MultiThreadedExecutor() devices_config_copy = copy.deepcopy(devices_config) for device_id, device_config in devices_config.items(): d = initialize_device_from_dict(device_id, device_config) @@ -120,7 +144,7 @@ def slave( logger.info(f"Slave resource added.") while True: - input() + time.sleep(1) if __name__ == "__main__": main() diff --git a/unilabos/ros/nodes/presets/joint_republisher.py b/unilabos/ros/nodes/presets/joint_republisher.py new file mode 100644 index 00000000..b731acc7 --- /dev/null +++ b/unilabos/ros/nodes/presets/joint_republisher.py @@ -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() diff --git a/unilabos/ros/nodes/presets/resource_mesh_manager.py b/unilabos/ros/nodes/presets/resource_mesh_manager.py new file mode 100644 index 00000000..012404b3 --- /dev/null +++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py @@ -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) + ) diff --git a/unilabos/ros/nodes/resource_tracker.py b/unilabos/ros/nodes/resource_tracker.py index c0886cd4..dc9f9c4a 100644 --- a/unilabos/ros/nodes/resource_tracker.py +++ b/unilabos/ros/nodes/resource_tracker.py @@ -48,6 +48,7 @@ class DeviceNodeResourceTracker: def loop_find_resource(self, resource, resource_cls_type, identifier_key, compare_value): res_list = [] + print(resource, resource_cls_type, identifier_key, compare_value) children = getattr(resource, "children", []) for child in children: res_list.extend(self.loop_find_resource(child, resource_cls_type, identifier_key, compare_value))