3d sim (#97)

* 修改lh的json启动

* 修改lh的json启动

* 修改backend，做成sim的通用backend

* 修改yaml的地址，3D模型适配网页生产环境

* 添加laiyu硬件连接

* 修改移液枪的状态判断方法，

修改移液枪的状态判断方法，
添加三轴的表定点与零点之间的转换
添加三轴真实移动的backend

* 修改laiyu移液站

简化移动方法，
取消软件限制位置，
修改当值使用Z轴时也需要重新复位Z轴的问题

* 更新lh以及laiyu workshop

1,现在可以直接通过修改backend，适配其他的移液站，主类依旧使用LiquidHandler，不用重新编写

2,修改枪头判断标准，使用枪头自身判断而不是类的判断，

3,将归零参数用毫米计算，方便手动调整，

4,修改归零方式，上电使用机械归零，确定机械零点，手动归零设置工作区域零点方便计算，二者互不干涉

* 修改枪头动作

* 修改虚拟仿真方法

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: Junhan Chang <changjh@dp.tech>

unilabos/devices/liquid_handling/rviz_backend.py

@@ -1,5 +1,6 @@
-      
+
 import json
+import threading
 from typing import List, Optional, Union
 
 from pylabrobot.liquid_handling.backends.backend import (
@@ -30,7 +31,7 @@ from rclpy.action import ActionClient
 from unilabos_msgs.action import SendCmd
 import re
 
-from unilabos.devices.ros_dev.liquid_handler_joint_publisher import JointStatePublisher
+from unilabos.devices.ros_dev.liquid_handler_joint_publisher_node import LiquidHandlerJointPublisher
 
 
 class UniLiquidHandlerRvizBackend(LiquidHandlerBackend):
@@ -48,27 +49,44 @@ class UniLiquidHandlerRvizBackend(LiquidHandlerBackend):
   _max_volume_length = 16
   _fitting_depth_length = 20
   _tip_length_length = 16
-  # _pickup_method_length = 20
   _filter_length = 10
 
-  def __init__(self, num_channels: int = 8 , tip_length: float = 0 , total_height: float = 310):
+  def __init__(self, num_channels: int = 8 , tip_length: float = 0 , total_height: float = 310, **kwargs):
     """Initialize a chatter box backend."""
     super().__init__()
     self._num_channels = num_channels
     self.tip_length = tip_length
     self.total_height = total_height
-# rclpy.init()
+    self.joint_config = kwargs.get("joint_config", None)
+    self.lh_device_id = kwargs.get("lh_device_id", "lh_joint_publisher")
     if not rclpy.ok():
         rclpy.init()
     self.joint_state_publisher = None
+    self.executor = None
+    self.executor_thread = None
 
   async def setup(self):
-    self.joint_state_publisher = JointStatePublisher()
+    self.joint_state_publisher = LiquidHandlerJointPublisher(
+                                joint_config=self.joint_config,
+                                lh_device_id=self.lh_device_id,
+                                simulate_rviz=True)
+
+    # 启动ROS executor
+    self.executor = rclpy.executors.MultiThreadedExecutor()
+    self.executor.add_node(self.joint_state_publisher)
+    self.executor_thread = threading.Thread(target=self.executor.spin, daemon=True)
+    self.executor_thread.start()
+
     await super().setup()
 
     print("Setting up the liquid handler.")
 
   async def stop(self):
+    # 停止ROS executor
+    if self.executor and self.joint_state_publisher:
+        self.executor.remove_node(self.joint_state_publisher)
+    if self.executor_thread and self.executor_thread.is_alive():
+        self.executor.shutdown()
     print("Stopping the liquid handler.")
 
   def serialize(self) -> dict:
@@ -123,7 +141,7 @@ class UniLiquidHandlerRvizBackend(LiquidHandlerBackend):
     y = coordinate.y + offset_xyz.y
     z = self.total_height - (coordinate.z + self.tip_length) + offset_xyz.z
     # print("moving")
-    self.joint_state_publisher.send_resource_action(ops[0].resource.name, x, y, z, "pick",channels=use_channels)
+    self.joint_state_publisher.move_joints(ops[0].resource.name, x, y, z, "pick",channels=use_channels)
     #   goback()
 
 
@@ -166,7 +184,7 @@ class UniLiquidHandlerRvizBackend(LiquidHandlerBackend):
     z = self.total_height - (coordinate.z + self.tip_length) + offset_xyz.z
     # print(x, y, z)
     # print("moving")
-    self.joint_state_publisher.send_resource_action(ops[0].resource.name, x, y, z, "drop_trash",channels=use_channels)
+    self.joint_state_publisher.move_joints(ops[0].resource.name, x, y, z, "drop_trash",channels=use_channels)
     #   goback()
 
   async def aspirate(
@@ -216,7 +234,7 @@ class UniLiquidHandlerRvizBackend(LiquidHandlerBackend):
     z = self.total_height - (coordinate.z + self.tip_length) + offset_xyz.z
     # print(x, y, z)
     # print("moving")
-    self.joint_state_publisher.send_resource_action(ops[0].resource.name, x, y, z, "",channels=use_channels)
+    self.joint_state_publisher.move_joints(ops[0].resource.name, x, y, z, "",channels=use_channels)
 
 
   async def dispense(
@@ -264,9 +282,8 @@ class UniLiquidHandlerRvizBackend(LiquidHandlerBackend):
     x = coordinate.x + offset_xyz.x
     y = coordinate.y + offset_xyz.y
     z = self.total_height - (coordinate.z + self.tip_length) + offset_xyz.z
-    # print(x, y, z)
-    # print("moving")
-    self.joint_state_publisher.send_resource_action(ops[0].resource.name, x, y, z, "",channels=use_channels)
+
+    self.joint_state_publisher.move_joints(ops[0].resource.name, x, y, z, "",channels=use_channels)
 
   async def pick_up_tips96(self, pickup: PickupTipRack, **backend_kwargs):
     print(f"Picking up tips from {pickup.resource.name}.")