#!/usr/bin/env python

# This program publishes a pointcloud2 which has much shorter data
# than what is described by width, height, and point_step.  RViz
# should catch this and show an error instead of crashing.

import rospy
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField

rospy.init_node("bad_pointcloud_publisher")

pfx = PointField()
pfx.name = "x"
pfx.offset = 0
pfx.datatype = 7
pfx.count = 1

pfy = PointField()
pfy.name = "y"
pfy.offset = 4
pfy.datatype = 7
pfy.count = 1

pfz = PointField()
pfz.name = "z"
pfz.offset = 8
pfz.datatype = 7
pfz.count = 1

pc = PointCloud2()

pc.header.frame_id = "map"
pc.header.stamp = rospy.Time.now()

pc.fields = [pfx, pfy, pfz]
pc.data = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
pc.width = 10
pc.height = 10
pc.point_step = 12
pc.row_step = 120
pc.is_dense = False

pub = rospy.Publisher("bad_pointcloud", PointCloud2)

while not rospy.is_shutdown():
    print("publishing bad PointCloud2.")
    pub.publish(pc)
    rospy.sleep(1.0)
