提取lh的joint发布

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": []

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: <Fixed 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
+  Width: 4096
-  X: 385
+  X: 0
-  Y: 120
+  Y: 0

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
+            }
+        }
+    }
+}

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
+        joint_config   = json.load(open('./lh_joint_config.json', encoding="utf-8"))
-        # self.j_msg.name     = joint_names
+        self.resource_config = resource_config
-        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.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
+            x_joint (dict): x轴关节配置，包含factor和offset
-            y_joint (dict): y轴关节配置，包含plus和offset
+            y_joint (dict): y轴关节配置，包含factor和offset
-            z_joint (dict): z轴关节配置，包含plus和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()))
+            x_joint_config = next(iter(self.lh_devices[parent_id]['x'].items()))
-        elif x_joint in self.joint_config['x']:
+        elif x_joint in self.lh_devices[parent_id]['x']:
-            x_joint_config = self.joint_config['x'][x_joint]
+            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()))
+            y_joint_config = next(iter(self.lh_devices[parent_id]['y'].items()))
-        elif y_joint in self.joint_config['y']:
+        elif y_joint in self.lh_devices[parent_id]['y']:
-            y_joint_config = self.joint_config['y'][y_joint]
+            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()))
+            z_joint_config = next(iter(self.lh_devices[parent_id]['z'].items()))
-        elif z_joint in self.joint_config['z']:
+        elif z_joint in self.lh_devices[parent_id]['z']:
-            z_joint_config = self.joint_config['z'][z_joint]
+            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(joint_positions):
-        while loop_flag < len(self.joint_config['joint_names']):
             loop_flag = 0
-            for i in range(len(self.joint_config['joint_names'])):
+            for i in range(len(joint_positions)):
-                distance = joint_positions_target[i] - self.j_msg.position[i]
+                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()
+        for id, config in self.lh_devices.items():
-        self.j_pub.publish(self.j_msg)
+            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():
 

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)
+                self.attach_collision_object(id=resource_id,link_name=target_parent)
-            req = ApplyPlanningScene.Request()
+            #     collision_object = AttachedCollisionObject(
-            req.scene = self.__planning_scene
+            #         id=resource_id,
-            self._apply_planning_scene_service.call_async(req)
+            #         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)
         )