lstplanning

lstplanning_dev/src/lstrobot_moveit_config_0605/launch/moveit.rviz

@@ -11,7 +11,7 @@ Panels:
         - /RobotModel1/Links1
         - /Trajectory1/Links1
       Splitter Ratio: 0.6529411673545837
-    Tree Height: 496
+    Tree Height: 486
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -388,7 +388,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.4031606912612915
+      Distance: 1.9429523944854736
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -396,17 +396,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 1.0696083307266235
-        Y: 0.35716989636421204
-        Z: 0.7947128415107727
+        X: 0.6881747245788574
+        Y: 0.13520599901676178
+        Z: 0.9280014634132385
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8452028036117554
+      Pitch: 0.3752032518386841
       Target Frame: base_link
-      Yaw: 1.5693169832229614
+      Yaw: 1.529317021369934
     Saved: ~
 Window Geometry:
   Displays:
@@ -420,7 +420,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000010000000000000156000002c8fc0200000009fb000000100044006900730070006c006100790073010000003d00000281000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000002c4000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000210000001e60000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007200000003b5000000410000001600000016000003a9000002c800000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000010000000000000156000002c4fc0200000009fb000000100044006900730070006c006100790073010000003f0000027a000000cc00fffffffb0000000800480065006c00700000000342000000bb0000006f00fffffffb0000000a00560069006500770073000000010c000000a4000000a900fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000002bf000000440000004400fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000210000001e60000019600fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007200000003b5000000410000001700000017000003a9000002c400000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Trajectory - Trajectory Slider:
     collapsed: false
   Views:

lstplanning_dev/src/lstrobot_planning/.vscode/settings.json

@@ -6,5 +6,6 @@
     "python.analysis.extraPaths": [
         "/home/tong/moveir_ws/devel/lib/python3/dist-packages",
         "/opt/ros/noetic/lib/python3/dist-packages"
-    ]
+    ],
+    "Codegeex.RepoIndex": true
 }

lstplanning_dev/src/lstrobot_planning/README.md

@@ -82,4 +82,15 @@ roslaunch lstrobot_planning set_update_paramter_p.launch
 ## 下周测试主要工作
 - T型板测试
 - 记录代码各部分运行时间
-- 精度问题!!!!!
+- 精度问题!!!!!
+
+## 2024.11.28
+- 精度问题解决，原因为相机手眼标定后未更新变换矩阵
+- 实际运动过程中，末端变换姿态会导致误差增大  原因查找中，只出现在测试电脑中！！！基本确定为moveit版本问题(1.1.16)
+- 增加轨迹平滑函数，通过三次样条插值平滑轨迹
+- 增加检查焊缝数据是否存在的接口，避免data中无数据时程序崩溃问题
+
+## 2024.12.04
+- 修复go_home_justplan因传入的轨迹信息列表为空导致的程序崩溃问题
+- 下载moveit_1.1.14版本deb包以及相关依赖包，并测试降级moveit至1.1.14版本
+- 导出YH1006A-200的urdf模型，并配置moveit_config包，已编译测试通过

lstplanning_dev/src/lstrobot_planning/scripts/dycl_0506.py

@@ -40,44 +40,48 @@ def cull_pointcloud(data,culling_radius):
 
 # 从文件中读取焊缝起点和终点，并计算两点构成的向量
 def read_and_calculate_vectors(file_path):
-    with open(file_path, 'r') as file:
-        # 逐行读取文件内容
-        lines = file.readlines()
-    start_points,end_points,vectors = [],[],[]
-    for line in lines:
-        # 去除行尾的换行符并按'/'分割每一行
-        points_str = line.strip().split('/')
-        
-        # 确保每一行都正确地分为两部分
-        if len(points_str) == 2:
-            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]
-
-            start_points.append(point1)
-            end_points.append(point2)
-            # 计算向量：向量 = 点2 - 点1
-            vector = [p2 - p1 for p1, p2 in zip(point1, point2)]
+    try:
+        with open(file_path, 'r') as file:
+            # 逐行读取文件内容
+            lines = file.readlines()
+        start_points,end_points,vectors = [],[],[]
+        for line in lines:
+            # 去除行尾的换行符并按'/'分割每一行
+            points_str = line.strip().split('/')
             
-            vectors.append(vector)
-    
-    return start_points,end_points,vectors
+            # 确保每一行都正确地分为两部分
+            if len(points_str) == 2:
+                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]
+
+                start_points.append(point1)
+                end_points.append(point2)
+                # 计算向量：向量 = 点2 - 点1
+                vector = [p2 - p1 for p1, p2 in zip(point1, point2)]
+                
+                vectors.append(vector)
+        
+        return start_points,end_points,vectors
+    except FileNotFoundError:
+        print(f"File '{file_path}' not found.")
 
 # 读取文件
 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)
-        # 将数组转换为可写状态
-        point_cloud = np.copy(point_cloud)
-    return point_cloud
-
+    try:
+        with open(file_path, 'rb') as f:
+            binary_data = f.read()
 
+            # 将二进制数据转换为 NumPy 数组
+            point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
+            # 将数组转换为可写状态
+            point_cloud = np.copy(point_cloud)
+        return point_cloud
+    except FileNotFoundError:
+        print(f"File '{file_path}' not found.")
 
 
 pcd = o3d.geometry.PointCloud()
@@ -108,11 +112,12 @@ ptCloud_scaled1 = o3d.geometry.PointCloud()
 ptCloud_scaled2 = o3d.geometry.PointCloud()
 ptCloud_scaled2.points = o3d.utility.Vector3dVector(data_scaled2)
 
-#####################################################################################################################################################################
+###################################################################################
+    
 def build_pointcloud2_msg(points):
     msg = PointCloud2()
     msg.header.stamp = rospy.Time(0)
-    msg.header.frame_id = "base_link"
+    msg.header.frame_id = "base_link"   #参考坐标系未正确选择会导致点云无法加载
 
     if len(points.shape) == 3:
         msg.height = points.shape[1]

lstplanning_dev/src/lstrobot_planning/scripts/hanqiangpose.py

@@ -78,71 +78,85 @@ def check_x_axis_direction(rotation_matrix):
 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)
+    try:
+        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)
+    except FileNotFoundError:
+        print(f"Error: The file '{file_path}' was not found.")
+
 def load_point_cloud_from_binary_txt(file_path):
-    with open(file_path, 'rb') as f:
-        binary_data = f.read()
+    try:
+        with open(file_path, 'rb') as f:
+            binary_data = f.read()
+
+            # 将二进制数据转换为 NumPy 数组，(固定3列x,y,z，行数自动计算)
+            point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
+        return point_cloud
+    except FileNotFoundError:
+        print(f"Error: The file '{file_path}' was not found.")
 
-        # 将二进制数据转换为 NumPy 数组
-        point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
-    return point_cloud
 
 def get_hanfeng(file_path):
     """从文件中读取焊缝起点、终点和中间点，并计算两点构成的向量"""
-    with open(file_path, 'r') as file:
-        # 逐行读取文件内容
-        lines = file.readlines()
-    start_points, end_points, midpoints , hanfeng = [],[],[],[]
-    for line in lines:
-        # 去除行尾的换行符并按'/'分割每一行
-        points_str = line.strip().split('/')
-        
-        # 确保每一行都正确地分为两部分
-        if len(points_str) == 2:
-            point1_str = points_str[0].split(',')
-            point2_str = points_str[1].split(',')
+    try:
+        with open(file_path, 'r') as file:
+            # 逐行读取文件内容
+            lines = file.readlines()
+        start_points, end_points, midpoints , hanfeng = [],[],[],[]
+        for line in lines:
+            # 去除行尾的换行符并按'/'分割每一行
+            points_str = line.strip().split('/')
             
-            # 转换字符串为浮点数列表，构造三维点
-            point1 = [float(coord) for coord in point1_str]
-            point2 = [float(coord) for coord in point2_str]
-
-            midpoint = [(p2 + p1)/2 for p1, p2 in zip(point1, point2)]
-            vector = [p2 - p1 for p1, p2 in zip(point1, point2)]
-            
-            start_points.append(point1)
-            end_points.append(point2)
-            midpoints.append(midpoint)  
-            hanfeng.append(vector)   
-
-    return start_points, end_points, midpoints, hanfeng
+            # 确保每一行都正确地分为两部分
+            if len(points_str) == 2:
+                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]
+
+                midpoint = [(p2 + p1)/2 for p1, p2 in zip(point1, point2)]  #焊缝的中间点
+                vector = [p2 - p1 for p1, p2 in zip(point1, point2)]        #焊缝的方向向量
+                
+                start_points.append(point1)
+                end_points.append(point2)
+                midpoints.append(midpoint)  
+                hanfeng.append(vector)   
+
+        return start_points, end_points, midpoints, hanfeng
+    except FileNotFoundError:
+        print(f"Error: The file '{file_path}' was not found.")
 
 def compute_Rc2w_tc2w(file_path_Tw2c):
     """相机和基底坐标系的转换关系"""
+    try:
     # 读取文件
-    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
+        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]  #前3行的前3列
+        tw2c = data[:3,-1]  #前3行的最后一列
+
+        Rc2w = Rw2c.T   # 旋转矩阵的转置
+        tc2w = -np.matmul(Rc2w, tw2c)   #Rc2w乘tw2c，负号表示平移方向是从相机指向基底
+        return Rc2w,tc2w
+    except FileNotFoundError:
+        print(f"Error: The file '{file_path_Tw2c}' was not found.")
 
 def compute_pose_R(fangxiang, hanfeng,start_point, end_point):
-    """根据计算出的焊接时z轴负方向的向量和由起(fangxiang)点指向终点的向量(hanfeng)计算旋转矩阵"""
+    """根据计算出的焊接时z轴负方向的向量和由起点(fangxiang)指向终点的向量(hanfeng)计算旋转矩阵"""
     x, y, z = fangxiang
 
     # 计算旋转角度
@@ -176,7 +190,7 @@ def compute_pose_R(fangxiang, hanfeng,start_point, end_point):
 
     return R_mat, ispingfeng
 
-def get_quaternion(data, midpoint, hanfeng, start_point, end_point,result_ping,result_shu,result_wai,file_path_Tw2c): 
+def get_quaternion(data, midpoint, hanfeng, start_point, end_point, result_ping, result_shu, result_wai, file_path_Tw2c): 
 
     # 构建KDTree索引
     tree = KDTree(data)
@@ -203,7 +217,7 @@ def get_quaternion(data, midpoint, hanfeng, start_point, end_point,result_ping,r
         v = P - midpoint
         anglecos = np.dot(v, hanfeng) / (np.linalg.norm(v) * np.linalg.norm(hanfeng))
         
-        if abs(anglecos) <= 0.015:
+        if abs(anglecos) <= 0.015:  #余弦值接近0，即角度接近90度
             # print(anglecos)
             data_shu.append(P)
     data_shu = np.array(data_shu)
@@ -215,7 +229,7 @@ def get_quaternion(data, midpoint, hanfeng, start_point, end_point,result_ping,r
  ##################################################
  ##################################################
     random_point = data_shu[np.random.randint(0, len(data_shu))]
-    # 与midpoint一起定义直线向量
+    # data_shu中随机一点与midpoint一起定义直线向量
     nohanfeng = random_point - midpoint  #data_shu中任意一点midpoint构成的向量，计算所有点和该向量的距离判断是否是外缝(只拍到一面)
 
     distances_to_line = []
@@ -225,31 +239,31 @@ def get_quaternion(data, midpoint, hanfeng, start_point, end_point,result_ping,r
         A = midpoint
         u = nohanfeng
         v = P - A
-        distance = np.linalg.norm(np.cross(u, v)) / np.linalg.norm(u)
+        distance = np.linalg.norm(np.cross(u, v)) / np.linalg.norm(u) #计算点P到nohanfeng距离
         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只有一列，侧着拍得外缝
+    all_distances_less_than_10 = np.all(distances_to_line < 5)#都小于5为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) 
+        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
+        point1 = midpoint + 50 * fa_xian    #沿法线方向偏移50个单位
         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)
+        a = np.linalg.norm(point1_c)    #到原点的距离
         b = np.linalg.norm(point2_c)      
 
         if a < b:
@@ -430,7 +444,7 @@ def run():
     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)
     start_points, end_points, midpoints, hanfeng = get_hanfeng(file_path_points)
 
@@ -449,6 +463,7 @@ def run():
             file.write(line)
 
     rospy.loginfo("Welding Angle calculation completed")
+        
 if __name__ == '__main__':     
     
     run()

lstplanning_dev/src/lstrobot_planning/scripts/hjsx.py

@@ -2,6 +2,7 @@ import numpy as np
 import os
 import rospy
 from math import acos, degrees
+
 def calculate_distance(point1, point2):
     return np.linalg.norm(np.array(point1) - np.array(point2))
 
@@ -20,51 +21,57 @@ def calculate_angle_with_xy_plane(point1,point2):
 
 def read_points_from_file(file_path):
     """从文件中读取焊缝数据"""
-    with open(file_path, 'r') as file:
-        lines = file.readlines()
-    data = [line.strip().split('/') for line in lines]
-    points_list = [[tuple(map(float, pair.split(','))) for pair in line] for line in data]
-    return points_list
+    try:
+        with open(file_path, 'r') as file:
+            lines = file.readlines()
+        data = [line.strip().split('/') for line in lines]
+        points_list = [[tuple(map(float, pair.split(','))) for pair in line] for line in data]
+        return points_list
+    except FileNotFoundError:
+        print(f"Error: The file '{file_path}' was not found.")
 ########
 # 从文件中读取焊缝起点和终点，并计算两点构成的向量
 def get_hanfeng(file_path):
 
     data_ping, data_shu = [],[]
 
-    with open(file_path, 'r') as file:
-        # 逐行读取文件内容
-        lines = file.readlines()
-    flag_sequence = 0
-    for line in lines:
-        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) < 30:
-            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
-
-        # midpoint = [(p2 + p1)/2 for p1, p2 in zip(point1, point2)]
-        # vector = [p2 - p1 for p1, p2 in zip(point1, point2)]
-        
-        # start_points.append(point1)
-        # end_points.append(point2)
-        # midpoints.append(midpoint)  
-        # hanfeng.append(vector)   
-    # return start_points, end_points, midpoints, hanfeng
-    return data_ping, data_shu, data
+    try:
+        with open(file_path, 'r') as file:
+            # 逐行读取文件内容
+            lines = file.readlines()
+        flag_sequence = 0
+        for line in lines:
+            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) < 30:
+                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
+
+            # midpoint = [(p2 + p1)/2 for p1, p2 in zip(point1, point2)]
+            # vector = [p2 - p1 for p1, p2 in zip(point1, point2)]
+            
+            # start_points.append(point1)
+            # end_points.append(point2)
+            # midpoints.append(midpoint)  
+            # hanfeng.append(vector)   
+        # return start_points, end_points, midpoints, hanfeng
+        return data_ping, data_shu, data
+    except FileNotFoundError:
+        print(f"Error: The file '{file_path}' was not found.")
 
 
 ################################################################################################################
@@ -102,6 +109,7 @@ def sort_welds_by_distance_shu(data_shu, reference_point):
         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 run():
     
     # 定义参考点
@@ -113,6 +121,7 @@ def run():
     file_path_points = os.path.join(file_path, 'points.txt')
     file_path_result = os.path.join(file_path, 'result.txt')
     file_path_points_guihua = os.path.join(file_path, 'points_guihua.txt')
+    
 
     data_ping, data_shu, data = get_hanfeng(file_path_points)
 

lstplanning_dev/src/lstrobot_planning/scripts/moveitServer2.py

@@ -5,6 +5,7 @@ import moveit_msgs.msg
 import rospy, sys
 import moveit_commander      
 import moveit_msgs
+import geometry_msgs
 from visualization_msgs.msg import Marker, MarkerArray
 
 from moveit_commander import MoveGroupCommander, PlanningSceneInterface, RobotCommander
@@ -203,13 +204,13 @@ class MoveIt_Control:
                     trajj_with_type = mark_the_traj(trajj,"during-p", welding_sequence)     
                     trajectory_with_type.append(trajj_with_type)
                     points.append(point)
-                    trajectory.append(trajj)
+                    trajectory.append(trajj)  
                     #平滑轨迹
                     # smooth_traj = self.smooth_trajectory(trajj)
                     # trajj_with_type = mark_the_traj(smooth_traj,"during-p", welding_sequence)     
                     # trajectory_with_type.append(trajj_with_type)
                     # points.append(point)
-                    # trajectory.append(smooth_traj)    
+                    # trajectory.append(smooth_traj)  
                     break
                 else:
                     rospy.loginfo("check failed! 移动轨迹无效")
@@ -438,9 +439,12 @@ class MoveIt_Control:
     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)
-        state = self.arm.get_current_state()
-        state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
-        self.arm.set_start_state(state)
+        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)
+        else:
+            self.arm.set_start_state_to_current_state()
         self.arm.set_named_target('home')
         traj = self.arm.plan()
         trajj = traj[1]
@@ -621,6 +625,10 @@ def ROS2PY_msgs(msgs, m_sr):
         'weld_order': py_msgs.shun_xv,        # 规划路径顺序
         'failed': py_msgs.fail,               # 规划路径失败的焊缝
     }
+    f_pp = os.path.join(folder_path, 'message.txt')
+    with open(f_pp,'w') as file:
+        json.dump(message,file,indent=4)
+        
     return json.dumps(message)
     # for i in range(len(py_msgs.point.type)):
     #     moveit_server.move_j(py_msgs.point.xyz_list[i])
@@ -680,14 +688,14 @@ 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)
+    # ROS2_msgs(trajectory_with_type_merge,moveit_server)
     #write_ee(trajectory_with_type_merge, moveit_server)
     
     moveit_server.arm.execute(trajectory_merge)
     print(f"本次规划失败焊缝序号:{moveit_server.hf_fail}")
     
     #使用redis发布规划后的轨迹信息
-    #message = ROS2PY_msgs(trajectory_with_type_merge,moveit_server)
+    message = ROS2PY_msgs(trajectory_with_type_merge,moveit_server)
     #publisher = Publisher()
     #publisher.pub_plan_result(message)
     

lstplanning_dev/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -48,34 +48,33 @@ def check_file():
 
 waited_once = False
 tim_list=[]
+num = 0
 
 #publisher = Publisher()
 
 if __name__ == "__main__":
-    command.load_rviz()
-    #command.launch_rviz_background()
+    command.load_rviz()                 #启动rviz并在terminal输出相关信息
+    #command.launch_rviz_background()   #启动rviz不在terminal输出信息
     rospy.init_node("set_update_paramter_p")
 
     # 初始化各种类型参数
-
     rospy.set_param("cishu",0)
     rospy.set_param("time",0)
 
     rospy.set_param("sign_control",0)
     rospy.set_param("sign_pointcloud",0)
     rospy.set_param("sign_traj_accepted",0)
-    #rospy.set_param("/move_group/max_replan_attempts",1)
 
     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",7) #焊缝剔除半径
-    #rospy.set_param("folder_path","/home/tong/catkin_ws/data/20240913-1")     #4条焊缝
-    rospy.set_param("folder_path","/home/tong/moveir_ws/data/1111")      #6条焊缝
-    #rospy.set_param("folder_path","/home/tong/moveir_ws/data/6_Welds")
+    rospy.set_param("culling_radius",0) #焊缝剔除半径
+    rospy.set_param("folder_path","/home/tong/catkin_ws/data/t_xing")       #T型板(1焊缝)   
+    #rospy.set_param("folder_path","/home/tong/moveir_ws/data/6_Welds")     #法兰座(6焊缝)
 
-    rospy.loginfo("当前参数值:")# 显示参数当前值
+    # 显示参数当前值
+    rospy.loginfo("当前参数值:")
     rospy.loginfo("sign_pointcloud = %s", rospy.get_param("sign_pointcloud"))
     rospy.loginfo("yaw = %s",rospy.get_param("yaw")/np.pi*180)
     rospy.loginfo("pitch_of_Horizontalweld = %s",rospy.get_param("pitch_of_Horizontalweld")/np.pi*180)
@@ -83,9 +82,10 @@ if __name__ == "__main__":
     rospy.loginfo("等待rviz启动")
     wait_for_topic("/execute_trajectory/status",actionlib_msgs.msg.GoalStatusArray)
     
+    
     while not rospy.is_shutdown():
         sign_control = str(rospy.get_param("sign_control"))
-        if sign_control == "0":
+        if sign_control == "0": 
             if not waited_once:
                 print("等待点云数据准备完成...")
                 # termios.tcflush(sys.stdin, termios.TCIFLUSH) #清空键盘缓冲区
@@ -116,7 +116,7 @@ if __name__ == "__main__":
                 #计算焊接顺序和焊接姿态
                 hjsx.run()
                 hanqiangpose.run()
-                # command.load_visual()
+                command.load_visual()
                 # 等待 /move_group/monitored_planning_scene 话题发布消息
                 rospy.loginfo("等待场景地图加载完毕...")
                 wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
@@ -156,8 +156,9 @@ if __name__ == "__main__":
 
                 tim=time.time()-tim
                 tim_list.append(tim)
+                num += 1
                 print("-------------------------------------")
-                print(f'第{rospy.get_param("cishu")}次规划：')
+                print(f'第{num}次重规划：')
                 print(tim_list)
                 print("")
                 #运行结束，重置参数

lstplanning_dev/src/mr12urdf20240605/urdf/mr12urdf20240605.urdf

@@ -439,4 +439,4 @@
   <link name="link_end_now">
   </link>
 
-</robot>
+</robot>