jsk_pcl_ros_utils: cloud_on_plane

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import rospy
00003 from jsk_rviz_plugins.msg import OverlayText
00004 from std_msgs.msg import Float32
00005 from threading import Lock
00006 from jsk_rviz_plugins.overlay_text_interface import OverlayTextInterface
00007 from jsk_recognition_msgs.msg import BoolStamped
00008 
00009 g_lock = Lock()
00010 g_msg = None
00011 def callback(msg):
00012     global g_msg
00013     with g_lock:
00014         g_msg = msg
00015 def publish_text(event):
00016     with g_lock:
00017         if g_msg:
00018             if g_msg.data:
00019                 text_interface.publish('Is Object On Plane?: ON')
00020             else:
00021                 text_interface.publish('Is Object On Plane?: <span style="color: red">Off</span>')
00022 
00023 
00024 if __name__ == "__main__":
00025     rospy.init_node("cloud_on_plane_info")
00026     text_interface = OverlayTextInterface("~text")
00027     sub = rospy.Subscriber("~input", BoolStamped, callback)
00028     rospy.Timer(rospy.Duration(0.1), publish_text)
00029     rospy.spin()