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