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

修改物料跟随与物料添加逻辑

Browse Source 
修改物料跟随与物料添加逻辑
将joint_publisher类移出lh的backends，但仍需要对lh的backends进行一些改写
This commit is contained in:
zhangshixiang
2025-05-16 18:43:26 +08:00
parent 6a33f9986b
commit 498c997ad7
5 changed files with 168 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
unilabos/devices/ros_dev/lh_joint_config.json
View File

@@ -1,30 +1,36 @@
{
"OTdeck":{
"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":-1,
"offset":0.0
"factor":-0.001,
"offset":0.163
}
},
"x":{
"second_joint":{
"factor":-1,
"offset":0.0
"factor":-0.001,
"offset":0.1775
}
},
"z":{
"third_joint":{
"factor":1,
"factor":0.001,
"offset":0.0
},
"fourth_joint":{
"factor":1,
"factor":0.001,
"offset":0.0
}
}

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

@@ -1,4 +1,7 @@
import asyncio
import copy
from pathlib import Path
import threading
import rclpy
import json
import time
@@ -15,7 +18,7 @@ from rclpy.node import Node
import re
class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
def __init__(self,resource_config:list, resource_tracker, rate=50, device_id:str = "lh_joint_publisher"):
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,
@@ -28,8 +31,8 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
# 初始化参数
self.j_msg = JointState()
joint_config = json.load(open('./lh_joint_config.json', encoding="utf-8"))
self.resource_config = resource_config
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)
@@ -50,7 +53,7 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
self.deck_list = []
self.lh_devices = {}
# 初始化设备ID与config信息
for resource in resource_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]
@@ -96,13 +99,15 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
def find_resource_parent(self, resource_id:str):
# 遍历父辈，找到父辈的父辈，直到找到设备ID
parent_id = self.resource_config[resource_id]['parent']
parent_id = self.resources_config[resource_id]['parent']
try:
if parent_id in self.deck_list:
return f'{self.resource_config[parent_id]['parent']}_{parent_id}'
p_ = self.resources_config[parent_id]['parent']
str_ = f'{p_}_{parent_id}'
return str(str_)
else:
self.find_resource_parent(parent_id)
except:
return self.find_resource_parent(parent_id)
except Exception as e:
return None
@@ -156,10 +161,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
print('='*20)
print(result)
print('='*20)
return result
def inverse_kinematics(self, x, y, z,
parent_id,
@@ -198,36 +206,40 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
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 ,z_index
def move_joints(self, resource_names, speed,x,y,z,option, 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
parent_id = self.find_resource_parent(resource_name_)
if x_joint is None:
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]
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.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]
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.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]
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, 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
@@ -235,10 +247,13 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
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]
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])
time.sleep(1)
elif option == "drop":
self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7])
time.sleep(1)
self.move_to(joint_positions_target_zero, speed, parent_id)
@@ -307,14 +322,59 @@ class JointStatePublisher(Node):
return None
def send_resource_action(self, resource_id_list:list[str], link_name:str):
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 = {}
for resource in resource_id_list:
str_dict[resource] = link_name
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)
self.lh_action_client.send_goal_async(goal_msg)
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.SingleThreadedExecutor()
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():

25
unilabos/resources/graphio.py
View File

@@ -9,7 +9,7 @@ try:
from pylabrobot.resources.resource import Resource as ResourcePLR
except ImportError:
pass
from typing import Union, get_origin, get_args
physical_setup_graph: nx.Graph = None
@@ -297,7 +297,6 @@ def nested_dict_to_list(nested_dict: dict) -> list[dict]: # FIXME 是tree？
return result
def convert_resources_to_type(
resources_list: list[dict], resource_type: type, *, plr_model: bool = False
) -> Union[list[dict], dict, None, "ResourcePLR"]:
@@ -319,9 +318,15 @@ def convert_resources_to_type(
return resource_ulab_to_plr(resources_list, plr_model)
resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
return resource_ulab_to_plr(resources_tree[0], plr_model)
elif isinstance(resource_type, list) and all(issubclass(t, ResourcePLR) for t in resource_type):
resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
return [resource_ulab_to_plr(r, plr_model) for r in resources_tree]
elif isinstance(resource_type, list) :
if all((get_origin(t) is Union) for t in resource_type):
resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
return [resource_ulab_to_plr(r, plr_model) for r in resources_tree]
elif all(issubclass(t, ResourcePLR) for t in resource_type):
resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
return [resource_ulab_to_plr(r, plr_model) for r in resources_tree]
else:
return None
@@ -342,9 +347,13 @@ def convert_resources_from_type(resources_list, resource_type: type) -> Union[li
elif isinstance(resource_type, type) and issubclass(resource_type, ResourcePLR):
resources_tree = [resource_plr_to_ulab(resources_list)]
return tree_to_list(resources_tree)
elif isinstance(resource_type, list) and all(issubclass(t, ResourcePLR) for t in resource_type):
resources_tree = [resource_plr_to_ulab(r) for r in resources_list]
return tree_to_list(resources_tree)
elif isinstance(resource_type, list) :
if all((get_origin(t) is Union) for t in resource_type):
resources_tree = [resource_plr_to_ulab(r) for r in resources_list]
return tree_to_list(resources_tree)
elif all(issubclass(t, ResourcePLR) for t in resource_type):
resources_tree = [resource_plr_to_ulab(r) for r in resources_list]
return tree_to_list(resources_tree)
else:
return None

7
unilabos/ros/main_slave_run.py
View File

@@ -9,6 +9,7 @@ import rclpy
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
from unilabos.devices.ros_dev.liquid_handler_joint_publisher import LiquidHandlerJointPublisher
from unilabos_msgs.msg import Resource # type: ignore
from unilabos_msgs.srv import ResourceAdd, SerialCommand # type: ignore
from rclpy.executors import MultiThreadedExecutor
@@ -72,16 +73,18 @@ def main(
resource_mesh_manager = ResourceMeshManager(
resources_mesh_config,
resources_config,
resource_tracker= DeviceNodeResourceTracker(),
resource_tracker = host_node.resource_tracker,
device_id = 'resource_mesh_manager',
)
joint_republisher = JointRepublisher(
'joint_republisher',
DeviceNodeResourceTracker()
host_node.resource_tracker
)
executor.add_node(resource_mesh_manager)
executor.add_node(joint_republisher)
lh_joint_pub = LiquidHandlerJointPublisher(resources_config=resources_config, resource_tracker=host_node.resource_tracker)
executor.add_node(lh_joint_pub)
thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread")
thread.start()

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

@@ -91,6 +91,9 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.__collision_object_publisher = self.create_publisher(
CollisionObject, "/collision_object", 10
)
self.__planning_scene_publisher = self.create_publisher(
PlanningScene, "/planning_scene", 10
)
self.__attached_collision_object_publisher = self.create_publisher(
AttachedCollisionObject, "/attached_collision_object", 0
)
@@ -145,7 +148,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"""刷新资源配置"""
registry = lab_registry
resource_config = json.loads(resource_config_str)
resource_config = json.loads(resource_config_str.replace("'",'"'))
if resource_config['id'] in self.resource_config_dict:
self.get_logger().info(f'资源 {resource_config["id"]} 已存在')
@@ -377,14 +380,15 @@ 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
planning_scene = PlanningScene()
planning_scene.is_diff = True
planning_scene.robot_state.is_diff = True
for resource_id, target_parent in cmd_dict.items():
# 获取从resource_id到target_parent的转换
@@ -415,9 +419,32 @@ class ResourceMeshManager(BaseROS2DeviceNode):
}
self.attach_collision_object(id=resource_id,link_name=target_parent)
# self.attach_collision_object(id=resource_id,link_name=target_parent)
# time.sleep(0.02)
operation_attach = CollisionObject.ADD
operation_remove = CollisionObject.REMOVE
if target_parent == 'world':
operation_attach = CollisionObject.REMOVE
operation_remove = CollisionObject.ADD
remove_object = CollisionObject(
id=resource_id,
operation=operation_remove
)
planning_scene.world.collision_objects.append(remove_object)
collision_object = AttachedCollisionObject(
object=CollisionObject(
id=resource_id,
operation=operation_attach
)
)
if target_parent != 'world':
collision_object.link_name = target_parent
planning_scene.robot_state.attached_collision_objects.append(collision_object)
# collision_object = AttachedCollisionObject(
# id=resource_id,
# link_name=target_parent,
# object=CollisionObject(
# id=resource_id,
@@ -426,9 +453,12 @@ class ResourceMeshManager(BaseROS2DeviceNode):
# )
# 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)
req = ApplyPlanningScene.Request()
req.scene = planning_scene
self._apply_planning_scene_service.call_async(req)
self.__planning_scene_publisher.publish(planning_scene)
# self.__collision_object_publisher.publish(CollisionObject())
self.publish_resource_tf()
except Exception as e:
@@ -452,6 +482,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.__planning_scene = self._get_planning_scene_service.call(
GetPlanningScene.Request()
).scene
planning_scene = PlanningScene()
planning_scene.is_diff = True
for resource_id, tf_info in resource_tf_dict.items():
if resource_id in self.resource_model:
@@ -479,7 +511,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
quat_xyzw=q,
frame_id=resource_id
)
self.__planning_scene.world.collision_objects.append(collision_object)
planning_scene.world.collision_objects.append(collision_object)
elif f"{tf_info['parent']}_" in self.resource_model:
# 获取资源的父级框架ID
id_ = f"{tf_info['parent']}_"
@@ -508,12 +540,13 @@ class ResourceMeshManager(BaseROS2DeviceNode):
frame_id=resource_id
)
self.__planning_scene.world.collision_objects.append(collision_object)
planning_scene.world.collision_objects.append(collision_object)
req = ApplyPlanningScene.Request()
req.scene = self.__planning_scene
req.scene = planning_scene
self._apply_planning_scene_service.call_async(req)
self.__planning_scene_publisher.publish(planning_scene)
self.publish_resource_tf()
self.get_logger().info('资源碰撞网格添加完成')