初始化

catkin_ws/src/lstrobot_moveit_config_0605/launch/moveit.rviz

@@ -5,6 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
+        - /MotionPlanning1/Scene Geometry1
         - /MotionPlanning1/Planned Path1
         - /RobotModel1/Links1
         - /Trajectory1/Links1
@@ -157,7 +158,7 @@ Visualization Manager:
         Scene Alpha: 1
         Scene Color: 50; 230; 50
         Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: true
+        Show Scene Geometry: false
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
       Scene Robot:
@@ -350,16 +351,7 @@ Visualization Manager:
       Marker Topic: /rviz_visual_tools
       Name: MarkerArray
       Namespaces:
-        Path: true
-        Text: true
-        point0  0.978994  -0.262691  0.279523: true
-        point0  0.980265  0.126875  0.278115: true
-        point0  0.983265  0.126875  0.278115: true
-        point0  0.983265  0.126875  0.473115: true
-        point1  0.978378  0.122593  0.278064: true
-        point1  0.983265  -0.262875  0.473115: true
-        point1  0.983265  0.126875  0.468115: true
-        point1  1.125382  0.127069  0.280059: true
+        {}
       Queue Size: 100
       Value: true
     - Class: rviz/TF
@@ -395,7 +387,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 3.102827310562134
+      Distance: 0.9001314043998718
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -403,17 +395,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 1.3195676803588867
-        Y: 0.5246121883392334
-        Z: 0.8483012914657593
+        X: 0.877734899520874
+        Y: -0.03542045131325722
+        Z: 0.13408130407333374
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4452030062675476
+      Pitch: 0.42520225048065186
       Target Frame: base_link
-      Yaw: 4.942476749420166
+      Yaw: 5.237476825714111
     Saved: ~
 Window Geometry:
   Displays:

catkin_ws/src/lstrobot_planning/scripts/check.py

@@ -0,0 +1,145 @@
+import rospy, sys
+import numpy as np
+from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply
+
+#####检查关节限位#####
+def check_joint_positions(joint_trajectory):
+    is_limited = False      #初始化为满足限制条件
+    Limit_margin = True     #限制边界，初始化为True，用来检查是否超出限制条件
+    
+    for i in range(len(joint_trajectory.joint_trajectory.points) - 1):  #遍历除了最后一个关节轨迹点其他所有轨迹点
+        if  not is_limited:
+            joint_positions1 = joint_trajectory.joint_trajectory.points[i].positions    #选择两个相邻的关节轨迹点
+            joint_positions2 = joint_trajectory.joint_trajectory.points[i + 1].positions 
+
+            for j in range(len(joint_positions1)):    #遍历但前轨迹点的每个关节点，这里是joint_positions1
+                position_diff = abs(joint_positions1[j] - joint_positions2[j])  #计算相邻关节轨迹点的关节点绝对差值，归一化到[-pi,pi]
+                
+                if position_diff > 6:   #限位阈值，6rad=343.8度
+                    rospy.loginfo("发生大角度翻转: point{}-joint{}:{}".format(i,j+1,joint_positions1))
+                    
+                    #关节限位值
+                    joint_limt =[[],[],[],[-1.6,1.6],[],[-3.838,3.838]] 
+                    
+                    #设定限位点为当前关节点
+                    Limit_point = joint_positions1[j]  
+                     
+                    #计算限位点(当前点)与起点位置的偏移量的绝对值
+                    distance_to_Limit_point = abs(joint_trajectory.joint_trajectory.points[0].positions[j]- Limit_point)
+                    Joint_range = abs(joint_limt[j][0] - joint_limt[j][1])  #计算关节限位范围
+                    margin = distance_to_Limit_point / Joint_range          #计算限位余量：偏移量的绝对值/关节限位范围
+                    if margin > 0.3:        
+                        Limit_margin = True  
+                    else:
+                        Limit_margin = False
+                    rospy.loginfo("margin = {}".format(margin))
+                    is_limited = True           
+                    break
+                
+                if position_diff > 3:    #限位阈值，3rad=171.9度
+                    rospy.loginfo("point{}-joint{}:{}".format(i,j+1,joint_positions1))
+      
+                    Limit_margin = True
+                    is_limited = True
+                    break
+        if  is_limited:
+            break
+    if not is_limited:
+        rospy.loginfo("Check OK! 轨迹有效")         
+    return is_limited,Limit_margin
+
+#姿态调整
+def angle_adjustment(pose,axis,angle):      
+    if len(pose) == 6:
+        q_initial = quaternion_from_euler(pose[3], pose[4], pose[5])    #初始姿态四元数，将欧拉角转换为四元数
+    else:
+        q_initial = (pose[3], pose[4], pose[5],pose[6])     #len>6，默认为四元数
+    if axis == 'z':
+        q_rotation = quaternion_from_euler(0, 0, angle)     #绕z轴旋转，转换得到关于z轴的旋转四元数
+    if axis == 'y':
+        q_rotation = quaternion_from_euler(0, angle, 0)
+    if axis == 'x':
+        q_rotation = quaternion_from_euler(angle, 0, 0)    
+    q_new = quaternion_multiply(q_initial, q_rotation)      #q_new为调整后的姿态四元数，值初始姿态四元数乘以旋转四元数
+    pose[3:] = q_new    #将q_new放入pose列表中的3号索引以后的位置
+    return pose
+
+
+
+# def traj_validity_check(trajectory):
+#     if type (trajectory)is moveit_msgs.msg._RobotTrajectory.RobotTrajectory:
+#         point_num=len(trajectory.joint_trajectory.point)
+#         trajectory.joint_trajectory.point.positions
+        
+#         # for i
+#         trajectory.joint_trajectory.point.velocities
+
+#         trajectory.joint_trajectory.point.accelerations
+        
+#         return 0
+#     # else:
+#     #         raise MoveItCommanderException(
+#     #             "Expected value in the range from 0 to 1 for scaling factor"
+#     #         )
+#     def scale_trajectory_speed(traj, scale):
+#        # Create a new trajectory object
+#        new_traj = RobotTrajectory()
+       
+#        # Initialize the new trajectory to be the same as the input trajectory
+#        new_traj.joint_trajectory = traj.joint_trajectory
+       
+#        # Get the number of joints involved
+#        n_joints = len(traj.joint_trajectory.joint_names)
+       
+#        # Get the number of points on the trajectory
+#        n_points = len(traj.joint_trajectory.points)
+        
+#        # Store the trajectory points
+#        points = list(traj.joint_trajectory.points)
+       
+#        # Cycle through all points and joints and scale the time from start,
+#        # as well as joint speed and acceleration
+#        for i in range(n_points):
+#            point = JointTrajectoryPoint()
+           
+#            # The joint positions are not scaled so pull them out first
+#            point.positions = list(traj.joint_trajectory.points[i].positions)
+
+#            # Next, scale the time_from_start for this point
+#             # point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
+           
+#            # Get the joint velocities for this point
+#            point.velocities = list(traj.joint_trajectory.points[i].velocities)
+           
+#            # Get the joint accelerations for this point
+#            point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
+           
+#            # Scale the velocity and acceleration for each joint at this point
+#            for j in range(n_joints):
+#             #    if point.positions[j]
+#                if has_velocity_limits:
+#                 if point.velocities[j] > joint_max_velocity[j]:
+#                     vel_exception_point
+#                     rospy.loginfo("velocities Test OK") 
+#                 else:
+#                     raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor")     
+#                if has_acceleration_limits:
+#                 point.accelerations[j] = point.accelerations[j] * scale * scale
+        
+#            # Store the scaled trajectory point
+#            points[i] = point
+
+#        rospy.loginfo("velocities Check OK") 
+
+#        # Assign the modified points to the new trajectory
+#        new_traj.joint_trajectory.points = points
+
+#        # Return the new trajecotry
+#        return new_traj
+
+#     else:
+#         print("traj type is not std")
+    
+
+
+

catkin_ws/src/lstrobot_planning/scripts/decorator_time.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python3
+import rospy
+import time
+
+#定义一个装饰器，用来计算函数运行时间
+#使用方法：在需要计算运行时间的函数前面加上@decorator_time
+
+def decorator_time(func):
+    def wrapper(*args, **kwargs):
+        start_time = time.time()
+        result = func(*args, **kwargs)
+        end_time = time.time()
+        elapsed_time = end_time - start_time
+        rospy.loginfo(f"Time taken by {func.__name__}: {elapsed_time:.4f} seconds")
+        return result
+    return wrapper

catkin_ws/src/lstrobot_planning/scripts/hanqiangpose.py

@@ -5,95 +5,15 @@ from scipy.spatial import KDTree
 import tf.transformations as tft
 import os
 import rospy
-from math import acos,tan,atan,degrees,sqrt
+from math import acos,tan,atan,degrees,sqrt,sin,cos
 
-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]
+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]
 
-    # 可视化
-    # 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):
     """从文件中读取焊缝起点、终点和中间点，并计算两点构成的向量"""
@@ -124,322 +44,37 @@ def get_hanfeng(file_path):
 
     return start_points, end_points, midpoints, hanfeng
 
-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
-
-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)]])
+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)
 
-            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        
+    return angle_deg  
 
 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')
 
-    # 获取数据
-    data = load_point_cloud_from_binary_txt(file_path_pointcloud)
+    result=[]
     start_points, end_points, midpoints, hanfeng = get_hanfeng(file_path_points)
-
     # 计算位姿
-    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
+    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))
+
 
     # 打开文件准备写入
     with open(file_path_result, "w") as file:

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -25,6 +25,8 @@ import traceback
 from lstrobot_planning.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
 import json
 import termios
+from decorator_time import decorator_time
+import check
 
 pi = np.pi
 
@@ -36,6 +38,7 @@ class MoveIt_Control:
         self.hf_fail=[]
         # Init ros config
         moveit_commander.roscpp_initialize(sys.argv)
+        self.robot = moveit_commander.RobotCommander()
         
         self.arm = moveit_commander.MoveGroupCommander('manipulator')
         self.arm.set_goal_joint_tolerance(0.0001)
@@ -73,6 +76,73 @@ 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
@@ -88,7 +158,8 @@ class MoveIt_Control:
         # 打印位姿信息       
         #rospy.lohome_poseinfo("Current Pose: {}".format(pose))
         return pose
-        
+    
+    #TODO move_p_path_constraints    
     def create_path_constraints2(self,start_point,end_point,r):#创建路径约束
         #计算起点指向终点的向量
         vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
@@ -130,6 +201,7 @@ class MoveIt_Control:
         # constraints.orientation_constraints.append(orientation_constraint)
         return constraints
     
+    #TODO move_p_flexible
     def move_p_flexible(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)
@@ -157,17 +229,21 @@ class MoveIt_Control:
             traj = self.arm.plan()#调用plan进行规划
             error=traj[3]
             if  error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
-                rospy.loginfo("本次移动OK")
                 trajj = traj[1] #取出规划的信息
-                trajj_with_type = mark_the_traj(trajj,"during-p",welding_sequence)
-                trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
-                trajj_with_type.points[-1].type = "end"      
-                trajectory_with_type.append(trajj_with_type)
-                points.append(point)
-                trajectory.append(trajj)  
-                break
+                rospy.loginfo("正在检查移动轨迹有效性...")
+                error_c, limit_margin=check.check_joint_positions(trajj)
+                if not error_c:       
+                    rospy.loginfo("本次移动OK")
+                    rospy.loginfo("*******************")
+                    trajj_with_type = mark_the_traj(trajj,"during-p",welding_sequence)
+                    trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
+                    trajj_with_type.points[-1].type = "end"      
+                    trajectory_with_type.append(trajj_with_type)
+                    points.append(point)
+                    trajectory.append(trajj)  
+                    break  
             else:
-                rospy.loginfo(error)
+                rospy.loginfo("check failed! 移动轨迹无效")
                 rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
                 self.arm.clear_path_constraints()
                 rrr=rrr+0.2#每次增加20厘米半径
@@ -189,7 +265,7 @@ class MoveIt_Control:
         
         return points,trajectory,trajectory_with_type,er
     
-    
+  
     def move_pl(self,point,points,trajectory,trajectory_with_type,a=1,v=1):#焊缝的规划  只规划当前的点到上一个点
         
         self.arm.set_max_acceleration_scaling_factor(a)
@@ -197,21 +273,22 @@ class MoveIt_Control:
 
         #设定约束
         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)#设定约束
-        
+
         traj = self.arm.plan()#调用plan进行规划
         error=traj[3]
         if  error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
             rospy.loginfo("本次焊缝规划 OK")
+            rospy.loginfo("*******************")
             trajj = traj[1] #取出规划的信息
             trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
             trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
@@ -224,17 +301,77 @@ class MoveIt_Control:
             points.pop()#将本条焊缝的起点从规划中删除
             trajectory.pop()
             trajectory_with_type.pop()
-            self.hf_fail.append(welding_sequence[self.hf_num])
+            self.hf_fail.append(welding_sequence[self.hf_num])#将焊缝添加到失败列表
         #清除约束
         self.arm.clear_path_constraints()
         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):
         #计算起点指向终点的向量
         vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
         height = np.linalg.norm(vector)+0.002
-        radius = 0.001
+        radius = 0.01
 
         constraints = Constraints()
         
@@ -271,7 +408,9 @@ class MoveIt_Control:
         constraints.position_constraints.append(position_constraint)
 
         return constraints
-       
+     
+    #TODO go_home
+    @decorator_time  
     def go_home(self,a=1,v=1):
         rospy.loginfo("go_home start")
         self.arm.set_max_acceleration_scaling_factor(a)
@@ -282,6 +421,7 @@ class MoveIt_Control:
         rospy.loginfo("go_home end")
         # rospy.sleep(1)
     
+    #TODO go_home_justplan
     def go_home_justplan(self,trajectory,trajectory_with_type,a=1,v=1):
         self.arm.set_max_acceleration_scaling_factor(a)
         self.arm.set_max_velocity_scaling_factor(v)
@@ -296,6 +436,8 @@ class MoveIt_Control:
         trajectory_with_type.append(traj_with_type)
         return trajectory,trajectory_with_type
 
+    #TODO path_planning
+    @decorator_time
     def path_planning(self,folder_path,gohome=True):
         file_path_result = os.path.join(folder_path, 'result.txt')
         all_data = process_welding_data(file_path_result)
@@ -309,16 +451,17 @@ class MoveIt_Control:
             q2 = all_data[i][3]
 
             point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0],q1[1],q1[2],q1[3]]
-            points,trajectory,trajectory_with_type,err = self.move_p_flexible(point11,points,trajectory,trajectory_with_type,v=speed_v)
+            points,trajectory,trajectory_with_type,err = self.move_p_flexible(point11,points,trajectory,trajectory_with_type)
             if err==1:#焊缝起点不可达 则跳过此次焊缝和移动的规划 并将焊缝添加到失败列表   
                 self.hf_fail.append(welding_sequence[self.hf_num])
                 self.hf_num=self.hf_num+1#焊缝计数
                 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.move_pl(point22,points,trajectory,trajectory_with_type)
+            points,trajectory,trajectory_with_type = self.plan_cartesian_path_LLL(point22,points,trajectory,trajectory_with_type,v=speed_v)
 
             rospy.loginfo("第%d条焊缝规划完毕",i+1)
+            rospy.loginfo("*******************")
         if gohome:
             points,trajectory,trajectory_with_type,err= self.move_p_flexible(self.home_pose,points,trajectory,trajectory_with_type,v=speed_v)
             trajectory,trajectory_with_type = self.go_home_justplan(trajectory,trajectory_with_type)
@@ -345,6 +488,7 @@ def process_welding_data(filename):
 
     return all_data
 
+
 def mark_the_traj(traj,TYPE,SEQUENCE):
     traj_with_type = JointTrajectory_ex()
     traj_with_type.header = traj.joint_trajectory.header
@@ -361,6 +505,7 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
     ]
     return traj_with_type
 
+
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -389,6 +534,7 @@ def merge_trajectories(trajectories):
     
     return merged_trajectory
 
+
 def merge_trajectories_with_type(trajectory_with_type):
     if not trajectory_with_type:
         return None
@@ -472,10 +618,10 @@ def get_valid_input(factor, default):
             return default
         try:
             value=float(user_input)
-            if 0<=value<=1:
+            if 0<value<=1:
                 return value
             else:
-                rospy.logerr("缩放因子必须在[0, 1]范围内，请重新输入!")
+                rospy.logerr("缩放因子必须在(0, 1]范围内，请重新输入!")
         except ValueError:
             rospy.logerr("输入值无效，请根据提示重新输入！")
 
@@ -495,8 +641,41 @@ def get_user_input():
         return vv
     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
     # publisher = Publisher()
@@ -506,12 +685,14 @@ if __name__ =="__main__":
     moveit_server = MoveIt_Control(is_use_gripper=False)  
     welding_sequence = rospy.get_param('welding_sequence')
 
-
-    speed_v=get_user_input()
-    rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
+    speed_v=1
+    # speed_v=get_user_input()
+    # 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)
     
     rospy.set_param("sign_control",0)

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -46,13 +46,19 @@ if __name__ == "__main__":
 
     rospy.set_param("sign_control",1)
     rospy.set_param("sign_pointcloud",0)
-    rospy.set_param("sign_traj_accepted",0)
+    #rospy.set_param("sign_traj_accepted",0)
 
     rospy.set_param("yaw",np.pi/6)  #内收角（偏航角）
     rospy.set_param("yaw_rate",100) #偏航角过渡速度（角度变化45度/100mm）
     rospy.set_param("pitch_of_Horizontalweld",np.pi/4)  #和底面夹角（俯仰角）
     rospy.set_param("pitch_of_Verticalweld",np.pi/5)
-    rospy.set_param("culling_radius",0) #焊缝剔除半径
+
+    # rospy.set_param("yaw",0)  #内收角（偏航角）
+    # rospy.set_param("yaw_rate",100) #偏航角过渡速度（角度变化45度/100mm）
+    # rospy.set_param("pitch_of_Horizontalweld",0)  #和底面夹角（俯仰角）
+    # rospy.set_param("pitch_of_Verticalweld",0)
+
+    #rospy.set_param("culling_radius",0) #焊缝剔除半径
     rospy.set_param("folder_path","/home/robot/ROS/catkin_ws/data/2")
 
     rospy.loginfo("当前参数值:")# 显示参数当前值
@@ -67,9 +73,11 @@ if __name__ == "__main__":
         sign_control = str(rospy.get_param("sign_control"))
         if sign_control == "0":
             if not waited_once:
-                print("等待点云数据准备完成...")
+                #print("等待点云数据准备完成...")
                 sys.stdin.flush()#清空键盘缓冲区
-                aaa=input("请选择 1-2: ")
+                aaa=input("请选择 1-2 默认为2: ")
+                if aaa=="":
+                    aaa='2'
                 #aaa='2'
 
                 rospy.set_param("cishu",rospy.get_param("cishu")+1)
@@ -82,20 +90,20 @@ if __name__ == "__main__":
         elif sign_control == "1":
             # 重置参数
             #rospy.set_param("sign_control",0)    #在轨迹规划里重置 防止重复调用
-            rospy.set_param("sign_traj_accepted",0)
+            #rospy.set_param("sign_traj_accepted",0)
             # 清除场景
             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)
-            rospy.loginfo(f"场景地图已加载完毕")
+            #rospy.loginfo("等待场景地图加载完毕...")
+            #wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
+            #rospy.loginfo(f"场景地图已加载完毕")
             rospy.set_param("sign_pointcloud",1)
             #rospy.loginfo("终止点云发布，关闭发布窗口")
             
@@ -113,21 +121,23 @@ if __name__ == "__main__":
             rospy.set_param("time",0)
             # 重置参数
             #rospy.set_param("sign_control",0)
-            rospy.set_param("sign_traj_accepted",0)
+            #rospy.set_param("sign_traj_accepted",0)
             hjsx.run()
             hanqiangpose.run()
+            command.load_visual()
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
 
             while rospy.get_param("sign_control"):
                 pass
 
-            tim=time.time()-tim
-            tim_list.append(tim)
-            print("-------------------------------------")
-            print(f'第{rospy.get_param("cishu")}次规划：')
-            print(tim_list)
-            print("")
+            # tim=time.time()-tim
+            # tim_list.append(tim)
+            # print("-------------------------------------")
+            # print(f'第{rospy.get_param("cishu")}次规划：')
+            # print(tim_list)
+            # print("")
+            
             #运行结束，重置参数
             waited_once = False
 

catkin_ws/src/mr12urdf20240605/urdf/mr12urdf20240605.urdf

@@ -387,8 +387,8 @@
     <axis
       xyz="0 0 1" />
     <limit
-      lower="-2.1"
-      upper="2.1"
+      lower="-2.8"
+      upper="2.8"
       effort="32.64"
       velocity="23.655" />
   </joint>

catkin_ws/src/visual/src/visual_tools.cpp

@@ -147,20 +147,60 @@ int main(int argc, char** argv)
 		  //   std::cout << l[i]<< " ";
 	    // }
       // std::cout<<std::endl;
+
     }
 
   }
   file.close(); // 关闭文件
   
+  // double xxx,yyy,zzz,xxx2,yyy2,zzz2,xxx3,yyy3,zzz3;
+  // double qx,qy,qz,qw;
+  // ros::param::get("xxx", xxx);
+  // ros::param::get("yyy", yyy);
+  // ros::param::get("zzz", zzz);
+
+  // ros::param::get("qx", qx);
+  // ros::param::get("qy", qy);
+  // ros::param::get("qz", qz);
+  // ros::param::get("qw", qw);
+
+  // target_pose2.position.x = xxx/1000;
+  // target_pose2.position.y = yyy/1000;
+  // target_pose2.position.z = zzz/1000;
+  // target_pose2.orientation.x= qx;
+  // target_pose2.orientation.y= qy;
+  // target_pose2.orientation.z= qz;
+  // target_pose2.orientation.w = qw;
+  // waypoints[num].push_back(target_pose2);//添加到路径点列表
+  // num++;
+
+  // ros::param::get("xxx2", xxx2);
+  // ros::param::get("yyy2", yyy2);
+  // ros::param::get("zzz2", zzz2);
+  // target_pose2.position.x = xxx2/1000;
+  // target_pose2.position.y = yyy2/1000;
+  // target_pose2.position.z = zzz2/1000;
+  // waypoints[num].push_back(target_pose2);//添加到路径点列表
+  // num++;
+
+  // ros::param::get("xxx3", xxx3);
+  // ros::param::get("yyy3", yyy3);
+  // ros::param::get("zzz3", zzz3);
+  // target_pose2.position.x = xxx3/1000;
+  // target_pose2.position.y = yyy3/1000;
+  // target_pose2.position.z = zzz3/1000;
+  // waypoints[num].push_back(target_pose2);//添加到路径点列表
+  // num++;
+
 
   visual_tools.deleteAllMarkers();//清除所有标记
   visual_tools.publishText(text_pose, "robot", rvt::WHITE, rvt::XLARGE);//文本标记
   for(int k=0;k<num;k++)
   {
-  visual_tools.publishPath(waypoints[k], rvt::PINK , rvt::XXXSMALL);//路径？
-  for (std::size_t i = 0; i < waypoints[k].size(); ++i)
-    visual_tools.publishAxisLabeled(waypoints[k][i], "point" + std::to_string (i)+ "  "+std::to_string(waypoints[k][i].position.x)+ "  "+ std::to_string(waypoints[k][i].position.y)+ "  "+ std::to_string(waypoints[k][i].position.z), rvt::XXXSMALL);//坐标系
-  visual_tools.trigger();
+    visual_tools.publishPath(waypoints[k], rvt::PINK , rvt::XXXSMALL);//路径？
+    for (std::size_t i = 0; i < waypoints[k].size(); ++i)
+      visual_tools.publishAxisLabeled(waypoints[k][i], "point" + std::to_string (k)+ "  "+std::to_string(waypoints[k][i].position.x)+ "  "+ std::to_string(waypoints[k][i].position.y)+ "  "+ std::to_string(waypoints[k][i].position.z), rvt::XXXSMALL);//坐标系
+    visual_tools.trigger();
   }
   // while (ros::ok())
   // {
@@ -171,6 +211,7 @@ int main(int argc, char** argv)
   ros::shutdown();
   return 0;
 }
+
 /*
 BLACK 	
 BROWN 	

catkin_ws/更新记录.txt

@@ -1,49 +0,0 @@
-2024-7-31
-优化了轨迹规划：
-	1、做了关节限位 规划时不会出现大角度差姿态来回切换
-	2、做了轨迹间路径约束 移动时末端执行器不会走冗余路径
-	
-2024-8-5
-	1、
-	修改rvizload  通过py程序调起rviz
-	增加rviz启动等待功能
-	增加close_rviz 通过py接口关闭程序时顺带关闭rviz
-	（后期可以通过include限制在同一个终端  只是输出信息不方便查看 same_terminal_to_run.launch ）
-	2、修改点云加载 不再保存又立即读出
-	3、修改轨迹规划 重写之前做限制的函数 去除check 转而判断规划成功与否
-	4、增加ROS转py成员变量接口  包括：
-	{
-		焊接轨迹点坐标
-		焊接轨迹点隶属类型
-		焊接序号列表
-		焊接轨迹规划失败序号列表
-	}
-	
-	5、增加 不重新加载点云和焊接顺序姿态只重新规划机器人路径和运动学逆解 的接口
-	6、因为是多文件启动 修改程序逻辑防止规划器被2次启动
-	
-2024-8-7
-	
-	取消了焊缝剔除 通过延长焊丝坐标系的方式防碰撞（同时修改urdf和config 的end_effector_link）
-	优点：省去20s点云剔除时间 同时消除碰撞检测隐患
-	问题：有的焊缝要求焊丝必须比平时伸出的要长才能够到的话就会一直规划失败 因为路径点本就不可达 
-	
-	增加了轨迹可视化节点
-	
-	未能解决：
-	做初始化时间优化（本质是go函数第一次启动时间很长  搞不了 是系统封装的函数   而且好像是因为第一次要加载点云到碰撞检测)  实测时间与运行ROS机器性能有关
-	同时go的调用只能放在点云加载之后 否则没有对应的topic会报错 也导致无法进行预加载
-	
-	遇到新的问题 第六轴的翻转问题    
-	在焊接姿态处理中应该可以让焊缝焊接过程不进行翻转从而保证焊接的连续性（可能已经预留了末端执行器关节角度代码？  也大概率没有）  
-	那就得移动过程中来解决旋转的问题（有的焊接顺序就是避免不了翻转  焊缝姿态定了 那移动过程就会不可避免的要翻转 ） 而移动过程的翻转面临的最大问题就是 翻转过程不检测碰撞
-	
-2024-9-1
-	修复从起点规划到第一个失败后的重新设定约束逻辑问题
-	六轴翻转问题依旧没有解决  其问题会带来运动碰撞风险 不止是极限角度  也是之前轴翻转的根源
-2024-9-2
-	通过限制6轴自由度解决六轴翻转问题
-2024-9-5
-	隐患： 焊缝周围没有点云的的话会报错  （这个问题不是问题）
-		  T型焊缝未测试   对向焊缝失败处理未测试
-	

catkin_ws/需求.txt

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