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

修复rviz位置问题,

Browse Source 
修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法
This commit is contained in:
zhangshixiang
2025-04-30 22:18:29 +08:00
parent 44c191fe90
commit 5212d2d8eb
8 changed files with 5323 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,3 +1,4 @@
import copy
import rclpy
import json
import time
@@ -12,7 +13,7 @@ from tf2_ros import TransformBroadcaster, Buffer, TransformListener
class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
def __init__(self,device_id:str, joint_config:dict,resource_tracker):
def __init__(self,device_id:str, joint_config:dict, lh_id:str,resource_tracker, rate=50):
super().__init__(
driver_instance=self,
device_id=device_id,
@@ -22,26 +23,61 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
print_publish=False,
resource_tracker=resource_tracker,
)
# self.station_name = station_name
# self.dev_name = dev_name
# 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.j_msg.name = [device_id+x for x in joint_config.keys()]
self.rate = 50
self.j_msg.position = [0.0 for i in range(len(joint_config.keys()))]
self.joint_config = joint_config
self.j_msg.position = [0.0 for i in range(len(joint_config['joint_names']))]
self.j_msg.name = [f"{self.lh_id}_{x}" for x in joint_config['joint_names']]
# self.joint_config = joint_config_dict
# self.j_msg.position = [0.0 for i in range(len(joint_config_dict['joint_names']))]
# self.j_msg.name = [f"{self.lh_id}_{x}" for x in joint_config_dict['joint_names']]
self.rate = rate
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.j_pub = self.create_publisher(JointState,'/joint_states',10)
self.create_timer(0.02,self.sim_joint_pub_callback)
self.create_timer(0.02,self.lh_joint_pub_callback)
self.j_action = ActionServer(
self,
SendCmd,
"joint",
self.sim_joint_action_callback,
self.lh_joint_action_callback,
result_timeout=5000
)
def sim_joint_action_callback(self,goal_handle: ServerGoalHandle):
def lh_joint_action_callback(self,goal_handle: ServerGoalHandle):
"""Move a single joint
Args:
@@ -60,7 +96,7 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
try:
cmd_dict = json.loads(cmd_str)
self.move_joint(**cmd_dict)
self.move_joints(**cmd_dict)
result.success = True
goal_handle.succeed()
@@ -70,44 +106,103 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
result.success = False
return result
def move_joint(self, joint_name, speed, position):
def inverse_kinematics(self, x, y, z,
x_joint:dict,
y_joint:dict,
z_joint:dict ):
"""
将x、y、z坐标转换为对应关节的位置
# joint_index = self.j_msg.name.index(self.station_name+self.dev_name+joint_name)
joint_index = self.j_msg.name.index(joint_name)
distance = position - self.j_msg.position[joint_index]
if distance == 0:
return
flag = abs(distance)/distance
while abs(distance)>speed/self.rate:
Args:
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
self.j_msg.position[joint_index] += flag*speed/self.rate
self.sim_joint_pub_callback()
time.sleep(0.02)
distance = position - self.j_msg.position[joint_index]
self.j_msg.position[joint_index] = position
self.sim_joint_pub_callback()
Returns:
dict: 关节名称和对应位置的字典
"""
joint_positions = copy.deepcopy(self.j_msg.position)
# 处理x轴关节
for joint_name, config in x_joint.items():
index = self.j_msg.name.index(f"{self.lh_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}")
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}")
joint_positions[index] = z * config["factor"] + config["offset"]
return joint_positions
def sim_joint_pub_callback(self):
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]
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]
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]
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)
loop_flag = 0
while loop_flag < len(self.joint_config['joint_names']):
loop_flag = 0
for i in range(len(self.joint_config['joint_names'])):
distance = joint_positions_target[i] - self.j_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
else :
self.j_msg.position[i] = joint_positions_target[i]
loop_flag += 1
# 发布关节状态
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)
def main():
joint_json:dict = json.load(open("device_data.json", encoding='utf-8'))
joint_action_list = {}
rclpy.init()
executor = MultiThreadedExecutor()
for station_name,dev_dict in joint_json.items():
for dev_name,joint_names in dev_dict.items():
joint_action_list[station_name+'_'+dev_name] = SimJointPublisher(station_name,dev_name,joint_names)
executor.add_node(joint_action_list[station_name+'_'+dev_name])
executor.spin()
pass
if __name__ == '__main__':
main()

79
unilabos/devices/ros_dev/resource_mesh_manager.py
View File

@@ -22,7 +22,7 @@ from rclpy.action.server import ServerGoalHandle
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode,DeviceNodeResourceTracker
class ResourceMeshManager(BaseROS2DeviceNode):
def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager"):
def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager", rate=50):
"""初始化资源网格管理器节点
Args:
@@ -47,7 +47,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.tf_broadcaster = TransformBroadcaster(self)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.rate = rate
self.zero_count = 0
self.old_resource_pose = {}
self.__planning_scene = None
@@ -101,8 +102,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
)
self.resource_mesh_setup()
self.create_timer(0.02, self.publish_resource_tf)
self.create_timer(1, self.check_resource_pose_changes)
self.create_timer(1/self.rate, self.publish_resource_tf)
self.create_timer(1/self.rate, self.check_resource_pose_changes)
@@ -116,6 +117,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
# if tf_ready:
if self._get_planning_scene_service.service_is_ready() and self._apply_planning_scene_service.service_is_ready() and tf_ready:
self.move_group_ready = True
self.publish_resource_tf()
self.add_resource_collision_meshes()
# time.sleep(1)
@@ -246,7 +248,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
transform = self.tf_buffer.lookup_transform(
"world",
resource_id,
rclpy.time.Time(),
rclpy.time.Time(seconds=0),
rclpy.duration.Duration(seconds=5)
)
@@ -280,13 +282,24 @@ class ResourceMeshManager(BaseROS2DeviceNode):
except Exception as e:
self.get_logger().warning(f"获取资源 {resource_id} 的世界坐标变换失败: {e}")
changed_poses_msg = String()
changed_poses_msg.data = json.dumps(changed_poses)
self.resource_pose_publisher.publish(changed_poses_msg)
if changed_poses != {}:
self.zero_count = 0
changed_poses_msg = String()
changed_poses_msg.data = json.dumps(changed_poses)
self.resource_pose_publisher.publish(changed_poses_msg)
else:
if self.zero_count > self.rate:
self.zero_count = 0
changed_poses_msg = String()
changed_poses_msg.data = json.dumps(changed_poses)
self.resource_pose_publisher.publish(changed_poses_msg)
self.zero_count += 1
def _is_pose_equal(self, pose1, pose2, tolerance=1e-5):
def _is_pose_equal(self, pose1, pose2, tolerance=1e-7):
"""
比较两个位姿是否相等（考虑浮点数精度）
@@ -328,7 +341,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
transform = self.tf_buffer.lookup_transform(
target_parent,
resource_id,
rclpy.time.Time()
rclpy.time.Time(seconds=0)
)
# 提取转换中的位置和旋转信息
@@ -350,7 +363,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"position": position,
"rotation": rotation
}
self.attach_collision_object(id=resource_id,link_name=target_parent)
self.publish_resource_tf()
@@ -362,48 +375,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
goal_handle.succeed()
return SendCmd.Result(success=True)
def publish_resource_tf(self):
"""
发布资源之间的TF关系
遍历self.resource_tf_dict中的每个元素，根据key，parent，以及position和rotation，
发布key和parent之间的tf关系
"""
transforms = []
# 遍历资源TF字典
resource_tf_dict = copy.deepcopy(self.resource_tf_dict)
for resource_id, tf_info in resource_tf_dict.items():
parent = tf_info['parent']
position = tf_info['position']
rotation = tf_info['rotation']
# 创建静态变换消息
transform = TransformStamped()
transform.header.stamp = self.get_clock().now().to_msg()
transform.header.frame_id = parent
transform.child_frame_id = resource_id
# 设置位置
transform.transform.translation.x = float(position['x'])
transform.transform.translation.y = float(position['y'])
transform.transform.translation.z = float(position['z'])
# 设置旋转
transform.transform.rotation.x = rotation['x']
transform.transform.rotation.y = rotation['y']
transform.transform.rotation.z = rotation['z']
transform.transform.rotation.w = rotation['w']
transforms.append(transform)
# 一次性发布所有静态变换
if transforms:
self.tf_broadcaster.sendTransform(transforms)
# self.get_logger().info(f'已发布 {len(transforms)} 个资源TF关系')
def add_resource_collision_meshes(self):
"""