#!/usr/bin/env python3

import rospy
import pcl
import numpy as np

def main():
    # 初始化ROS节点
    rospy.init_node('transform_pcd_node', anonymous=True)

    # 定义点云地图的路径和文件名
    pcd_path = "/home/tong/renyuan_pointcloud/4/trans/LazerData.pcd"

    # 读取点云地图
    cloud = pcl.load(pcd_path)
    
    points = cloud.to_array()
    centroid = np.mean(points, axis=0)
    points_centered = points - centroid
    
    theta = np.pi
    
    #旋转点云
    transform_matrix = np.array([[1,  0,          0],
                                 [0, np.cos(theta), -np.sin(theta)],
                                 [0, np.sin(theta),  np.cos(theta)]], dtype=np.float32)



    # 手动变换点云
    transformed_points = np.dot(points_centered[:, :3],transform_matrix.T)
    transformed_points += centroid
    
    # #定义位移向量(沿Z轴方向移动一个单位长度)
    
    # print("Transformed Points (before translation):", transformed_points)
    
    translation_vector = np.array([-1000, 0, 0], dtype=np.float32)
    transformed_points += translation_vector
    
    # print("Transformed Points (after translation):", transformed_points)
    
    # 创建新的点云对象
    transformed_cloud = pcl.PointCloud()
    transformed_cloud.from_array(transformed_points.astype(np.float32))

    # 保存旋转后的点云地图到PCD文件中
    pcl.save(transformed_cloud, "/home/tong/renyuan_pointcloud/4/trans/trans.pcd")

if __name__ == "__main__":
    main()