open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 04:51:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

提取lh的joint发布

Browse Source
This commit is contained in:
zhangshixiang
2025-05-13 21:45:13 +08:00
parent 013c25f3aa
commit 6a33f9986b
5 changed files with 297 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
test/experiments/plr_test_converted.json
View File

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

14
unilabos/device_mesh/view_robot.rviz
View File

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

32
unilabos/devices/ros_dev/lh_joint_config.json Normal file
View File

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

277
unilabos/devices/ros_dev/liquid_handler_joint_publisher.py
View File

@@ -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():

47
unilabos/ros/nodes/presets/resource_mesh_manager.py
View File

@@ -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)
)