TI voxelsdk點雲ros publisher

利用TI voxelsdk的python接口實時採集點雲數據並通過ros發佈

import Voxel
import numpy as np
import rospy
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2
from std_msgs.msg import Header

rospy.init_node('voxel_publisher')
pub=rospy.Publisher('pointcloude', PointCloud2)

def pointcloud_cb(cam, raw_frame, type_):
    frame = Voxel.XYZIPointCloudFrame.typeCast(raw_frame)
    data = np.array(frame.depth, copy=True).astype(np.float)
    data = np.reshape(data, (frame.size.height, frame.size.width))
    // 刪除intensity數據
    data = np.delete(data, 3, axis=1)
    // 發佈主題
    global pub
    header = Header()
    header.stamp = rospy.Time.now()
    header.frame_id = '%s'%frame.id
    pc = pc2.create_cloud_xyz32(header, data)
    pub.publish(pc)
    
sys=Voxel.CameraSystem()
devs=sys.scan()
cam=sys.connect(devs[0])
cam.registerCallback(Voxel.DepthCamera.FRAME_XYZI_POINT_CLOUD_FRAME, pointcloud_cb)
cam.start()

rate = rospy.Rate(30)
while not rospy.is_shutdown():
    rate.sleep()
    pass
    
cam.stop()