diff --git a/test/experiments/plr_test_converted.json b/test/experiments/plr_test_converted.json index 533d99c3..9e502e10 100644 --- a/test/experiments/plr_test_converted.json +++ b/test/experiments/plr_test_converted.json @@ -9584,6 +9584,45 @@ "pending_liquids": [], "liquid_history": [] } + }, + { + "id": "plate_well_H12", + "name": "plate_well_H12", + "sample_id": null, + "children": [], + "parent": "plate_well", + "type": "device", + "class": "", + "position": { + "x": 109.87, + "y": 7.77, + "z": 3.03 + }, + "config": { + "type": "Well", + "size_x": 6.86, + "size_y": 6.86, + "size_z": 10.67, + "rotation": { + "x": 0, + "y": 0, + "z": 0, + "type": "Rotation" + }, + "category": "well", + "model": null, + "max_volume": 360, + "material_z_thickness": 0.5, + "compute_volume_from_height": null, + "compute_height_from_volume": null, + "bottom_type": "flat", + "cross_section_type": "circle" + }, + "data": { + "liquids": [], + "pending_liquids": [], + "liquid_history": [] + } } ], "links": [] diff --git a/unilabos/device_mesh/view_robot.rviz b/unilabos/device_mesh/view_robot.rviz index 64f6b358..07bf15df 100644 --- a/unilabos/device_mesh/view_robot.rviz +++ b/unilabos/device_mesh/view_robot.rviz @@ -345,7 +345,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1.0284695625305176 + Distance: 1.595012903213501 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -363,25 +363,25 @@ Visualization Manager: Pitch: 0.38979560136795044 Target Frame: Value: Orbit (rviz) - Yaw: 0.06074193865060806 + Yaw: 0.05074193701148033 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1656 + Height: 2160 Hide Left Dock: false Hide Right Dock: true MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000002510000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002cb0000037f000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000627000005dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000006e000002510000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002cb0000037f000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000010000000087000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 2518 - X: 385 - Y: 120 + Width: 4096 + X: 0 + Y: 0 diff --git a/unilabos/devices/ros_dev/lh_joint_config.json b/unilabos/devices/ros_dev/lh_joint_config.json new file mode 100644 index 00000000..6fb8c1f2 --- /dev/null +++ b/unilabos/devices/ros_dev/lh_joint_config.json @@ -0,0 +1,32 @@ +{ + "OTdeck":{ + "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 + } + } + } +} \ No newline at end of file diff --git a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py index e593f425..3f160070 100644 --- a/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py +++ b/unilabos/devices/ros_dev/liquid_handler_joint_publisher.py @@ -3,7 +3,7 @@ import rclpy import json import time from rclpy.executors import MultiThreadedExecutor -from rclpy.action import ActionServer +from rclpy.action import ActionServer,ActionClient from sensor_msgs.msg import JointState from unilabos_msgs.action import SendCmd from rclpy.action.server import ServerGoalHandle @@ -11,9 +11,11 @@ from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode from tf_transformations import quaternion_from_euler from tf2_ros import TransformBroadcaster, Buffer, TransformListener +from rclpy.node import Node +import re class LiquidHandlerJointPublisher(BaseROS2DeviceNode): - def __init__(self,device_id:str, joint_config:dict, lh_id:str,resource_tracker, rate=50): + def __init__(self,resource_config:list, resource_tracker, rate=50, device_id:str = "lh_joint_publisher"): super().__init__( driver_instance=self, device_id=device_id, @@ -23,60 +25,113 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): print_publish=False, resource_tracker=resource_tracker, ) - - # joint_config_dict = { - # "joint_names":[ - # "first_joint", - # "second_joint", - # "third_joint", - # "fourth_joint" - # ], - # "y":{ - # "first_joint":{ - # "factor":-1, - # "offset":0.0 - # } - # }, - # "x":{ - # "second_joint":{ - # "factor":-1, - # "offset":0.0 - # } - # }, - # "z":{ - # "third_joint":{ - # "factor":1, - # "offset":0.0 - # }, - # "fourth_joint":{ - # "factor":1, - # "offset":0.0 - # } - # } - # } - + + # 初始化参数 self.j_msg = JointState() - self.lh_id = lh_id - # self.j_msg.name = joint_names - self.joint_config = joint_config - self.j_msg.position = [0.0 for i in range(len(joint_config['joint_names']))] - self.j_msg.name = [f"{self.lh_id}_{x}" for x in joint_config['joint_names']] - # self.joint_config = joint_config_dict - # self.j_msg.position = [0.0 for i in range(len(joint_config_dict['joint_names']))] - # self.j_msg.name = [f"{self.lh_id}_{x}" for x in joint_config_dict['joint_names']] + joint_config = json.load(open('./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) self.j_pub = self.create_publisher(JointState,'/joint_states',10) - self.create_timer(0.02,self.lh_joint_pub_callback) + self.create_timer(1,self.lh_joint_pub_callback) + + + self.resource_action = None + + while self.resource_action is None: + self.resource_action = self.check_tf_update_actions() + time.sleep(1) + + self.resource_action_client = ActionClient(self, SendCmd, self.resource_action) + while not self.resource_action_client.wait_for_server(timeout_sec=1.0): + self.get_logger().info('等待 TfUpdate 服务器...') + + self.deck_list = [] + self.lh_devices = {} + # 初始化设备ID与config信息 + for resource in 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] + key = f'{resource["id"]}_{deck_id}' + self.lh_devices[key] = { + 'joint_msg':JointState( + name=[f'{key}_{x}' for x in joint_config[deck_class]['joint_names']], + position=[0.0 for _ in joint_config[deck_class]['joint_names']] + ), + 'joint_config':joint_config[deck_class] + } + self.deck_list.append(deck_id) + + self.j_action = ActionServer( self, SendCmd, - "joint", + "hl_joint_action", self.lh_joint_action_callback, result_timeout=5000 ) + + def check_tf_update_actions(self): + topics = self.get_topic_names_and_types() + + + for topic_item in topics: + + topic_name, topic_types = topic_item + + if 'action_msgs/msg/GoalStatusArray' in topic_types: + # 删除 /_action/status 部分 + + base_name = topic_name.replace('/_action/status', '') + # 检查最后一个部分是否为 tf_update + parts = base_name.split('/') + if parts and parts[-1] == 'tf_update': + return base_name + + return None + + + def find_resource_parent(self, resource_id:str): + # 遍历父辈，找到父辈的父辈，直到找到设备ID + parent_id = self.resource_config[resource_id]['parent'] + try: + if parent_id in self.deck_list: + return f'{self.resource_config[parent_id]['parent']}_{parent_id}' + else: + self.find_resource_parent(parent_id) + except: + return None + + + def send_resource_action(self, resource_id_list:list[str], link_name:str): + goal_msg = SendCmd.Goal() + str_dict = {} + for resource in resource_id_list: + str_dict[resource] = link_name + + goal_msg.command = json.dumps(str_dict) + self.resource_action_client.send_goal_async(goal_msg) + + def resource_move(self, resource_id:str, link_name:str, channels:list[int]): + resource = resource_id.rsplit("_",1) + + channel_list = ['A','B','C','D','E','F','G','H'] + + resource_list = [] + match = re.match(r'([a-zA-Z_]+)(\d+)', resource[1]) + if match: + number = match.group(2) + for channel in channels: + resource_list.append(f"{resource[0]}_{channel_list[channel]}{number}") + + if len(resource_list) > 0: + self.send_resource_action(resource_list, link_name) + + + def lh_joint_action_callback(self,goal_handle: ServerGoalHandle): """Move a single joint @@ -107,6 +162,7 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): return result def inverse_kinematics(self, x, y, z, + parent_id, x_joint:dict, y_joint:dict, z_joint:dict ): @@ -117,77 +173,90 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): x (float): x坐标 y (float): y坐标 z (float): z坐标 - x_joint (dict): x轴关节配置，包含plus和offset - y_joint (dict): y轴关节配置，包含plus和offset - z_joint (dict): z轴关节配置，包含plus和offset + x_joint (dict): x轴关节配置，包含factor和offset + y_joint (dict): y轴关节配置，包含factor和offset + z_joint (dict): z轴关节配置，包含factor和offset Returns: dict: 关节名称和对应位置的字典 """ - joint_positions = copy.deepcopy(self.j_msg.position) + joint_positions = copy.deepcopy(self.lh_devices[parent_id]['joint_msg'].position) + z_index = 0 # 处理x轴关节 for joint_name, config in x_joint.items(): - index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}") + index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}") joint_positions[index] = x * config["factor"] + config["offset"] # 处理y轴关节 for joint_name, config in y_joint.items(): - index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}") + index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}") joint_positions[index] = y * config["factor"] + config["offset"] # 处理z轴关节 for joint_name, config in z_joint.items(): - index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}") + index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}") joint_positions[index] = z * config["factor"] + config["offset"] - + z_index = index - return joint_positions + return joint_positions ,z_index + + + 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_) - 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] + 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: - 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] + 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: - 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] + 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 = self.inverse_kinematics(x,y,z,x_joint_config,y_joint_config,z_joint_config) + joint_positions_target, z_index = self.inverse_kinematics(x,y,z,parent_id,x_joint_config,y_joint_config,z_joint_config) + joint_positions_target_zero = copy.deepcopy(joint_positions_target) + joint_positions_target_zero[z_index] = 0 + self.move_to(joint_positions_target_zero, speed, parent_id) + self.move_to(joint_positions_target, speed, parent_id) + if option == "pick": + 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]) + elif option == "drop": + self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7]) + self.move_to(joint_positions_target_zero, speed, parent_id) + + + def move_to(self, joint_positions ,speed, parent_id): loop_flag = 0 - - while loop_flag < len(self.joint_config['joint_names']): + while loop_flag < len(joint_positions): loop_flag = 0 - for i in range(len(self.joint_config['joint_names'])): - distance = joint_positions_target[i] - self.j_msg.position[i] + for i in range(len(joint_positions)): + distance = joint_positions[i] - self.lh_devices[parent_id]['joint_msg'].position[i] if distance == 0: loop_flag += 1 continue minus_flag = distance/abs(distance) if abs(distance) > speed/self.rate: - self.j_msg.position[i] += minus_flag * speed/self.rate + self.lh_devices[parent_id]['joint_msg'].position[i] += minus_flag * speed/self.rate else : - self.j_msg.position[i] = joint_positions_target[i] + self.lh_devices[parent_id]['joint_msg'].position[i] = joint_positions[i] loop_flag += 1 @@ -195,10 +264,58 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode): self.lh_joint_pub_callback() time.sleep(1/self.rate) - def lh_joint_pub_callback(self): - self.j_msg.header.stamp = self.get_clock().now().to_msg() - self.j_pub.publish(self.j_msg) + for id, config in self.lh_devices.items(): + config['joint_msg'].header.stamp = self.get_clock().now().to_msg() + self.j_pub.publish(config['joint_msg']) + + + + +class JointStatePublisher(Node): + def __init__(self): + super().__init__('joint_state_publisher') + + self.lh_action = None + + while self.lh_action is None: + self.lh_action = self.check_hl_joint_actions() + time.sleep(1) + + self.lh_action_client = ActionClient(self, SendCmd, self.lh_action) + while not self.lh_action_client.wait_for_server(timeout_sec=1.0): + self.get_logger().info('等待 TfUpdate 服务器...') + + + + def check_hl_joint_actions(self): + topics = self.get_topic_names_and_types() + + + for topic_item in topics: + + topic_name, topic_types = topic_item + + if 'action_msgs/msg/GoalStatusArray' in topic_types: + # 删除 /_action/status 部分 + + base_name = topic_name.replace('/_action/status', '') + # 检查最后一个部分是否为 tf_update + parts = base_name.split('/') + if parts and parts[-1] == 'hl_joint_action': + return base_name + + return None + + def send_resource_action(self, resource_id_list:list[str], link_name:str): + goal_msg = SendCmd.Goal() + str_dict = {} + for resource in resource_id_list: + str_dict[resource] = link_name + + goal_msg.command = json.dumps(str_dict) + self.lh_action_client.send_goal_async(goal_msg) + def main(): diff --git a/unilabos/ros/nodes/presets/resource_mesh_manager.py b/unilabos/ros/nodes/presets/resource_mesh_manager.py index c1495818..b8ed3257 100644 --- a/unilabos/ros/nodes/presets/resource_mesh_manager.py +++ b/unilabos/ros/nodes/presets/resource_mesh_manager.py @@ -92,7 +92,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): CollisionObject, "/collision_object", 10 ) self.__attached_collision_object_publisher = self.create_publisher( - AttachedCollisionObject, "/attached_collision_object", 10 + AttachedCollisionObject, "/attached_collision_object", 0 ) # 创建一个Action Server用于修改resource_tf_dict @@ -121,7 +121,6 @@ class ResourceMeshManager(BaseROS2DeviceNode): """检查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: @@ -129,8 +128,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): self.move_group_ready = True self.publish_resource_tf() self.add_resource_collision_meshes(self.resource_tf_dict) - - # time.sleep(1) + def add_resource_mesh_callback(self, goal_handle : ServerGoalHandle): tf_update_msg = goal_handle.request @@ -187,7 +185,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): pass elif parent is not None and resource_id in self.resource_model: - parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None","") + parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None_","") else: @@ -344,9 +342,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): self.resource_pose_publisher.publish(changed_poses_msg) self.zero_count += 1 - - - + def _is_pose_equal(self, pose1, pose2, tolerance=1e-7): """ 比较两个位姿是否相等（考虑浮点数精度） @@ -381,12 +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 for resource_id, target_parent in cmd_dict.items(): # 获取从resource_id到target_parent的转换 @@ -416,20 +414,21 @@ class ResourceMeshManager(BaseROS2DeviceNode): "rotation": rotation } - # 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, - operation=CollisionObject.ADD - ) - ) - self.__planning_scene.robot_state.attached_collision_objects.append(collision_object) - req = ApplyPlanningScene.Request() - req.scene = self.__planning_scene - self._apply_planning_scene_service.call_async(req) + 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, + # operation=CollisionObject.ADD + # ) + # ) + + # self.__planning_scene.robot_state.attached_collision_objects.append(collision_object) + # req = ApplyPlanningScene.Request() + # req.scene = self.__planning_scene + # self._apply_planning_scene_service.call_async(req) self.publish_resource_tf() except Exception as e: @@ -440,6 +439,7 @@ class ResourceMeshManager(BaseROS2DeviceNode): return SendCmd.Result(success=True) + def add_resource_collision_meshes(self,resource_tf_dict:dict): """ 遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格 @@ -959,9 +959,6 @@ class ResourceMeshManager(BaseROS2DeviceNode): Attach collision object to the robot. """ - if link_name is None: - link_name = self.__end_effector_name - msg = AttachedCollisionObject( object=CollisionObject(id=id, operation=CollisionObject.ADD) )