Merge branch 'dev' into Mock-Device/Action

unilabos/devices/liquid_handling/liquid_handler_abstract.py

@@ -6,13 +6,8 @@ import asyncio
 import time
 
 from pylabrobot.liquid_handling import LiquidHandler
-from pylabrobot.resources import (
-    Resource,
-    TipRack,
-    Container,
-    Coordinate,
-    Well
-)
+from pylabrobot.resources import Resource, TipRack, Container, Coordinate, Well
+
 
 class LiquidHandlerAbstract(LiquidHandler):
     """Extended LiquidHandler with additional operations."""
@@ -21,6 +16,19 @@ class LiquidHandlerAbstract(LiquidHandler):
     # REMOVE LIQUID --------------------------------------------------
     # ---------------------------------------------------------------
 
+    async def create_protocol(
+        self,
+        protocol_name: str,
+        protocol_description: str,
+        protocol_version: str,
+        protocol_author: str,
+        protocol_date: str,
+        protocol_type: str,
+        none_keys: List[str] = [],
+    ):
+        """Create a new protocol with the given metadata."""
+        pass
+
     async def remove_liquid(
         self,
         vols: List[float],
@@ -35,26 +43,26 @@ class LiquidHandlerAbstract(LiquidHandler):
         spread: Optional[Literal["wide", "tight", "custom"]] = "wide",
         delays: Optional[List[int]] = None,
         is_96_well: Optional[bool] = False,
-        top: Optional[List(float)] = None,
-        none_keys: List[str] = []
+        top: Optional[List[float]] = None,
+        none_keys: List[str] = [],
     ):
         """A complete *remove* (aspirate → waste) operation."""
         trash = self.deck.get_trash_area()
         try:
             if is_96_well:
-                pass # This mode is not verified
+                pass  # This mode is not verified
             else:
                 if len(vols) != len(sources):
                     raise ValueError("Length of `vols` must match `sources`.")
 
                 for src, vol in zip(sources, vols):
-                    self.move_to(src, dis_to_top=top[0] if top else 0)
+                    await self.move_to(src, dis_to_top=top[0] if top else 0)
                     tip = next(self.current_tip)
                     await self.pick_up_tips(tip)
                     await self.aspirate(
                         resources=[src],
                         vols=[vol],
-                        use_channels=use_channels, # only aspirate96 used, default to None
+                        use_channels=use_channels,  # only aspirate96 used, default to None
                         flow_rates=[flow_rates[0]] if flow_rates else None,
                         offsets=[offsets[0]] if offsets else None,
                         liquid_height=[liquid_height[0]] if liquid_height else None,
@@ -64,15 +72,15 @@ class LiquidHandlerAbstract(LiquidHandler):
                     await self.custom_delay(seconds=delays[0] if delays else 0)
                     await self.dispense(
                         resources=waste_liquid,
-                         vols=[vol],
-                         use_channels=use_channels,
-                         flow_rates=[flow_rates[1]] if flow_rates else None,
-                         offsets=[offsets[1]] if offsets else None,
-                         liquid_height=[liquid_height[1]] if liquid_height else None,
-                         blow_out_air_volume=blow_out_air_volume[1] if blow_out_air_volume else None,
-                         spread=spread,
-                     )
-                    await self.discard_tips() # For now, each of tips is discarded after use
+                        vols=[vol],
+                        use_channels=use_channels,
+                        flow_rates=[flow_rates[1]] if flow_rates else None,
+                        offsets=[offsets[1]] if offsets else None,
+                        liquid_height=[liquid_height[1]] if liquid_height else None,
+                        blow_out_air_volume=blow_out_air_volume[1] if blow_out_air_volume else None,
+                        spread=spread,
+                    )
+                    await self.discard_tips()  # For now, each of tips is discarded after use
 
         except Exception as e:
             raise RuntimeError(f"Liquid removal failed: {e}") from e
@@ -100,13 +108,13 @@ class LiquidHandlerAbstract(LiquidHandler):
         mix_vol: Optional[int] = None,
         mix_rate: Optional[int] = None,
         mix_liquid_height: Optional[float] = None,
-        none_keys: List[str] = []
+        none_keys: List[str] = [],
     ):
         """A complete *add* (aspirate reagent → dispense into targets) operation."""
 
         try:
             if is_96_well:
-                pass # This mode is not verified.
+                pass  # This mode is not verified.
             else:
                 if len(asp_vols) != len(targets):
                     raise ValueError("Length of `vols` must match `targets`.")
@@ -122,7 +130,7 @@ class LiquidHandlerAbstract(LiquidHandler):
                         offsets=[offsets[0]] if offsets else None,
                         liquid_height=[liquid_height[0]] if liquid_height else None,
                         blow_out_air_volume=[blow_out_air_volume[0]] if blow_out_air_volume else None,
-                        spread=spread
+                        spread=spread,
                     )
                     if delays is not None:
                         await self.custom_delay(seconds=delays[0])
@@ -144,7 +152,8 @@ class LiquidHandlerAbstract(LiquidHandler):
                         mix_vol=mix_vol,
                         offsets=offsets if offsets else None,
                         height_to_bottom=mix_liquid_height if mix_liquid_height else None,
-                        mix_rate=mix_rate if mix_rate else None)
+                        mix_rate=mix_rate if mix_rate else None,
+                    )
                     if delays is not None:
                         await self.custom_delay(seconds=delays[1])
                     await self.touch_tip(targets[_])
@@ -158,13 +167,13 @@ class LiquidHandlerAbstract(LiquidHandler):
     # ---------------------------------------------------------------
     async def transfer_liquid(
         self,
-        asp_vols: Union[List[float], float],
-        dis_vols: Union[List[float], float],
         sources: Sequence[Container],
         targets: Sequence[Container],
         tip_racks: Sequence[TipRack],
         *,
         use_channels: Optional[List[int]] = None,
+        asp_vols: Union[List[float], float],
+        dis_vols: Union[List[float], float],
         asp_flow_rates: Optional[List[Optional[float]]] = None,
         dis_flow_rates: Optional[List[Optional[float]]] = None,
         offsets: Optional[List[Coordinate]] = None,
@@ -179,7 +188,7 @@ class LiquidHandlerAbstract(LiquidHandler):
         mix_rate: Optional[int] = None,
         mix_liquid_height: Optional[float] = None,
         delays: Optional[List[int]] = None,
-        none_keys: List[str] = []
+        none_keys: List[str] = [],
     ):
         """Transfer liquid from each *source* well/plate to the corresponding *target*.
 
@@ -201,14 +210,15 @@ class LiquidHandlerAbstract(LiquidHandler):
             # 96‑channel head mode
             # ------------------------------------------------------------------
             if is_96_well:
-                pass # This mode is not verified
+                pass  # This mode is not verified
             else:
                 if not (len(asp_vols) == len(sources) and len(dis_vols) == len(targets)):
                     raise ValueError("`sources`, `targets`, and `vols` must have the same length.")
 
                 tip_iter = self.iter_tips(tip_racks)
-                for src, tgt, asp_vol, asp_flow_rate, dis_vol, dis_flow_rate in (
-                        zip(sources, targets, asp_vols, asp_flow_rates, dis_vols, dis_flow_rates)):
+                for src, tgt, asp_vol, asp_flow_rate, dis_vol, dis_flow_rate in zip(
+                    sources, targets, asp_vols, asp_flow_rates, dis_vols, dis_flow_rates
+                ):
                     tip = next(tip_iter)
                     await self.pick_up_tips(tip)
                     # Aspirate from source
@@ -247,9 +257,9 @@ class LiquidHandlerAbstract(LiquidHandler):
         except Exception as exc:
             raise RuntimeError(f"Liquid transfer failed: {exc}") from exc
 
-# ---------------------------------------------------------------
-# Helper utilities
-# ---------------------------------------------------------------
+    # ---------------------------------------------------------------
+    # Helper utilities
+    # ---------------------------------------------------------------
 
     async def custom_delay(self, seconds=0, msg=None):
         """
@@ -266,28 +276,26 @@ class LiquidHandlerAbstract(LiquidHandler):
                 print(f"Done: {msg}")
                 print(f"Current time: {time.strftime('%H:%M:%S')}")
 
-    async def touch_tip(self,
-                        targets: Sequence[Container],
-                        ):
+    async def touch_tip(self, targets: Sequence[Container]):
         """Touch the tip to the side of the well."""
         await self.aspirate(
             resources=[targets],
             vols=[0],
             use_channels=None,
             flow_rates=None,
-            offsets=[Coordinate(x=-targets.get_size_x()/2,y=0,z=0)],
+            offsets=[Coordinate(x=-targets.get_size_x() / 2, y=0, z=0)],
             liquid_height=None,
-            blow_out_air_volume=None
+            blow_out_air_volume=None,
         )
-        #await self.custom_delay(seconds=1) # In the simulation, we do not need to wait
+        # await self.custom_delay(seconds=1) # In the simulation, we do not need to wait
         await self.aspirate(
             resources=[targets],
             vols=[0],
             use_channels=None,
             flow_rates=None,
-            offsets=[Coordinate(x=targets.get_size_x()/2,y=0,z=0)],
+            offsets=[Coordinate(x=targets.get_size_x() / 2, y=0, z=0)],
             liquid_height=None,
-            blow_out_air_volume=None
+            blow_out_air_volume=None,
         )
 
     async def mix(
@@ -298,9 +306,9 @@ class LiquidHandlerAbstract(LiquidHandler):
         height_to_bottom: Optional[float] = None,
         offsets: Optional[Coordinate] = None,
         mix_rate: Optional[float] = None,
-        none_keys: List[str] = []
+        none_keys: List[str] = [],
     ):
-        if mix_time is None: # No mixing required
+        if mix_time is None:  # No mixing required
             return
         """Mix the liquid in the target wells."""
         for _ in range(mix_time):
@@ -333,7 +341,7 @@ class LiquidHandlerAbstract(LiquidHandler):
         tip_iter = self.iter_tips(tip_racks)
         self.current_tip = tip_iter
 
-    async def move_to(self, well: Well, dis_to_top: float = 0 , channel: int = 0):
+    async def move_to(self, well: Well, dis_to_top: float = 0, channel: int = 0):
         """
         Move a single channel to a specific well with a given z-height.
 
@@ -352,4 +360,3 @@ class LiquidHandlerAbstract(LiquidHandler):
         await self.move_channel_x(channel, abs_loc.x)
         await self.move_channel_y(channel, abs_loc.y)
         await self.move_channel_z(channel, abs_loc.z + well_height + dis_to_top)
-

unilabos/devices/liquid_handling/test liquid handler/convert_biomek.py

@@ -0,0 +1,154 @@
+
+import json
+from typing import Sequence, Optional, List, Union, Literal
+json_path = "/Users/guangxinzhang/Documents/Deep Potential/opentrons/convert/protocols/enriched_steps/sci-lucif-assay4.json"
+
+with open(json_path, "r") as f:
+    data = json.load(f)
+
+transfer_example = data[0]
+#print(transfer_example)
+
+
+temp_protocol = []
+TipLocation = "BC1025F"  # Assuming this is a fixed tip location for the transfer
+sources = transfer_example["sources"]  # Assuming sources is a list of Container objects
+targets = transfer_example["targets"]  # Assuming targets is a list of Container objects
+tip_racks = transfer_example["tip_racks"]  # Assuming tip_racks is a list of TipRack objects
+asp_vols = transfer_example["asp_vols"]  # Assuming asp_vols is a list of volumes
+solvent = "PBS"
+
+def transfer_liquid(
+        #self,
+        sources,#: Sequence[Container],
+        targets,#: Sequence[Container],
+        tip_racks,#: Sequence[TipRack],
+        TipLocation,
+        # *,
+        # use_channels: Optional[List[int]] = None,
+        asp_vols: Union[List[float], float],
+        solvent: Optional[str] = None,
+        # dis_vols: Union[List[float], float],
+        # asp_flow_rates: Optional[List[Optional[float]]] = None,
+        # dis_flow_rates: Optional[List[Optional[float]]] = None,
+        # offsets,#: Optional[List[]] = None,
+        # touch_tip: bool = False,
+        # liquid_height: Optional[List[Optional[float]]] = None,
+        # blow_out_air_volume: Optional[List[Optional[float]]] = None,
+        # spread: Literal["wide", "tight", "custom"] = "wide",
+        # is_96_well: bool = False,
+        # mix_stage: Optional[Literal["none", "before", "after", "both"]] = "none",
+        # mix_times,#: Optional[list() = None,
+        # mix_vol: Optional[int] = None,
+        # mix_rate: Optional[int] = None,
+        # mix_liquid_height: Optional[float] = None,
+        # delays: Optional[List[int]] = None,
+        # none_keys: List[str] = []
+    ):
+        # -------- Build Biomek transfer step --------
+        # 1) Construct default parameter scaffold (values mirror Biomek “Transfer” block).
+
+        transfer_params = {
+            "Span8": False,
+            "Pod": "Pod1",
+            "items": {},                       # to be filled below
+            "Wash": False,
+            "Dynamic?": True,
+            "AutoSelectActiveWashTechnique": False,
+            "ActiveWashTechnique": "",
+            "ChangeTipsBetweenDests": False,
+            "ChangeTipsBetweenSources": False,
+            "DefaultCaption": "",              # filled after we know first pair/vol
+            "UseExpression": False,
+            "LeaveTipsOn": False,
+            "MandrelExpression": "",
+            "Repeats": "1",
+            "RepeatsByVolume": False,
+            "Replicates": "1",
+            "ShowTipHandlingDetails": False,
+            "ShowTransferDetails": True,
+            "Solvent": "Water",
+            "Span8Wash": False,
+            "Span8WashVolume": "2",
+            "Span8WasteVolume": "1",
+            "SplitVolume": False,
+            "SplitVolumeCleaning": False,
+            "Stop": "Destinations",
+            "TipLocation": "BC1025F",
+            "UseCurrentTips": False,
+            "UseDisposableTips": True,
+            "UseFixedTips": False,
+            "UseJIT": True,
+            "UseMandrelSelection": True,
+            "UseProbes": [True, True, True, True, True, True, True, True],
+            "WashCycles": "1",
+            "WashVolume": "110%",
+            "Wizard": False
+        }
+
+        items: dict = {}
+        for idx, (src, dst) in enumerate(zip(sources, targets)):
+            items[str(idx)] = {
+                "Source": str(src),
+                "Destination": str(dst),
+                "Volume": asp_vols[idx] 
+            }
+        transfer_params["items"] = items
+        transfer_params["Solvent"] =  solvent if solvent else "Water"
+        transfer_params["TipLocation"] = TipLocation
+
+        if len(tip_racks) == 1:
+             transfer_params['UseCurrentTips'] = True
+        elif len(tip_racks) > 1:
+            transfer_params["ChangeTipsBetweenDests"] = True
+
+        return transfer_params
+
+action = transfer_liquid(sources=sources,targets=targets,tip_racks=tip_racks, asp_vols=asp_vols,solvent = solvent, TipLocation=TipLocation)
+print(json.dumps(action,indent=2))  
+# print(action)
+
+
+
+
+"""
+      "transfer": {
+
+        "items": {},
+        "Wash": false,
+        "Dynamic?": true,
+        "AutoSelectActiveWashTechnique": false,
+        "ActiveWashTechnique": "",
+        "ChangeTipsBetweenDests": true,
+        "ChangeTipsBetweenSources": false,
+        "DefaultCaption": "Transfer 100 µL from P13 to P3",
+        "UseExpression": false,
+        "LeaveTipsOn": false,
+        "MandrelExpression": "",
+        "Repeats": "1",
+        "RepeatsByVolume": false,
+        "Replicates": "1",
+        "ShowTipHandlingDetails": true,
+        "ShowTransferDetails": true,
+        
+        "Span8Wash": false,
+        "Span8WashVolume": "2",
+        "Span8WasteVolume": "1",
+        "SplitVolume": false,
+        "SplitVolumeCleaning": false,
+        "Stop": "Destinations",
+        "TipLocation": "BC1025F",
+        "UseCurrentTips": false,
+        "UseDisposableTips": false,
+        "UseFixedTips": false,
+        "UseJIT": true,
+        "UseMandrelSelection": true,
+        "UseProbes": [true, true, true, true, true, true, true, true],
+        "WashCycles": "3",
+        "WashVolume": "110%",
+        "Wizard": false
+"""
+
+
+
+

unilabos/devices/pump_and_valve/solenoid_valve_mock.py

@@ -5,22 +5,22 @@ class SolenoidValveMock:
     def __init__(self, port: str = "COM6"):
         self._status = "Idle"
         self._valve_position = "OPEN"
-    
+
     @property
     def status(self) -> str:
         return self._status
-    
+
     @property
     def valve_position(self) -> str:
         return self._valve_position
 
     def get_valve_position(self) -> str:
         return self._valve_position
-    
+
     def set_valve_position(self, position):
         self._status = "Busy"
         time.sleep(5)
-        
+
         self._valve_position = position
         time.sleep(5)
         self._status = "Idle"

unilabos/devices/pump_and_valve/vacuum_pump_mock.py

@@ -4,17 +4,15 @@ import time
 class VacuumPumpMock:
     def __init__(self, port: str = "COM6"):
         self._status = "OPEN"
-    
+
     @property
     def status(self) -> str:
         return self._status
 
     def get_status(self) -> str:
         return self._status
-    
+
     def set_status(self, string):
-        time.sleep(5)
-        
         self._status = string
         time.sleep(5)
 

unilabos/devices/resource_container/container.py

@@ -0,0 +1,9 @@
+
+class HotelContainer:
+    def __init__(self, rotation: dict, device_config: dict):
+        self.rotation = rotation
+        self.device_config = device_config
+        self.status = 'idle'
+
+    def get_rotation(self):
+        return self.rotation

unilabos/devices/ros_dev/lh_joint_config.json

@@ -0,0 +1,38 @@
+{
+    "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.166
+            }
+        },
+        "x":{
+            "second_joint":{
+            "factor":-0.001,
+            "offset":0.1775
+            }
+        },
+        "z":{
+            "third_joint":{
+            "factor":0.001,
+            "offset":0.0
+            },
+            "fourth_joint":{
+            "factor":0.001,
+            "offset":0.0
+            }
+        }
+    }
+}

unilabos/devices/ros_dev/liquid_handler_joint_publisher.py

@@ -1,9 +1,12 @@
+import asyncio
 import copy
+from pathlib import Path
+import threading
 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 +14,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,resources_config:list, resource_tracker, rate=50, device_id:str = "lh_joint_publisher"):
         super().__init__(
             driver_instance=self,
             device_id=device_id,
@@ -23,60 +28,118 @@ 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(f"{Path(__file__).parent.absolute()}/lh_joint_config.json", encoding="utf-8"))
+        self.resources_config = {x['id']:x for x in resources_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 resources_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'{deck_id}'
+                # 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)
+
+        print('='*20)
+        print(self.lh_devices)
+        print('='*20)
         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.resources_config[resource_id]['parent']
+        try:
+            if parent_id in self.deck_list:
+                p_ = self.resources_config[parent_id]['parent']
+                str_ = f'{parent_id}'
+                return str(str_)
+            else:
+                return self.find_resource_parent(parent_id)
+        except Exception as e:
+            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(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
 
@@ -101,12 +164,13 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
             goal_handle.succeed()
             
         except Exception as e:
-            print(e)
+            print(f'Liquid handler action error: \n{e}')
             goal_handle.abort()
             result.success = False
-
+        
         return result
     def inverse_kinematics(self, x, y, z, 
+                           parent_id,
                            x_joint:dict, 
                            y_joint:dict, 
                            z_joint:dict   ):
@@ -117,77 +181,102 @@ 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"]
-            
-        
-        return joint_positions
+            z_index = index
+
+        return joint_positions ,z_index
 
 
-    def move_joints(self, resource_name, link_name, speed, x_joint=None, y_joint=None, z_joint=None):
+    def move_joints(self, resource_names, x, y, z, option, speed = 0.1 ,x_joint=None, y_joint=None, z_joint=None):
+        if isinstance(resource_names, list):
+            resource_name_ = resource_names[0]
+        else:
+            resource_name_ = resource_names
         
-        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
+        parent_id = self.find_resource_parent(resource_name_)
+
+
+        print('!'*20)
+        print(parent_id)
+        print('!'*20)
         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]
+            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]
         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]
+            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]
         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]
+            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]
         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)
+        time.sleep(1)
+        if option == "pick":
+            link_name =  self.lh_devices[parent_id]['joint_config']['link_names'][z_index]
+            link_name =  f'{parent_id}_{link_name}'
+            self.resource_move(resource_name_, link_name, [0,1,2,3,4,5,6,7])
+        elif option == "drop_trash":
+            self.resource_move(resource_name_, "__trash", [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 +284,103 @@ 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_name, x,y,z,option, speed = 0.1,x_joint=None, y_joint=None, z_joint=None):
+        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
+        }
+        
+
+        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.MultiThreadedExecutor()
+            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)
+
 
 def main():
 

unilabos/devices/ros_dev/moveit_interface.py

@@ -0,0 +1,384 @@
+import json
+import time
+from copy import deepcopy
+from pathlib import Path
+
+from moveit_msgs.msg import JointConstraint, Constraints
+from rclpy.action import ActionClient
+from tf2_ros import Buffer, TransformListener
+from unilabos_msgs.action import SendCmd
+
+from unilabos.devices.ros_dev.moveit2 import MoveIt2
+from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
+
+
+class MoveitInterface:
+    _ros_node: BaseROS2DeviceNode
+    tf_buffer: Buffer
+    tf_listener: TransformListener
+
+    def __init__(self, moveit_type, joint_poses, rotation=None, device_config=None):
+        self.device_config = device_config
+        self.rotation = rotation
+        self.data_config = json.load(
+            open(
+                f"{Path(__file__).parent.parent.parent.absolute()}/device_mesh/devices/{moveit_type}/config/move_group.json",
+                encoding="utf-8",
+            )
+        )
+        self.arm_move_flag = False
+        self.move_option = ["pick", "place", "side_pick", "side_place"]
+        self.joint_poses = joint_poses
+        self.cartesian_flag = False
+        self.mesh_group = ["reactor", "sample", "beaker"]
+        self.moveit2 = {}
+        self.resource_action = None
+        self.resource_client = None
+        self.resource_action_ok = False
+
+
+    def post_init(self, ros_node: BaseROS2DeviceNode):
+        self._ros_node = ros_node
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self._ros_node)
+
+        for move_group, config in self.data_config.items():
+
+            base_link_name = f"{self._ros_node.device_id}_{config['base_link_name']}"
+            end_effector_name = f"{self._ros_node.device_id}_{config['end_effector_name']}"
+            joint_names = [f"{self._ros_node.device_id}_{name}" for name in config["joint_names"]]
+
+            self.moveit2[f"{move_group}"] = MoveIt2(
+                node=self._ros_node,
+                joint_names=joint_names,
+                base_link_name=base_link_name,
+                end_effector_name=end_effector_name,
+                group_name=f"{self._ros_node.device_id}_{move_group}",
+                callback_group=self._ros_node.callback_group,
+                use_move_group_action=True,
+                ignore_new_calls_while_executing=True,
+            )
+            self.moveit2[f"{move_group}"].allowed_planning_time = 3.0
+
+        self._ros_node.create_timer(1, self.wait_for_resource_action, callback_group=self._ros_node.callback_group)
+
+
+    def wait_for_resource_action(self):
+        if not self.resource_action_ok:
+
+            while self.resource_action is None:
+                self.resource_action = self.check_tf_update_actions()
+                time.sleep(1)
+            self.resource_client = ActionClient(self._ros_node, SendCmd, self.resource_action)
+            self.resource_action_ok = True
+            while not self.resource_client.wait_for_server(timeout_sec=5.0):
+                self._ros_node.lab_logger().info("等待 TfUpdate 服务器...")
+
+    def check_tf_update_actions(self):
+        topics = self._ros_node.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 set_position(self, command):
+        """使用moveit 移动到指定点
+        Args:
+            command: A JSON-formatted string that includes quaternion, speed, position
+
+                    position (list)     : 点的位置 [x,y,z]
+                    quaternion (list)   : 点的姿态(四元数) [x,y,z,w]
+                    move_group (string) : The move group moveit will plan
+                    speed (float)       : The speed of the movement, speed > 0
+                    retry (int)         : Retry times when moveit plan fails
+
+        Returns:
+            None
+        """
+
+        result = SendCmd.Result()
+        cmd_str = command.replace("'", '"')
+        cmd_dict = json.loads(cmd_str)
+        self.moveit_task(**cmd_dict)
+        return result
+
+    def moveit_task(
+        self, move_group, position, quaternion, speed=1, retry=10, cartesian=False, target_link=None, offsets=[0, 0, 0]
+    ):
+
+        speed_ = float(max(0.1, min(speed, 1)))
+
+        self.moveit2[move_group].max_velocity = speed_
+        self.moveit2[move_group].max_acceleration = speed_
+
+        re_ = False
+
+        pose_result = [x + y for x, y in zip(position, offsets)]
+        # print(pose_result)
+
+        while retry > -1 and not re_:
+
+            self.moveit2[move_group].move_to_pose(
+                target_link=target_link,
+                position=pose_result,
+                quat_xyzw=quaternion,
+                cartesian=cartesian,
+                # cartesian_fraction_threshold=0.0,
+                cartesian_max_step=0.01,
+                weight_position=1.0,
+            )
+            re_ = self.moveit2[move_group].wait_until_executed()
+            retry += -1
+
+        return re_
+
+    def moveit_joint_task(self, move_group, joint_positions, joint_names=None, speed=1, retry=10):
+
+        re_ = False
+
+        joint_positions_ = [float(x) for x in joint_positions]
+
+        speed_ = float(max(0.1, min(speed, 1)))
+
+        self.moveit2[move_group].max_velocity = speed_
+        self.moveit2[move_group].max_acceleration = speed_
+
+        while retry > -1 and not re_:
+
+            self.moveit2[move_group].move_to_configuration(joint_positions=joint_positions_, joint_names=joint_names)
+            re_ = self.moveit2[move_group].wait_until_executed()
+
+            retry += -1
+            print(self.moveit2[move_group].compute_fk(joint_positions))
+        return re_
+
+    def resource_manager(self, resource, parent_link):
+        goal_msg = SendCmd.Goal()
+        str_dict = {}
+        str_dict[resource] = parent_link
+
+        goal_msg.command = json.dumps(str_dict)
+        assert self.resource_client is not None
+        self.resource_client.send_goal(goal_msg)
+
+        return True
+
+    def pick_and_place(self, command: str):
+        """
+        Using MoveIt to make the robotic arm pick or place materials to a target point.
+
+        Args:
+            command: A JSON-formatted string that includes option, target, speed, lift_height, mt_height
+
+                    *option (string)    : Action type: pick/place/side_pick/side_place
+                    *move_group (string): The move group moveit will plan
+                    *status(string)     : Target pose
+                    resource(string)    : The target resource
+                    x_distance (float)  : The distance to the target in x direction(meters)
+                    y_distance (float)  : The distance to the target in y direction(meters)
+                    lift_height (float) : The height at which the material should be lifted(meters)
+                    retry (float)       : Retry times when moveit plan fails
+                    speed (float)       : The speed of the movement, speed > 0
+        Returns:
+            None
+        """
+        result = SendCmd.Result()
+
+        try:
+            cmd_str = str(command).replace("'", '"')
+            cmd_dict = json.loads(cmd_str)
+
+            if cmd_dict["option"] in self.move_option:
+                option_index = self.move_option.index(cmd_dict["option"])
+                place_flag = option_index % 2
+
+                config = {}
+                function_list = []
+
+                status = cmd_dict["status"]
+                joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
+
+                config.update({k: cmd_dict[k] for k in ["speed", "retry", "move_group"] if k in cmd_dict})
+
+                # 夹取
+                if not place_flag:
+                    if "target" in cmd_dict.keys():
+                        function_list.append(lambda: self.resource_manager(cmd_dict["resource"], cmd_dict["target"]))
+                    else:
+                        function_list.append(
+                            lambda: self.resource_manager(
+                                cmd_dict["resource"], self.moveit2[cmd_dict["move_group"]].end_effector_name
+                            )
+                        )
+                else:
+                    function_list.append(lambda: self.resource_manager(cmd_dict["resource"], "world"))
+
+                constraints = []
+                if "constraints" in cmd_dict.keys():
+
+                    for i in range(len(cmd_dict["constraints"])):
+                        v = float(cmd_dict["constraints"][i])
+                        if v > 0:
+                            constraints.append(
+                                JointConstraint(
+                                    joint_name=self.moveit2[cmd_dict["move_group"]].joint_names[i],
+                                    position=joint_positions_[i],
+                                    tolerance_above=v,
+                                    tolerance_below=v,
+                                    weight=1.0,
+                                )
+                            )
+
+                if "lift_height" in cmd_dict.keys():
+                    retval = None
+                    retry = config.get("retry", 10)
+                    while retval is None and retry > 0:
+                        retval = self.moveit2[cmd_dict["move_group"]].compute_fk(joint_positions_)
+                        time.sleep(0.1)
+                        retry -= 1
+                    if retval is None:
+                        result.success = False
+                        return result
+                    pose = [retval.pose.position.x, retval.pose.position.y, retval.pose.position.z]
+                    quaternion = [
+                        retval.pose.orientation.x,
+                        retval.pose.orientation.y,
+                        retval.pose.orientation.z,
+                        retval.pose.orientation.w,
+                    ]
+
+                    function_list = [
+                        lambda: self.moveit_task(
+                            position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z],
+                            quaternion=quaternion,
+                            **config,
+                            cartesian=self.cartesian_flag,
+                        )
+                    ] + function_list
+
+                    pose[2] += float(cmd_dict["lift_height"])
+                    function_list.append(
+                        lambda: self.moveit_task(
+                            position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
+                        )
+                    )
+                    end_pose = pose
+
+                    if "x_distance" in cmd_dict.keys() or "y_distance" in cmd_dict.keys():
+                        if "x_distance" in cmd_dict.keys():
+                            deep_pose = deepcopy(pose)
+                            deep_pose[0] += float(cmd_dict["x_distance"])
+                        elif "y_distance" in cmd_dict.keys():
+                            deep_pose = deepcopy(pose)
+                            deep_pose[1] += float(cmd_dict["y_distance"])
+
+                        function_list = [
+                            lambda: self.moveit_task(
+                                position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
+                            )
+                        ] + function_list
+                        function_list.append(
+                            lambda: self.moveit_task(
+                                position=deep_pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
+                            )
+                        )
+                        end_pose = deep_pose
+
+                    retval_ik = None
+                    retry = config.get("retry", 10)
+                    while retval_ik is None and retry > 0:
+                        retval_ik = self.moveit2[cmd_dict["move_group"]].compute_ik(
+                            position=end_pose, quat_xyzw=quaternion, constraints=Constraints(joint_constraints=constraints)
+                        )
+                        time.sleep(0.1)
+                        retry -= 1
+                    if retval_ik is None:
+                        result.success = False
+                        return result
+                    position_ = [
+                        retval_ik.position[retval_ik.name.index(i)]
+                        for i in self.moveit2[cmd_dict["move_group"]].joint_names
+                    ]
+                    function_list = [
+                        lambda: self.moveit_joint_task(
+                            joint_positions=position_,
+                            joint_names=self.moveit2[cmd_dict["move_group"]].joint_names,
+                            **config,
+                        )
+                    ] + function_list
+                else:
+                    function_list = [
+                        lambda: self.moveit_joint_task(**config, joint_positions=joint_positions_)
+                    ] + function_list
+
+                for i in range(len(function_list)):
+                    if i == 0:
+                        self.cartesian_flag = False
+                    else:
+                        self.cartesian_flag = True
+
+                    re = function_list[i]()
+                    if not re:
+                        print(i, re)
+                        result.success = False
+                        return result
+                result.success = True
+
+        except Exception as e:
+            print(e)
+            self.cartesian_flag = False
+            result.success = False
+
+        return result
+
+    def set_status(self, command: str):
+        """
+        Goto home position
+
+            Args:
+                command: A JSON-formatted string that includes speed
+                        *status (string)    : The joint status moveit will plan
+                        *move_group (string): The move group moveit will plan
+                        separate (list)     : The joint index to be separated
+                        lift_height (float) : The height at which the material should be lifted(meters)
+                        x_distance (float)  : The distance to the target in x direction(meters)
+                        y_distance (float)  : The distance to the target in y direction(meters)
+                        speed (float)       : The speed of the movement, speed > 0
+                        retry (float)       : Retry times when moveit plan fails
+
+            Returns:
+                None
+        """
+
+        result = SendCmd.Result()
+
+        try:
+            cmd_str = command.replace("'", '"')
+            cmd_dict = json.loads(cmd_str)
+            config = {}
+            config["move_group"] = cmd_dict["move_group"]
+            if "speed" in cmd_dict.keys():
+                config["speed"] = cmd_dict["speed"]
+            if "retry" in cmd_dict.keys():
+                config["retry"] = cmd_dict["retry"]
+
+            status = cmd_dict["status"]
+            joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
+            re = self.moveit_joint_task(**config, joint_positions=joint_positions_)
+            if not re:
+                result.success = False
+                return result
+            result.success = True
+        except Exception as e:
+            print(e)
+            result.success = False
+
+        return result