import numpy as np import os import rospy from math import acos, degrees,tan,atan,sqrt,sin,cos,radians from scipy.spatial.transform import Rotation as R #求向量的模 def calculate_distance(point1, point2): return np.linalg.norm(np.array(point1) - np.array(point2)) #计算焊缝向量与xy平面夹角 用于判断是平缝还是竖缝 def calculate_angle_with_xy_plane(point1,point2): # 计算方向向量 dir_vector = np.array([point2[0] - point1[0], point2[1] - point1[1], point2[2] - point1[2]]) # 计算方向向量在 xy 平面上的投影 proj_vector = np.array([point2[0] - point1[0], point2[1] - point1[1], 0]) # 计算夹角 angle = acos(np.dot(dir_vector, proj_vector) / (np.linalg.norm(dir_vector) * np.linalg.norm(proj_vector))) angle_deg = degrees(angle) return angle_deg # 从文件中读取焊缝起点和终点，并分为平缝和竖缝 def read_hanfeng_from_point_txt(file_path): data_ping, data_shu = [],[] with open(file_path, 'r') as file: # 逐行读取文件内容 lines = file.readlines() flag_sequence = 0 for line in lines: # print(line) flag_sequence += 1 # 去除行尾的换行符并按'/'分割每一行 points_str = line.strip().split('/') point1_str = points_str[0].split(',') point2_str = points_str[1].split(',') # 转换字符串为浮点数列表，构造三维点 point1 = [float(coord) for coord in point1_str] point2 = [float(coord) for coord in point2_str] angle = calculate_angle_with_xy_plane(point1,point2) if abs(angle) < rospy.get_param("ping_sta"):#用于判断平缝的角度 data_ping.append([point1, point2, flag_sequence]) else: data_shu.append([*sorted([point1, point2], key=lambda p: p[2]), flag_sequence]) data = data_ping + data_shu return data_ping, data_shu def sort_welds_by_distance_ping(data_ping, reference_point): """根据与参考点的距离对焊缝进行排序""" sorted_data_ping = []#一定放for外 for i in range(len(data_ping)): # 计算起点到参考点的距离并排序[[起点与参考点距离，终点与参考点距离，起点坐标，终点坐标，焊缝序号（与points文件中顺序对应）],[],[]....] data_with_distances = [[calculate_distance(start, reference_point),calculate_distance(end, reference_point), start, end,flag_sequence] for start, end,flag_sequence in data_ping] for i in range(len(data_with_distances)): if data_with_distances[i][0] > data_with_distances[i][1]:#若起点比终点离参考点更远 则互换起始点 且将第一个元素变为最近的点（后面用lambda针对第一个元素大小排序） data_with_distances[i][0] = data_with_distances[i][1] tem = data_with_distances[i][2] data_with_distances[i][2] = data_with_distances[i][3] data_with_distances[i][3] = tem data_with_distances.sort(key=lambda x: x[0])#将水平焊缝（起点是离机器人近的那一端）按照起点离参考点的距离排序 # 初始化结果和当前终点 sorted_data_ping.append(data_with_distances.pop(0)[-3:]) # 取出data_with_distances第一个元素的 起点坐标 终点坐标 焊缝序号 #reference_point = sorted_data_ping[-1][1] #参考点变更为 sorted_data_ping中的最后一个点 即寻找离当前焊缝终点最近的 data_ping = [[start, end,flag_sequence] for distances1,distances2, start,end,flag_sequence in data_with_distances]#从弹出已经排好的点的数组重新更新被处理列表 #将起始点位置交换回来，保证平缝焊接方向一致 # for i in range(len(sorted_data_ping)): # sorted_data_ping[i][0], sorted_data_ping[i][1] = sorted_data_ping[i][1], sorted_data_ping[i][0] return sorted_data_ping #焊接方向最终是由参考点近端向远端焊接 顺序为先找离参考点最近的 然后找其次的 # def sort_welds_by_distance_shu(data_shu, reference_point): # """根据与参考点的距离对焊缝进行排序""" # sorted_data_shu = [] # for i in range(len(data_shu)): # # 计算起点到参考点的距离并排序 # data_with_distances = [[calculate_distance(start, reference_point), start, end ,flag_sequence] for start, end ,flag_sequence in data_shu] # data_with_distances.sort(key=lambda x: x[0]) # # 初始化结果和当前终点 # sorted_data_shu.append(data_with_distances.pop(0)[-3:]) # reference_point = sorted_data_shu[-1][1] # data_shu = [[start, end ,flag_sequence] for distances, start,end ,flag_sequence in data_with_distances] # return sorted_data_shu def compute_pose_R(fangxiang, hanfeng): """wg根据计算出的焊接时z轴负方向的向量(fangxiang)和由起点指向终点的向量(hanfeng)计算旋转矩阵""" #得到的是fangxiang向量相对于基坐标系原点的姿态变换矩阵 多了个绕x转180的变换 x, y, z = fangxiang # 计算旋转角度 rotx = -np.arctan2(y, z)#投影到yz平面的的夹角 roty = np.arctan2(x, np.sqrt(y**2 + z**2))#向量和yz面的夹角 # 构造旋转矩阵 rot x y 先绕x转不影响向量和yz面的夹角？ A = np.array([[1, 0, 0], [0, np.cos(rotx), -np.sin(rotx)], [0, np.sin(rotx), np.cos(rotx)]]) B = np.array([[np.cos(roty), 0, np.sin(roty)], [0, 1, 0], [-np.sin(roty), 0, np.cos(roty)]]) bb = np.matmul(hanfeng, np.matmul(A, B))#先转到XOZ 再转到X 相当于通过将方向向量转到z轴上的方式 让焊缝向量转到XOY平面 通过方向向量和焊缝向量的正交关系求出向量在Z轴上的转角 # 区分平缝竖缝 rotz = np.arctan2(-bb[1], -bb[0]) # 构造旋转矩阵 rot z x C = np.array([[np.cos(rotz), -np.sin(rotz), 0], [np.sin(rotz), np.cos(rotz), 0], [0, 0, 1]]) D = np.array([[1, 0, 0], [0, np.cos(np.pi), -np.sin(np.pi)], [0, np.sin(np.pi), np.cos(np.pi)]]) ANG_Y=-25 E = np.array([[np.cos(radians(ANG_Y)), 0, np.sin(radians(ANG_Y))], [0, 1, 0], [-np.sin(radians(ANG_Y)), 0, np.cos(radians(ANG_Y))]]) #A*(B*(C*D)) x*y*z*x(180) #R_mat = np.matmul(A, np.matmul(B, np.matmul(C, D))) #R_mat = np.matmul(A, np.matmul(B, C)) R_mat = np.matmul(A, np.matmul(B, np.matmul(C, E))) return R_mat def run(): result=[] # 定义参考点，排序 #reference_point = (1172.85077147, -1.50259925, 668.30298144) reference_point = (0.0,9000.0, 0.0) file_path = rospy.get_param("folder_path") file_path_points = os.path.join(file_path, 'points.txt') file_path_result = os.path.join(file_path, 'result.txt') data_ping, data_shu= read_hanfeng_from_point_txt(file_path_points) # 对焊缝排序 sorted_data_ping = sort_welds_by_distance_ping(data_ping, reference_point) # if len(data_ping) > 0: # a = tuple(sorted_data_ping[-1][1]) # else: # a = reference_point # sorted_welds_shu = sort_welds_by_distance_shu(data_shu, a) sorted_welds = sorted_data_ping# + sorted_welds_shu #[[起点，终点，序号],[],[]。。。。] #传递焊接顺序(points读入时的编号，从零开始) flag_sequence = [flag_sequence for start,end ,flag_sequence in sorted_welds] rospy.set_param('welding_sequence',flag_sequence) sorted_welds_withnoflag = [[start,end]for start,end ,flag_sequence in sorted_welds] # with open(file_path_points_guihua, "w") as file: # for start, end in sorted_welds_withnoflag: # # 将每一对起点和终点转换为字符串，并用逗号连接，最后以换行符分隔不同的焊缝对 # line = "{},{},{}".format(start[0], start[1], start[2]) + "/" + "{},{},{}".format(end[0], end[1], end[2]) + "\n" # file.write(line) # rospy.loginfo("Welding sequence calculation completed") # 计算位姿 ######################################################################################################## if len(sorted_welds_withnoflag)!=2: rospy.loginfo("焊缝数量错误！！！！") #print(len(sorted_welds_withnoflag)) # exit(0) return len(sorted_welds_withnoflag) else: point1=sorted_welds_withnoflag[0] point2=sorted_welds_withnoflag[1] # middle1=[(point1[0][0]+point1[1][0])/2,(point1[0][1]+point1[1][1])/2,(point1[0][2]+point1[1][2])/2] # middle2=[(point2[0][0]+point2[1][0])/2,(point2[0][1]+point2[1][1])/2,(point2[0][2]+point2[1][2])/2] middle1=[(point1[0][0]+point1[1][0])/2,(point1[0][1]+point1[1][1])/2,0] middle2=[(point2[0][0]+point2[1][0])/2,(point2[0][1]+point2[1][1])/2,0]#不计算z的偏移 就是相对于水平的旋转 hanfeng1=[point1[1][0]-point1[0][0],point1[1][1]-point1[0][1],point1[1][2]-point1[0][2]] hanfeng2=[point2[1][0]-point2[0][0],point2[1][1]-point2[0][1],point2[1][2]-point2[0][2]] #sorted_welds #[[起点，终点，序号],[],[]。。。。] faxiang=[middle2[0]-middle1[0],middle2[1]-middle1[1],middle2[2]-middle1[2]] #shuzhi=np.cross(faxiang,hanfeng1)#向量叉乘 shuzhi=[0,0,1] faxiang/= np.linalg.norm(faxiang) # 单位化 shuzhi /= np.linalg.norm(shuzhi) # 单位化 #ang_md=calculate_angle_with_xy_plane(middle1,middle2) # if(point1[0][0]<0 and point2[0][0]<0): # fangxiang=faxiang+shuzhi*5.6 #arctan(4)=76度 # else: # fangxiang=faxiang-shuzhi*5.6 #arctan(4)=76度 ang_deg=35 fangxiang=-faxiang-shuzhi*tan(radians(90-ang_deg))#arctan(4)=76度 mat=compute_pose_R(fangxiang,hanfeng1) q = R.from_matrix(mat).as_quat()#应该是RPY转四元数 result.append(("s", point1[0],point1[1], q, q)) # if(point1[0][0]<0 and point2[0][0]<0): # fangxiang=-faxiang+shuzhi*5.6 #arctan(4)=76度 # else: # fangxiang=-faxiang-shuzhi*5.6 #arctan(4)=76度 fangxiang=faxiang-shuzhi*tan(radians(90-ang_deg)) #arctan(4)=76度 mat=compute_pose_R(fangxiang,hanfeng1) q = R.from_matrix(mat).as_quat()#应该是RPY转四元数 result.append(("s", point2[0],point2[1], q, q)) # 打开文件准备写入 with open(file_path_result, "w") as file: for group in result: # 对于每个数据组，将每个部分用逗号连接，然后在各部分间插入斜杠 line = "/".join([",".join(map(str, sublist)) for sublist in group]) + "\n" file.write(line) rospy.loginfo("顺序与姿态规划完毕 0v0") return len(sorted_welds_withnoflag) if __name__ == '__main__': run()