def pcl_sparse_msg()

in a2d2/src/ros_util.py [0:0]

• 25 lines of code
• 2 McCabe index (conditional complexity)

    def pcl_sparse_msg(cls, points=None, reflectance=None, rows=None, cols=None, ts=None, frame_id=None):
  
        rows = (rows + 0.5).astype(np.int)
        height= np.amax(rows) + 1
        
        cols = (cols + 0.5).astype(np.int)
        width=np.amax(cols) + 1

        colors = np.stack([reflectance, reflectance, reflectance], axis=1)
        pca = np.full((height, width, 3), np.inf)
        ca =np.full((height, width, 3), np.inf)
        assert(pca.shape == ca.shape)

        count = points.shape[0]
        for i in range(0, count):
            pca[rows[i], cols[i], :] = points[i]
            ca[rows[i], cols[i], : ] = colors[i]
            
        msg = PointCloud2()
        cls.set_ros_msg_header(ros_msg=msg, ts=ts, frame_id=frame_id)
        
        msg.width = width
        msg.height = height
        
        msg.fields = cls.get_pcl_fields()

        msg.is_bigendian = False
        msg.point_step = 24
        msg.row_step = msg.point_step * width
        msg.is_dense = False
        data_array = np.array(np.hstack([pca, ca]), dtype=np.float32)
        msg.data = data_array.tostring()

        return msg