jsk_rviz_plugins: pointcloud_information.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 try:
00003   from jsk_rviz_plugins.msg import *
00004 except:
00005   import roslib;roslib.load_manifest("jsk_rviz_plugins")
00006   from jsk_rviz_plugins.msg import *
00007 
00008 from sensor_msgs.msg import PointCloud2
00009 from std_msgs.msg import ColorRGBA, Float32
00010 import rospy
00011 
00012 text_pub = rospy.Publisher("output_text", OverlayText)
00013 
00014 def cloud_cb(cloud):
00015   point_num = cloud.width * cloud.height
00016 
00017   point_type = ""
00018   for field in cloud.fields:
00019     point_type +=field.name
00020     frame_id = cloud.header.frame_id
00021 
00022   text = OverlayText()
00023   text.width = 500
00024   text.height = 80
00025   text.left = 10
00026   text.top = 10
00027   text.text_size = 12
00028   text.line_width = 2
00029   text.font = "DejaVu Sans Mono"
00030   text.text = """Point Cloud Num : %d
00031 PointType       : %s
00032 PointFrame      : %s
00033 """ % (point_num, point_type, frame_id)
00034   text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
00035   text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
00036   text_pub.publish(text)
00037 
00038 
00039 if __name__ == "__main__":
00040   rospy.init_node("pointcloud_information_text")
00041   rospy.Subscriber("input", PointCloud2, cloud_cb)
00042   rospy.spin()