ROS/catkin_ws/src/lstrobot_planning/scripts/dycl_0506.py

#!/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()