diff --git a/unilabos/devices/ros_dev/lh_joint_config.json b/unilabos/devices/ros_dev/lh_joint_config.json index 3f316f55..6fb8c1f2 100644 --- a/unilabos/devices/ros_dev/lh_joint_config.json +++ b/unilabos/devices/ros_dev/lh_joint_config.json @@ -1,36 +1,30 @@ { - "OTDeck":{ + "OTdeck":{ "joint_names":[ "first_joint", "second_joint", "third_joint", "fourth_joint" ], - "link_names":[ - "first_link", - "second_link", - "third_link", - "fourth_link" - ], "y":{ "first_joint":{ - "factor":-0.001, - "offset":0.163 + "factor":-1, + "offset":0.0 } }, "x":{ "second_joint":{ - "factor":-0.001, - "offset":0.1775 + "factor":-1, + "offset":0.0 } }, "z":{ "third_joint":{ - "factor":0.001, + "factor":1, "offset":0.0 }, "fourth_joint":{ - "factor":0.001, + "factor":1, "offset":0.0 } } diff --git a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py index 9a1f1f04..3f160070 100644 --- a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py +++ b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py @@ -1,7 +1,4 @@ -import asyncio import copy -from pathlib import Path -import threading import rclpy import json import time @@ -18,7 +15,7 @@ from rclpy.node import Node import re class LiquidHandlerJointPublisher(BaseROS2DeviceNode): - def __init__(self,resources_config:list, resource_tracker, rate=50, device_id:str = "lh_joint_publisher"): + def __init__(self,resource_config:list, resource_tracker, rate=50, device_id:str = "lh_joint_publisher"): super().__init__( driver_instance=self, device_id=device_id, @@ -31,8 +28,8 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): # 初始化参数 self.j_msg = JointState() - joint_config = json.load(open(f"{Path(__file__).parent.absolute()}/lh_joint_config.json", encoding="utf-8")) - self.resources_config = {x['id']:x for x in resources_config} + joint_config = json.load(open('./lh_joint_config.json', encoding="utf-8")) + self.resource_config = resource_config self.rate = rate self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) @@ -53,7 +50,7 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): self.deck_list = [] self.lh_devices = {} # 初始化设备ID与config信息 - for resource in resources_config: + for resource in resource_config: if resource['class'] == 'liquid_handler': deck_id = resource['config']['data']['children'][0]['_resource_child_name'] deck_class = resource['config']['data']['children'][0]['_resource_type'].split(':')[-1] @@ -99,15 +96,13 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): def find_resource_parent(self, resource_id:str): # 遍历父辈，找到父辈的父辈，直到找到设备ID - parent_id = self.resources_config[resource_id]['parent'] + parent_id = self.resource_config[resource_id]['parent'] try: if parent_id in self.deck_list: - p_ = self.resources_config[parent_id]['parent'] - str_ = f'{p_}_{parent_id}' - return str(str_) + return f'{self.resource_config[parent_id]['parent']}_{parent_id}' else: - return self.find_resource_parent(parent_id) - except Exception as e: + self.find_resource_parent(parent_id) + except: return None @@ -161,13 +156,10 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): goal_handle.succeed() except Exception as e: - print(f'Liquid handler action error: \n{e}') + print(e) goal_handle.abort() result.success = False - - print('='*20) - print(result) - print('='*20) + return result def inverse_kinematics(self, x, y, z, parent_id, @@ -206,40 +198,36 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}") joint_positions[index] = z * config["factor"] + config["offset"] z_index = index - + return joint_positions ,z_index - def move_joints(self, resource_names, x, y, z, option, speed = 0.1 ,x_joint=None, y_joint=None, z_joint=None): + def move_joints(self, resource_names, speed,x,y,z,option, x_joint=None, y_joint=None, z_joint=None): if isinstance(resource_names, list): resource_name_ = resource_names[0] else: resource_name_ = resource_names - parent_id = self.find_resource_parent(resource_name_) + if x_joint is None: - xa,xb = next(iter(self.lh_devices[parent_id]['joint_config']['x'].items())) - x_joint_config = {xa:xb} - elif x_joint in self.lh_devices[parent_id]['joint_config']['x']: - x_joint_config = self.lh_devices[parent_id]['joint_config']['x'][x_joint] + x_joint_config = next(iter(self.lh_devices[parent_id]['x'].items())) + elif x_joint in self.lh_devices[parent_id]['x']: + x_joint_config = self.lh_devices[parent_id]['x'][x_joint] else: raise ValueError(f"x_joint {x_joint} not in joint_config['x']") if y_joint is None: - ya,yb = next(iter(self.lh_devices[parent_id]['joint_config']['y'].items())) - y_joint_config = {ya:yb} - elif y_joint in self.lh_devices[parent_id]['joint_config']['y']: - y_joint_config = self.lh_devices[parent_id]['joint_config']['y'][y_joint] + y_joint_config = next(iter(self.lh_devices[parent_id]['y'].items())) + elif y_joint in self.lh_devices[parent_id]['y']: + y_joint_config = self.lh_devices[parent_id]['y'][y_joint] else: raise ValueError(f"y_joint {y_joint} not in joint_config['y']") if z_joint is None: - za, zb = next(iter(self.lh_devices[parent_id]['joint_config']['z'].items())) - z_joint_config = {za :zb} - elif z_joint in self.lh_devices[parent_id]['joint_config']['z']: - z_joint_config = self.lh_devices[parent_id]['joint_config']['z'][z_joint] + z_joint_config = next(iter(self.lh_devices[parent_id]['z'].items())) + elif z_joint in self.lh_devices[parent_id]['z']: + z_joint_config = self.lh_devices[parent_id]['z'][z_joint] else: raise ValueError(f"z_joint {z_joint} not in joint_config['z']") - joint_positions_target, z_index = self.inverse_kinematics(x,y,z,parent_id,x_joint_config,y_joint_config,z_joint_config) joint_positions_target_zero = copy.deepcopy(joint_positions_target) joint_positions_target_zero[z_index] = 0 @@ -247,13 +235,10 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): self.move_to(joint_positions_target_zero, speed, parent_id) self.move_to(joint_positions_target, speed, parent_id) if option == "pick": - link_name = self.lh_devices[parent_id]['joint_config']['link_names'][z_index] - link_name = f'{parent_id}_{link_name}' + link_name = self.lh_devices[parent_id]['joint_msg'].name[z_index] self.resource_move(resource_name_, link_name, [0,1,2,3,4,5,6,7]) - time.sleep(1) elif option == "drop": self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7]) - time.sleep(1) self.move_to(joint_positions_target_zero, speed, parent_id) @@ -322,59 +307,14 @@ class JointStatePublisher(Node): return None - def send_resource_action(self, resource_name, x,y,z,option, speed = 0.1,x_joint=None, y_joint=None, z_joint=None): + def send_resource_action(self, resource_id_list:list[str], link_name:str): goal_msg = SendCmd.Goal() - str_dict = { - 'resource_names':resource_name, - 'x':x, - 'y':y, - 'z':z, - 'option':option, - 'speed':speed, - 'x_joint':x_joint, - 'y_joint':y_joint, - 'z_joint':z_joint - } - + str_dict = {} + for resource in resource_id_list: + str_dict[resource] = link_name goal_msg.command = json.dumps(str_dict) - - if not self.lh_action_client.wait_for_server(timeout_sec=5.0): - self.get_logger().error('Action server not available') - return None - - try: - # 创建新的executor - executor = rclpy.executors.SingleThreadedExecutor() - executor.add_node(self) - - # 发送目标 - future = self.lh_action_client.send_goal_async(goal_msg) - - # 使用executor等待结果 - while not future.done(): - executor.spin_once(timeout_sec=0.1) - - handle = future.result() - - if not handle.accepted: - self.get_logger().error('Goal was rejected') - return None - - # 等待最终结果 - result_future = handle.get_result_async() - while not result_future.done(): - executor.spin_once(timeout_sec=0.1) - - result = result_future.result() - return result - - except Exception as e: - self.get_logger().error(f'Error during action execution: {str(e)}') - return None - finally: - # 清理executor - executor.remove_node(self) + self.lh_action_client.send_goal_async(goal_msg) def main(): diff --git a/unilabos/resources/graphio.py b/unilabos/resources/graphio.py index 242d5a1a..9621b5de 100644 --- a/unilabos/resources/graphio.py +++ b/unilabos/resources/graphio.py @@ -9,7 +9,7 @@ try: from pylabrobot.resources.resource import Resource as ResourcePLR except ImportError: pass -from typing import Union, get_origin, get_args + physical_setup_graph: nx.Graph = None @@ -297,6 +297,7 @@ def nested_dict_to_list(nested_dict: dict) -> list[dict]: # FIXME 是tree？ return result + def convert_resources_to_type( resources_list: list[dict], resource_type: type, *, plr_model: bool = False ) -> Union[list[dict], dict, None, "ResourcePLR"]: @@ -318,15 +319,9 @@ def convert_resources_to_type( return resource_ulab_to_plr(resources_list, plr_model) resources_tree = dict_to_tree({r["id"]: r for r in resources_list}) return resource_ulab_to_plr(resources_tree[0], plr_model) - elif isinstance(resource_type, list) : - if all((get_origin(t) is Union) for t in resource_type): - resources_tree = dict_to_tree({r["id"]: r for r in resources_list}) - return [resource_ulab_to_plr(r, plr_model) for r in resources_tree] - elif all(issubclass(t, ResourcePLR) for t in resource_type): - resources_tree = dict_to_tree({r["id"]: r for r in resources_list}) - return [resource_ulab_to_plr(r, plr_model) for r in resources_tree] - - + elif isinstance(resource_type, list) and all(issubclass(t, ResourcePLR) for t in resource_type): + resources_tree = dict_to_tree({r["id"]: r for r in resources_list}) + return [resource_ulab_to_plr(r, plr_model) for r in resources_tree] else: return None @@ -347,13 +342,9 @@ def convert_resources_from_type(resources_list, resource_type: type) -> Union[li elif isinstance(resource_type, type) and issubclass(resource_type, ResourcePLR): resources_tree = [resource_plr_to_ulab(resources_list)] return tree_to_list(resources_tree) - elif isinstance(resource_type, list) : - if all((get_origin(t) is Union) for t in resource_type): - resources_tree = [resource_plr_to_ulab(r) for r in resources_list] - return tree_to_list(resources_tree) - elif all(issubclass(t, ResourcePLR) for t in resource_type): - resources_tree = [resource_plr_to_ulab(r) for r in resources_list] - return tree_to_list(resources_tree) + elif isinstance(resource_type, list) and all(issubclass(t, ResourcePLR) for t in resource_type): + resources_tree = [resource_plr_to_ulab(r) for r in resources_list] + return tree_to_list(resources_tree) else: return None diff --git a/unilabos/ros/main_slave_run.py b/unilabos/ros/main_slave_run.py index 4f78b1c9..c4c5a172 100644 --- a/unilabos/ros/main_slave_run.py +++ b/unilabos/ros/main_slave_run.py @@ -9,7 +9,6 @@ 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.devices.ros_dev.liquid_handler_joint_publisher import LiquidHandlerJointPublisher from unilabos_msgs.msg import Resource # type: ignore from unilabos_msgs.srv import ResourceAdd, SerialCommand # type: ignore from rclpy.executors import MultiThreadedExecutor @@ -73,18 +72,16 @@ def main( resource_mesh_manager = ResourceMeshManager( resources_mesh_config, resources_config, - resource_tracker = host_node.resource_tracker, + resource_tracker= DeviceNodeResourceTracker(), device_id = 'resource_mesh_manager', ) joint_republisher = JointRepublisher( 'joint_republisher', - host_node.resource_tracker + DeviceNodeResourceTracker() ) executor.add_node(resource_mesh_manager) executor.add_node(joint_republisher) - lh_joint_pub = LiquidHandlerJointPublisher(resources_config=resources_config, resource_tracker=host_node.resource_tracker) - executor.add_node(lh_joint_pub) thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread") thread.start() diff --git a/unilabos/ros/nodes/presets/resource_mesh_manager.py b/unilabos/ros/nodes/presets/resource_mesh_manager.py index 0ceb5807..b8ed3257 100644 --- a/unilabos/ros/nodes/presets/resource_mesh_manager.py +++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py @@ -91,9 +91,6 @@ class ResourceMeshManager(BaseROS2DeviceNode): self.__collision_object_publisher = self.create_publisher( CollisionObject, "/collision_object", 10 ) - self.__planning_scene_publisher = self.create_publisher( - PlanningScene, "/planning_scene", 10 - ) self.__attached_collision_object_publisher = self.create_publisher( AttachedCollisionObject, "/attached_collision_object", 0 ) @@ -148,7 +145,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): """刷新资源配置""" registry = lab_registry - resource_config = json.loads(resource_config_str.replace("'",'"')) + resource_config = json.loads(resource_config_str) if resource_config['id'] in self.resource_config_dict: self.get_logger().info(f'资源 {resource_config["id"]} 已存在') @@ -380,15 +377,14 @@ class ResourceMeshManager(BaseROS2DeviceNode): def tf_update(self, goal_handle : ServerGoalHandle): tf_update_msg = goal_handle.request + # 获取调用节点的信息 + try: cmd_dict = json.loads(tf_update_msg.command.replace("'",'"')) self.__planning_scene = self._get_planning_scene_service.call( GetPlanningScene.Request() ).scene self.__planning_scene.is_diff = True - planning_scene = PlanningScene() - planning_scene.is_diff = True - planning_scene.robot_state.is_diff = True for resource_id, target_parent in cmd_dict.items(): # 获取从resource_id到target_parent的转换 @@ -419,32 +415,9 @@ class ResourceMeshManager(BaseROS2DeviceNode): } - # self.attach_collision_object(id=resource_id,link_name=target_parent) - # time.sleep(0.02) - operation_attach = CollisionObject.ADD - operation_remove = CollisionObject.REMOVE - if target_parent == 'world': - operation_attach = CollisionObject.REMOVE - operation_remove = CollisionObject.ADD - - remove_object = CollisionObject( - id=resource_id, - operation=operation_remove - ) - planning_scene.world.collision_objects.append(remove_object) - - - collision_object = AttachedCollisionObject( - object=CollisionObject( - id=resource_id, - operation=operation_attach - ) - ) - if target_parent != 'world': - collision_object.link_name = target_parent - planning_scene.robot_state.attached_collision_objects.append(collision_object) - + self.attach_collision_object(id=resource_id,link_name=target_parent) # collision_object = AttachedCollisionObject( + # id=resource_id, # link_name=target_parent, # object=CollisionObject( # id=resource_id, @@ -453,12 +426,9 @@ class ResourceMeshManager(BaseROS2DeviceNode): # ) # self.__planning_scene.robot_state.attached_collision_objects.append(collision_object) - req = ApplyPlanningScene.Request() - req.scene = planning_scene - self._apply_planning_scene_service.call_async(req) - self.__planning_scene_publisher.publish(planning_scene) - - # self.__collision_object_publisher.publish(CollisionObject()) + # req = ApplyPlanningScene.Request() + # req.scene = self.__planning_scene + # self._apply_planning_scene_service.call_async(req) self.publish_resource_tf() except Exception as e: @@ -482,8 +452,6 @@ class ResourceMeshManager(BaseROS2DeviceNode): self.__planning_scene = self._get_planning_scene_service.call( GetPlanningScene.Request() ).scene - planning_scene = PlanningScene() - planning_scene.is_diff = True for resource_id, tf_info in resource_tf_dict.items(): if resource_id in self.resource_model: @@ -511,7 +479,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): quat_xyzw=q, frame_id=resource_id ) - planning_scene.world.collision_objects.append(collision_object) + self.__planning_scene.world.collision_objects.append(collision_object) elif f"{tf_info['parent']}_" in self.resource_model: # 获取资源的父级框架ID id_ = f"{tf_info['parent']}_" @@ -540,13 +508,12 @@ class ResourceMeshManager(BaseROS2DeviceNode): frame_id=resource_id ) - planning_scene.world.collision_objects.append(collision_object) + self.__planning_scene.world.collision_objects.append(collision_object) req = ApplyPlanningScene.Request() - req.scene = planning_scene + req.scene = self.__planning_scene self._apply_planning_scene_service.call_async(req) - self.__planning_scene_publisher.publish(planning_scene) - self.publish_resource_tf() + self.get_logger().info('资源碰撞网格添加完成')