2024-9-23_L/catkin_ws/src/lst_robot_r/scripts/hjsx.py

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