PointCloud2_to_PCD.py

PointCloud2_to_PCD.py

import ros_numpy
import rospy
from sensor_msgs.msg import PointCloud2
import open3d as o3d

class PointCloud2Converter:
    def __init__(self):    
        self.pc_npy = None
        self.sub_pc = rospy.Subscriber('/aspara_detector/ground_point_cloud', PointCloud2, self.callback_pcl)
        self.pcd = o3d.geometry.PointCloud()

    def callback_pcl(self, pc2_msg):
        rospy.loginfo("PointCloud acquired")
        self.pc_npy  = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pc2_msg)
        self.pcd.points = o3d.utility.Vector3dVector(self.pc_npy)
        o3d.io.write_point_cloud("points.pcd", self.pcd)

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            rate.sleep()

if __name__ == "__main__":
    rospy.init_node("ui", anonymous=True)
    conv = PointCloud2Converter()
    conv.run()