#!/usr/bin/env python3 import rospy import numpy as np import open3d as o3d from scipy.spatial.transform import Rotation as R import os from sensor_msgs.msg import PointCloud2 from sensor_msgs.msg import PointField folder_path = rospy.get_param("folder_path") file_path = os.path.join(folder_path, 'pointcloud.txt') file_path_points = os.path.join(folder_path, 'points.txt') file_path_down = os.path.join(folder_path, 'pointcloud_down.pcd') file_path_octomap = os.path.join(folder_path, 'pointcloud_octomap.pcd') #chen############################################################################################ def cull_pointcloud(data,culling_radius): data_retained,data_culled = [],[] for i in range(data.shape[0]): # 遍历每一个点 P for j in range(len(vectors)): # 遍历每一个向量 u A = start_points[j] - culling_radius * (vectors[j] / np.linalg.norm(vectors[j])) B = end_points[j] + culling_radius * (vectors[j] / np.linalg.norm(vectors[j])) aaa = (np.linalg.norm(data[i] - A) + np.linalg.norm(data[i] - B)) <= (2 * culling_radius + np.linalg.norm(B - A)) distance = np.linalg.norm(np.cross(vectors[j], data[i]-start_points[j])) / np.linalg.norm(vectors[j]) # 如果有任何一个距离小于等于10，则标记为不满足条件 if distance <= culling_radius and aaa: # if aaa: data_culled.append(data[i]) break # 如果该点对所有向量的距离都大于10，则保留该点 elif j == len(vectors)-1: data_retained.append(data[i]) data_retained = np.array(data_retained) data_culled = np.array(data_culled) return data_retained,data_culled def visual_data(data): # 可视化 pt_cloud_circular = o3d.geometry.PointCloud() pt_cloud_circular.points = o3d.utility.Vector3dVector(data) o3d.visualization.draw_geometries([pt_cloud_circular]) ################################点云降采样################################ # 从文件中读取焊缝起点和终点，并计算两点构成的向量 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)] vectors.append(vector) return start_points,end_points,vectors # 读取文件 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 def open_file(file_path): if '.txt' in file_path: pcd = o3d.io.read_point_cloud(file_path, format='xyz') else: pcd = o3d.io.read_point_cloud(file_path) return pcd def save_file(content, file_path, write_ascii=True): if '.txt' in file_path: np.savetxt(file_path, np.asarray(content.points), fmt='%f %f %f') else: o3d.io.write_point_cloud(file_path, content, write_ascii=write_ascii) def voxel_down_sample(pcd, voxel_size): """ 体素方法点云降采样 :param pcd: 点云数据 :param voxel_size: 体素降采样的网格长度 :return: 降采样点云数据 """ return pcd.voxel_down_sample(voxel_size=voxel_size) def add_rgb_to_pcd(pcd): # 为点云中的每个点指定RGB颜色，这里我们使用红色作为示例 # RGB值范围为[0, 1]，红色可以表示为(1, 0, 0) pcd = np.asarray(pcd.points).shape[0] colors = np.repeat([[1, 0, 0]], pcd, axis=0) # 创建一个全是红色的颜色数组 # 将颜色信息添加到点云对象中 pcd.colors = o3d.utility.Vector3dVector(colors) return pcd # 降采样 # pcd = open_file(file_path) pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(load_point_cloud_from_binary_txt(file_path)) pcd = pcd.voxel_down_sample(voxel_size = 4) save_file(pcd,file_path_down) ################################去掉焊缝周围的点云################################# # 计算焊缝向量 start_points,end_points,vectors = np.array(read_and_calculate_vectors(file_path_points)) # 使用pcread直接读取点云数据 ptCloud = o3d.io.read_point_cloud(file_path_down) data = np.asarray(ptCloud.points) culling_radius1 = 20 culling_radius2 = float(rospy.get_param('culling_radius')) # culling_radius2 = 6 data_retained2,data_culled2 =cull_pointcloud(data,culling_radius2) # rospy.loginfo('Point cloud culling is complete') #分批剔除 # data_retained1,data_culled1 =cull_pointcloud(data,culling_radius1) # # visual_data(data_culled1) # data_retained2,data_culled2 =cull_pointcloud(data_culled1,culling_radius2) # visual_data(data_retained2) # visual_data(data_culled2) ################################变换单位并保存################################ # 将点云数据的小数点向前移动三位（即乘以1000） # data_scaled1 = np.array(data_retained1) / 1000 data_scaled2 = np.array(data_retained2) / 1000 # 创建一个新的pointCloud对象来存储缩放后的数据 ptCloud_scaled1 = o3d.geometry.PointCloud() # ptCloud_scaled1.points = o3d.utility.Vector3dVector(data_scaled1) ptCloud_scaled2 = o3d.geometry.PointCloud() ptCloud_scaled2.points = o3d.utility.Vector3dVector(data_scaled2) # 将缩放后的点云数据写入新的 PCD 文件 # o3d.io.write_point_cloud(file_path_octomap, ptCloud_scaled1, write_ascii=True) # 读取保存的点云数据 # pcd = o3d.io.read_point_cloud(file_path_octomap) ##################################################################################################################################################################### def build_pointcloud2_msg(points): msg = PointCloud2() msg.header.stamp = rospy.Time(0) msg.header.frame_id = "base_link" if len(points.shape) == 3: msg.height = points.shape[1] msg.width = points.shape[0] else: msg.height = 1 msg.width = len(points) msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1)] msg.is_bigendian = False msg.point_step = 12 msg.row_step = msg.point_step * points.shape[0] msg.is_dense = False msg.data = np.asarray(points, np.float32).tobytes() return msg def talker(): pointcloud_topic = rospy.get_param('pointcloud_topic') pub1 = rospy.Publisher("/pointcloud/output", PointCloud2, queue_size=10) # pub2 = rospy.Publisher("/pointcloud/output2", PointCloud2, queue_size=10) rospy.init_node('publish_pointcloud', anonymous=True) rate = rospy.Rate(10) points1 = np.asarray(ptCloud_scaled1.points) points2 = np.asarray(ptCloud_scaled2.points) # msg1 = build_pointcloud2_msg(points1) msg2 = build_pointcloud2_msg(points2) while not rospy.is_shutdown(): #机械臂完毕，退出点云发布 sign_pointcloud = str(rospy.get_param("sign_pointcloud")) if sign_pointcloud == "1": rospy.set_param("sign_pointcloud",0) break pub1.publish(msg2) # pub2.publish(msg2) # print("pointcloud has published,sign_pointcloud ={}".format(sign_pointcloud)) rate.sleep() if __name__ == '__main__': talker()