2024-9-24

catkin_ws/src/lstrobot_planning/scripts/hanqiangpose.py

@@ -5,15 +5,95 @@ from scipy.spatial import KDTree
 import tf.transformations as tft
 import os
 import rospy
-from math import acos,tan,atan,degrees,sqrt,sin,cos
+from math import acos,tan,atan,degrees,sqrt
 
-def rpy2quaternion(roll, pitch, yaw):#zyx
-    x=sin(roll/2)*cos(pitch/2)*cos(yaw/2)-cos(roll/2)*sin(pitch/2)*sin(yaw/2)
-    y=cos(roll/2)*sin(pitch/2)*cos(yaw/2)+sin(roll/2)*cos(pitch/2)*sin(yaw/2)
-    z=cos(roll/2)*cos(pitch/2)*sin(yaw/2)-sin(roll/2)*sin(pitch/2)*cos(yaw/2)
-    w=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(pitch/2)*sin(yaw/2)
-    return [x, y, z, w]
+def corner_angle_discrimination(point,data,fangxiang):
+    # 构建KDTree索引
+    tree = KDTree(data)
+    indices = np.array(tree.query_ball_point(point, r=30, p=2)) # 返回这些点的索引数组
+    distances = np.array([np.linalg.norm(point - data[index]) for index in indices])
+    distance_filter = distances >= 10
+    filtered_indices = indices[distance_filter]
+    data_circular = data[filtered_indices]
 
+    # 可视化
+    # pt_cloud_circular = o3d.geometry.PointCloud()
+    # pt_cloud_circular.points = o3d.utility.Vector3dVector(data_circular)
+    # o3d.visualization.draw_geometries([pt_cloud_circular])
+
+    # 获取与方向向量相似的点云
+    data_similar = []
+    for i in range(len(data_circular)):
+        v = np.array(data_circular[i]) - point
+        angle = acos(np.dot(v, fangxiang) / (np.linalg.norm(v) * np.linalg.norm(fangxiang)))
+        angle_deg = degrees(angle)
+        if angle_deg <= 15:
+            data_similar.append(data_circular[i])
+    
+    if len(data_similar) >= 1:
+        corner = True
+    else:
+        corner = False
+    return corner
+def set_yaw_angle(R_mat,yaw):
+    pz1 = np.array([[1, 0, 0],
+                    [0, np.cos(yaw), -np.sin(yaw)],
+                    [0, np.sin(yaw),  np.cos(yaw)]])
+    Z_PI = np.array([[np.cos(np.pi), -np.sin(np.pi), 0],
+                    [np.sin(np.pi), np.cos(np.pi), 0],
+                    [0, 0, 1]])
+    R1_mat = np.matmul(R_mat,pz1)
+    
+    sign = check_x_axis_direction(R1_mat)
+    if sign == 1:
+        pass
+    elif sign == -1:
+        R1_mat = np.matmul(R1_mat, Z_PI)
+
+    q = R.from_matrix(R1_mat).as_quat()
+    q = q.tolist()  # 将NumPy数组转换为列表
+    return q
+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 check_x_axis_direction(rotation_matrix):
+    # 提取旋转后的 x 轴方向向量
+    x_prime = rotation_matrix[:, 0]
+    # 检查 x 轴的 z 分量
+    if x_prime[2] > 0:
+        return 1
+    elif x_prime[2] < 0:
+        return -1
+    else:
+        return 0
+def load_point_cloud_from_txt(file_path):
+    """导入点云数据"""
+    points = []
+    with open(file_path, 'r') as file:
+        for line in file:
+            # 假设每一行是“x y z”格式，没有多余空格
+            coordinates = line.strip().split()
+            if len(coordinates) == 3:  # 检查是否为有效的三维点
+                point = [float(coord) for coord in coordinates]
+                points.append(point)
+
+    return np.array(points)
+def load_point_cloud_from_binary_txt(file_path):
+    with open(file_path, 'rb') as f:
+        binary_data = f.read()
+
+        # 将二进制数据转换为 NumPy 数组
+        point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
+    return point_cloud
 
 def get_hanfeng(file_path):
     """从文件中读取焊缝起点、终点和中间点，并计算两点构成的向量"""
@@ -44,37 +124,322 @@ def get_hanfeng(file_path):
 
     return start_points, end_points, midpoints, hanfeng
 
-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)
+def compute_Rc2w_tc2w(file_path_Tw2c):
+    """相机和基底坐标系的转换关系"""
+    # 读取文件
+    with open(file_path_Tw2c, 'r') as file:
+        lines = file.readlines()
+
+    lines = [line.strip().replace('[', '').replace(']', '').split() for line in lines]
+    data = [[float(num) for num in line] for line in lines]
+    data = np.array(data)
+    # 提取Rw2c和tw2c
+    Rw2c = data[:3,:3]
+    tw2c = data[:3,-1]
+
+    Rc2w = Rw2c.T
+    tc2w = -np.matmul(Rc2w, tw2c)
+    return Rc2w,tc2w
 
-    return angle_deg  
+def compute_pose_R(fangxiang, hanfeng,start_point, end_point):
+    """根据计算出的焊接时z轴负方向的向量和由起(fangxiang)点指向终点的向量(hanfeng)计算旋转矩阵"""
+    x, y, z = fangxiang
+
+    # 计算旋转角度
+    rotx = -np.arctan2(y, z)
+    roty = np.arctan2(x, np.sqrt(y**2 + z**2))
+
+    # 构造旋转矩阵
+    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))
+    # 区分平缝竖缝
+    angle = calculate_angle_with_xy_plane(start_point,end_point)
+    ispingfeng = abs(angle) < 30
+    if ispingfeng: 
+        rotz = np.arctan2(bb[0], -bb[1])
+    else:
+        rotz = np.arctan2(bb[1], bb[0])
+
+    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)]])
+
+    R_mat = np.matmul(A, np.matmul(B, np.matmul(C, D)))
+
+    return R_mat, ispingfeng
+
+def get_quaternion(data, midpoint, hanfeng, start_point, end_point,result_ping,result_shu,result_wai,file_path_Tw2c): 
+
+    # 构建KDTree索引
+    tree = KDTree(data)
+    # 查询半径范围内的点,找出"data"中所有距离"midpoint"点在r以内的点，这里使用的是欧几里得距离（由p=2表示）
+    indices = np.array(tree.query_ball_point(midpoint, r=30, p=2)) # 返回这些点的索引数组
+    distances = np.array([np.linalg.norm(midpoint - data[index]) for index in indices])
+    # 对numpy数组进行布尔索引
+    # distance_filter = np.logical_and(distances >= 10, distances <= 50)
+    distance_filter = distances >= 10
+    # 使用布尔索引过滤出符合条件的索引
+    filtered_indices = indices[distance_filter]
+    # 提取符合条件的点
+    data_circular = data[filtered_indices]
+
+    # 可视化
+    # pt_cloud_circular = o3d.geometry.PointCloud()
+    # pt_cloud_circular.points = o3d.utility.Vector3dVector(data_circular)
+    # o3d.visualization.draw_geometries([pt_cloud_circular])
+
+    # 获取竖直方向上的点云
+    data_shu = []
+    for i in range(len(data_circular)):
+        P = np.array(data_circular[i])
+        v = P - midpoint
+        anglecos = np.dot(v, hanfeng) / (np.linalg.norm(v) * np.linalg.norm(hanfeng))
+        
+        if abs(anglecos) <= 0.015:
+            # print(anglecos)
+            data_shu.append(P)
+    data_shu = np.array(data_shu)
+    # # 可视化
+    # pt_cloud_shu = o3d.geometry.PointCloud()
+    # pt_cloud_shu.points = o3d.utility.Vector3dVector(np.array(data_shu))
+    # o3d.visualization.draw_geometries([pt_cloud_shu])
+
+ ##################################################
+ ##################################################
+    random_point = data_shu[np.random.randint(0, len(data_shu))]
+    # 与midpoint一起定义直线向量
+    nohanfeng = random_point - midpoint  #data_shu中任意一点midpoint构成的向量，计算所有点和该向量的距离判断是否是外缝(只拍到一面)
+
+    distances_to_line = []
+
+    for i in range(data_shu.shape[0]):  # 遍历每一个点 P
+        P = data_shu[i]
+        A = midpoint
+        u = nohanfeng
+        v = P - A
+        distance = np.linalg.norm(np.cross(u, v)) / np.linalg.norm(u)
+        distances_to_line.append(distance)
+
+    distances_to_line = np.array(distances_to_line)
+
+    all_distances_less_than_10 = np.all(distances_to_line < 5)#都小于10为true，也就是data_shu只有一列，侧着拍得外缝
+
+    if all_distances_less_than_10:
+
+        # 随机选取data_shu中的第一个点
+        random_index = np.random.randint(0, data_shu.shape[0])
+        vector1 = data_shu[random_index] - midpoint
+        vector1 /= np.linalg.norm(vector1)  # 单位化vector1
+        fa_xian = np.cross(vector1, hanfeng)
+        fa_xian /= np.linalg.norm(fa_xian) 
+
+        point = []
+        point1 = midpoint + 50 * fa_xian
+        point2 = midpoint - 50 * fa_xian
+
+        Rc2w,tc2w = compute_Rc2w_tc2w(file_path_Tw2c)
+
+        point1_c = np.matmul(Rc2w, point1) + tc2w
+        point2_c = np.matmul(Rc2w, point2) + tc2w
+        a = np.linalg.norm(point1_c)
+        b = np.linalg.norm(point2_c)      
+
+        if a < b:
+            point = point1
+        else:
+            point = point2
+
+        faxian = point - midpoint
+        vector2 =  faxian / np.linalg.norm(faxian)  # 单位化vector2
+
+        fangxiang = vector1  + vector2*1.2
+
+        R_mat, ispingfeng = compute_pose_R(fangxiang, hanfeng, start_point, end_point)
+        if ispingfeng:
+            ##偏转##
+            pz1 = np.array([[1, 0, 0],
+                        [0, np.cos(yaw), -np.sin(yaw)],
+                        [0, np.sin(yaw),  np.cos(yaw)]])
+            pz2 = np.array([[1, 0, 0],
+                        [0, np.cos(-yaw), -np.sin(-yaw)],
+                        [0, np.sin(-yaw),  np.cos(-yaw)]])
+            Z_PI = np.array([[np.cos(np.pi), -np.sin(np.pi), 0],
+                    [np.sin(np.pi), np.cos(np.pi), 0],
+                    [0, 0, 1]])
+            R1_mat = np.matmul(R_mat,pz1)
+            R2_mat = np.matmul(R_mat,pz2)
+
+            sign = check_x_axis_direction(R1_mat)
+            if sign == 1:
+                pass
+            elif sign == -1:
+                R1_mat = np.matmul(R1_mat, Z_PI)
+                R2_mat = np.matmul(R2_mat, Z_PI)
+
+            q1 = R.from_matrix(R1_mat).as_quat()
+            q2 = R.from_matrix(R2_mat).as_quat()
+            q1 = q1.tolist()  # 将NumPy数组转换为列表
+            q2 = q2.tolist()
+            # print("一个点云面的平外缝")
+            # print(q1,q2)
+            result_ping.append(("pw", start_point, end_point, q1, q2))
+            
+            return result_ping,result_shu,result_wai
+        else:
+            ##偏转##
+            E = np.array([[np.cos(-pitch_of_Verticalweld), 0, np.sin(-pitch_of_Verticalweld)],
+                        [0, 1, 0],
+                        [-np.sin(-pitch_of_Verticalweld), 0, np.cos(-pitch_of_Verticalweld)]])
+
+            R_mat = np.matmul(R_mat,E)
+            
+            q1 = R.from_matrix(R_mat).as_quat()
+            q2 = R.from_matrix(R_mat).as_quat()
+            q1 = q1.tolist()  # 将NumPy数组转换为列表
+            q2 = q2.tolist()
+            # print("即一个点云面的竖外缝")
+            # print(q1,q2)
+            result_wai.append(("sw", start_point, end_point, q1, q2))
+            return result_ping,result_shu,result_wai
+
+    else:
+        # 选择位于平面两侧的点
+        # 随机选取data_shu中的第一个点
+        random_index = np.random.randint(0, data_shu.shape[0])
+        vector1 = data_shu[random_index] - midpoint
+        vector1 /= np.linalg.norm(vector1)  # 单位化vector1
+
+        # 随机选取下一个点，直到找到与vector1夹角大于10度的向量
+        found = False
+        while not found:
+            random_index = np.random.randint(0, data_shu.shape[0])
+            random_point = data_shu[random_index] - midpoint
+            vector2 = random_point / np.linalg.norm(random_point)  # 单位化vector2
+            
+            # 计算两个向量之间的夹角（弧度）
+            dot_product = np.dot(vector1, vector2)
+            angle_rad = np.arccos(dot_product)
+            
+            # 判断角度是否大于10度（转换为弧度）
+            if angle_rad > np.deg2rad(20) and angle_rad < np.deg2rad(180):
+                found = True
+                # 如果找到了，则保存vector2
+            else:
+                continue
+
+        # 计算并保存最终的fangxiang向量, vector1与vector2均为单位向量
+        vector1_sim_to_z = np.dot(vector1, [0,0,1]) / (np.linalg.norm(vector1))
+        vector2_sim_to_z = np.dot(vector2, [0,0,1]) / (np.linalg.norm(vector2))
+        if vector1_sim_to_z > vector2_sim_to_z:
+            vector_shu = vector1
+            vector_ping = vector2
+        else:
+            vector_shu = vector2
+            vector_ping = vector1
+        fangxiang = vector_ping + vector_shu*tan(pitch_of_Horizontalweld)
+        # fangxiang = vector1 + vector2
+        fangxiang /= np.linalg.norm(fangxiang)  # 单位化fangxiang
+
+        #########################################根据vector1和vector2判断内缝外缝#########################################
+        point1 = midpoint + 50 * fangxiang 
+        point2 = midpoint - 50 * fangxiang 
+
+        Rc2w,tc2w = compute_Rc2w_tc2w(file_path_Tw2c)
+        
+        point1_c = np.matmul(Rc2w, point1) + tc2w
+        point2_c = np.matmul(Rc2w, point2) + tc2w
+        a = np.linalg.norm(point1_c)
+        b = np.linalg.norm(point2_c)    
+
+        if a < b: # 内缝
+            # print("两个点云面的内缝")
+            fangxiang = fangxiang
+        else: # 外缝
+            # print("两个点云面的外缝")
+            fangxiang = -fangxiang
+
+        R_mat, ispingfeng = compute_pose_R(fangxiang, hanfeng, start_point, end_point)
+        if ispingfeng:
+            ##偏转##
+            corner_of_start_point = corner_angle_discrimination(start_point,data,fangxiang)
+            corner_of_end_point = corner_angle_discrimination(end_point,data,fangxiang)
+            pi4_pz = atan(1 / (sqrt(2) * tan(np.pi/4)))#CL公式
+            yaw_pz =atan(1 / (sqrt(2) * tan(yaw)))
+            if corner_of_start_point:
+                q1 = set_yaw_angle(R_mat,pi4_pz)
+                if not corner_of_end_point:
+                    len_hanfeng = np.linalg.norm(hanfeng)
+                    angle = pi4_pz-(pi4_pz)*len_hanfeng/yaw_rate
+                    if angle < -yaw_pz:
+                        angle =-yaw_pz
+                    q2 = set_yaw_angle(R_mat,angle)
+            if corner_of_end_point:
+                q2 = set_yaw_angle(R_mat,-pi4_pz)
+                if not corner_of_start_point:
+                    len_hanfeng = np.linalg.norm(hanfeng)
+                    angle = -pi4_pz+(pi4_pz)*len_hanfeng/yaw_rate
+                    if angle > yaw_pz:
+                        angle = yaw_pz
+                    q1 = set_yaw_angle(R_mat,angle)
+            if not corner_of_start_point and not corner_of_end_point:     
+                q1 = set_yaw_angle(R_mat,yaw_pz)
+                len_hanfeng = np.linalg.norm(hanfeng)
+                angle = yaw_pz-(pi4_pz)*len_hanfeng/yaw_rate
+                if angle < -yaw_pz:
+                    angle =-yaw_pz
+                q2 = set_yaw_angle(R_mat,angle)
+            
+            result_ping.append(("p", start_point, end_point, q1, q2))
+            
+            return result_ping,result_shu,result_wai
+        else:
+            ##偏转##
+            E = np.array([[np.cos(-pitch_of_Verticalweld), 0, np.sin(-pitch_of_Verticalweld)],
+                        [0, 1, 0],
+                        [-np.sin(-pitch_of_Verticalweld), 0, np.cos(-pitch_of_Verticalweld)]])
+
+            R_mat = np.matmul(R_mat,E)
+            
+            q1 = R.from_matrix(R_mat).as_quat()
+            q2 = R.from_matrix(R_mat).as_quat()
+            q1 = q1.tolist()  # 将NumPy数组转换为列表
+            q2 = q2.tolist()
+            # print(q1,q2)
+            result_wai.append(("s", start_point, end_point, q1, q2))
+            return result_ping,result_shu,result_wai        
 
 def run():
+    global yaw,yaw_rate,pitch_of_Horizontalweld,pitch_of_Verticalweld
+    # yaw = np.pi/8
+    yaw = np.pi/2-rospy.get_param("yaw")
+    pitch_of_Verticalweld = rospy.get_param("pitch_of_Verticalweld")
+    pitch_of_Horizontalweld = rospy.get_param("pitch_of_Horizontalweld")
+    yaw_rate = rospy.get_param("yaw_rate")
     file_path = rospy.get_param("folder_path")
+    # file_path = ('/home/chen/catkin_ws/data/0603')
+    file_path_pointcloud = os.path.join(file_path, 'pointcloud.txt')
     file_path_points = os.path.join(file_path, 'points_guihua.txt')
     file_path_result = os.path.join(file_path, 'result.txt')
+    file_path_Tw2c = os.path.join(file_path, 'Tw2c.txt')
 
-    result=[]
+    # 获取数据
+    data = load_point_cloud_from_binary_txt(file_path_pointcloud)
     start_points, end_points, midpoints, hanfeng = get_hanfeng(file_path_points)
-    # 计算位姿
-    for i in range(len(hanfeng)): 
-        if(calculate_angle_with_xy_plane(start_points[i], end_points[i])<45):#平缝
-            q1 = rpy2quaternion(0,np.pi,0)
-            q2 = rpy2quaternion(0,np.pi,0)
-            result.append(("s", start_points[i], end_points[i], q1, q2))
-        else:
-            q1 = rpy2quaternion(0,np.pi,0)
-            q2 = rpy2quaternion(0,np.pi,0)
-            result.append(("s", start_points[i], end_points[i], q1, q2))
 
+    # 计算位姿
+    result_ping,result_shu,result_wai = [],[],[]
+    for i in range(len(midpoints)):
+        result_ping,result_shu,result_wai = get_quaternion(data, midpoints[i], hanfeng[i], start_points[i], end_points[i],result_ping,result_shu,result_wai, file_path_Tw2c)
+        # print(q1," ",q2)
+    result = result_ping + result_shu + result_wai
 
     # 打开文件准备写入
     with open(file_path_result, "w") as file:

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -76,73 +76,6 @@ class MoveIt_Control:
         self.arm.go()
         #rospy.sleep(1)
 
-    
-    def plan_cartesian_path_LLL(self,point,points,trajectory,trajectory_with_type,a=1,v=1):
-        fraction = 0.0  # 路径规划覆盖率
-        maxtries = 100  # 最大尝试规划次数
-        attempts = 0  # 已经尝试规划次数
-
-        self.arm.set_max_acceleration_scaling_factor(a)
-        self.arm.set_max_velocity_scaling_factor(v)
-
-        waypoints=[]
-        if trajectory:
-        #起点位置设置为规划组最后一个点
-            pose = self.arm.get_current_pose(self.end_effector_link).pose
-            wpose = deepcopy(pose)
-            wpose.position.x=points[-1][0]
-            wpose.position.y=points[-1][1]
-            wpose.position.z=points[-1][2]
-            wpose.orientation.x=points[-1][3]
-            wpose.orientation.y=points[-1][4]
-            wpose.orientation.z=points[-1][5]
-            wpose.orientation.w=points[-1][6]
-            waypoints.append(deepcopy(wpose))
-
-            wpose.position.x=point[0]
-            wpose.position.y=point[1]
-            wpose.position.z=point[2]
-            wpose.orientation.x=point[3]
-            wpose.orientation.y=point[4]
-            wpose.orientation.z=point[5]
-            wpose.orientation.w=point[6]
-            waypoints.append(deepcopy(wpose))
-        else:
-            rospy.loginfo("error empty trajectory")
-            return 
-        # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
-        while fraction < 1.0 and attempts < maxtries:
-            (plan, fraction) = self.arm.compute_cartesian_path(
-                waypoints,  # waypoint poses，路点列表
-                0.001,  # eef_step，终端步进值
-                0.0,  # jump_threshold，跳跃阈值
-                True)  # avoid_collisions，避障规划
-            attempts += 1
-
-        if fraction == 1.0 :
-            rospy.loginfo("Path computed successfully.")
-            #traj = plan
-            trajj = plan #取出规划的信息
-            trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
-            trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
-            trajj_with_type.points[-1].type = "end"
-            points.append(point)
-            trajectory.append(trajj)
-            trajectory_with_type.append(trajj_with_type)
-        else:
-            rospy.loginfo(plan[3].val)
-            rospy.loginfo(
-                "fraction=" + str(fraction) + " success after " + str(maxtries))   
-            
-            # rospy.loginfo("本次焊缝规划失败")
-            # points.pop()#将本条焊缝的起点从规划中删除
-            # trajectory.pop()
-            # trajectory_with_type.pop()
-            # self.hf_fail.append(welding_sequence[self.hf_num])#将焊缝添加到失败列表
-        #self.hf_num=self.hf_num+1#焊缝计数
-        return points,trajectory,trajectory_with_type
-    
-
     def get_now_pose(self):
         # 获取机械臂当前位姿
         current_pose = self.arm.get_current_pose(self.end_effector_link).pose
@@ -241,31 +174,31 @@ class MoveIt_Control:
                     trajectory_with_type.append(trajj_with_type)
                     points.append(point)
                     trajectory.append(trajj)  
-                    break  
-            else:
-                rospy.loginfo("check failed! 移动轨迹无效")
-                rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
-                self.arm.clear_path_constraints()
-                rrr=rrr+0.2#每次增加20厘米半径
-                rospy.loginfo("R值：{}".format(rrr))
-                if trajectory:
-                    path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
+                    break
                 else:
-                    path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
-                self.arm.set_path_constraints(path_constraints)#设定约束
-                if(i>=(b-5)):
-                    rospy.loginfo("所有移动规划尝试失败 取消路径约束")
+                    rospy.loginfo("check failed! 移动轨迹无效")
+                    rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
                     self.arm.clear_path_constraints()
-                if(i==(b-1)):
-                    rospy.loginfo("所有移动规划尝试失败  焊缝起点不可达")
-                    er=1
-                    break
+                    rrr=rrr+0.2#每次增加20厘米半径
+                    rospy.loginfo("R值：{}".format(rrr))
+                    if trajectory:
+                        path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
+                    else:
+                        path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
+                    self.arm.set_path_constraints(path_constraints)#设定约束
+                    if(i>=(b-5)):
+                        rospy.loginfo("所有移动规划尝试失败 取消路径约束")
+                        self.arm.clear_path_constraints()
+                    if(i==(b-1)):
+                        rospy.loginfo("所有移动规划尝试失败  焊缝起点不可达")
+                        er=1
+                        break  
         #清除约束
         self.arm.clear_path_constraints()
         
         return points,trajectory,trajectory_with_type,er
     
-  
+    #TODO move_pl
     def move_pl(self,point,points,trajectory,trajectory_with_type,a=1,v=1):#焊缝的规划  只规划当前的点到上一个点
         
         self.arm.set_max_acceleration_scaling_factor(a)
@@ -307,65 +240,7 @@ class MoveIt_Control:
         self.hf_num=self.hf_num+1#焊缝计数
         return points,trajectory,trajectory_with_type
 
-    def move_pl_check(self,point,points,trajectory,trajectory_with_type,a=1,v=1):
-        self.arm.set_max_acceleration_scaling_factor(a)
-        self.arm.set_max_velocity_scaling_factor(v)
-        
-        #重新规划起点次数
-        a=10
-        for j in range(a):
-            if trajectory:
-                state=self.arm.get_current_state()
-                state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
-                self.arm.set_start_state(state)
-                
-            self.arm.set_pose_target(point, self.end_effector_link)
-            
-            path_constraints = self.create_path_constraints(points[-1],point)
-            self.arm.set_path_constraints(path_constraints)
-            
-            #当前起点规划次数
-            b=10
-            for i in range(b):
-                traj = self.arm.plan()
-                error=traj[3]
-                if  error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
-                    trajj = traj[1] #取出规划的信息
-                    rospy.loginfo("正在检查焊缝轨迹有效性...")
-                    error_c,limit_margin=check.check_joint_positions(trajj)
-                    if not error_c:       
-                        rospy.loginfo("本次焊缝规划OK")
-                        rospy.loginfo("*******************")
-                        break
-                    if not limit_margin or i==b-1:
-                        prepoint=points.pop()
-                        trajectory.pop()
-                        trajectory_with_type.pop()
-                        #清除约束
-                        self.arm.clear_path_constraints()
-                        points,trajectory,trajectory_with_type,er1= self.move_p_flexible(prepoint,points,trajectory,trajectory_with_type)
-                        break
-                rospy.loginfo("The {}-{}th replanning".format(j,i+1))
-            if not error_c:
-                break
-        if error_c:
-            rospy.loginfo("本次焊缝规划失败")
-            rospy.loginfo("*******************")
-            self.hf_fail.append(welding_sequence[self.hf_num])
-            
-        self.arm.clear_path_constraints()
-        self.hf_num=self.hf_num+1#焊缝计数
-        
-        trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
-        trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
-        trajj_with_type.points[-1].type = "end"      
-
-        points.append(point)
-        trajectory.append(trajj)
-        trajectory_with_type.append(trajj_with_type)
-        
-        return points,trajectory,trajectory_with_type
-
+    
     #TODO move_pl_path_constraints
     def create_path_constraints(self,start_point,end_point):
         #计算起点指向终点的向量
@@ -458,7 +333,7 @@ class MoveIt_Control:
                 continue
 
             point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0],q2[1],q2[2],q2[3]]
-            points,trajectory,trajectory_with_type = self.plan_cartesian_path_LLL(point22,points,trajectory,trajectory_with_type,v=speed_v)
+            points,trajectory,trajectory_with_type = self.move_pl(point22,points,trajectory,trajectory_with_type,v=speed_v)
 
             rospy.loginfo("第%d条焊缝规划完毕",i+1)
             rospy.loginfo("*******************")
@@ -488,7 +363,7 @@ def process_welding_data(filename):
 
     return all_data
 
-
+#TODO mark_the_traj
 def mark_the_traj(traj,TYPE,SEQUENCE):
     traj_with_type = JointTrajectory_ex()
     traj_with_type.header = traj.joint_trajectory.header
@@ -505,7 +380,7 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
     ]
     return traj_with_type
 
-
+#TODO merge_trajectories
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -534,7 +409,7 @@ def merge_trajectories(trajectories):
     
     return merged_trajectory
 
-
+#TODO merge_trajectories_with_type
 def merge_trajectories_with_type(trajectory_with_type):
     if not trajectory_with_type:
         return None
@@ -642,39 +517,8 @@ def get_user_input():
     except Exception as e:
         rospy.logerr(f"发生错误：{e}")
         
-def ROS2_msgs(msgs, m_sr):
-    for i in range(len(msgs.points)):
-        py_msgs.point.xyz_list.append(msgs.points[i].positions)
-    
-    f_p = os.path.join(folder_path, 'outtt.txt')
-    with open(f_p,'w') as file:
-        for point in py_msgs.point.xyz_list:
-            file.write(' '.join(str(value) for value in point)+ "\n")
-            
-# def write_ee(msgs, m_sr):
-#     for i in range(len(msgs.points)):
-#         py_msgs.point.xyz_list.append(msgs.points[i].positions)
-        
-#     f_p1 = os.path.join(folder_path, 'ee_pos.txt')
-#     with open(f_p1,'w') as file:
-#         for point in py_msgs.point.xyz_list:
-#             #更新机械臂状态
-#             state=m_sr.arm.get_current_state()
-#             #state.joint_state.position=point
-            
-#             #获取末端执行器的位姿
-#             fk_result=m_sr.arm.get_current_pose(moveit_server.end_effector_link)
-            
-#             #获取位置信息
-#             position=fk_result.pose.position
-#             orientation=fk_result.pose.orientation
-            
-#             #格式化数据
-#             pose_data = f"Position: x={position.x}, y={position.y}, z={position.z}\n"
-#             pose_data += f"Orientation (Quaternion): x={orientation.x}, y={orientation.y}, z={orientation.z}, w={orientation.w}\n"
-#             pose_data += "-" * 50 + "\n"
+
             
-#             file.write(pose_data)
 
 if __name__ =="__main__":
     # from redis_publisher import Publisher
@@ -690,8 +534,7 @@ if __name__ =="__main__":
     # rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
 
     trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
-    #ROS2_msgs(trajectory_with_type_merge,moveit_server)
-    #write_ee(trajectory_with_type_merge, moveit_server)
+   
     
     moveit_server.arm.execute(trajectory_merge)
     

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -94,15 +94,15 @@ if __name__ == "__main__":
             # 清除场景
             clear_octomap()
             #点云计算并发布
-            # process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
-            # process.start()
+            process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
+            process.start()
             #计算焊接顺序和焊接姿态
             hjsx.run()
             hanqiangpose.run()
             command.load_visual()
             # 等待 /move_group/monitored_planning_scene 话题发布消息
             #rospy.loginfo("等待场景地图加载完毕...")
-            #wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
+            wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
             #rospy.loginfo(f"场景地图已加载完毕")
             rospy.set_param("sign_pointcloud",1)
             #rospy.loginfo("终止点云发布，关闭发布窗口")

catkin_ws/需求.txt

@@ -1 +0,0 @@
-记得修改焊接顺序规划