tracking_info.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # it depends on jsk_rviz_plugins
00004 
00005 import rospy
00006 from jsk_rviz_plugins.msg import OverlayText
00007 from std_msgs.msg import Float32
00008 from threading import Lock
00009 from jsk_rviz_plugins.overlay_text_interface import OverlayTextInterface
00010 from jsk_recognition_msgs.msg import TrackerStatus
00011 g_lock = Lock()
00012 g_angle_error_msg = None
00013 g_distance_error_msg = None
00014 g_tracker_status_msg = None
00015 def angle_error_callback(msg):
00016     global g_angle_error_msg
00017     with g_lock:
00018         g_angle_error_msg = msg
00019 def distance_error_callback(msg):
00020     global g_distance_error_msg
00021     with g_lock:
00022         g_distance_error_msg = msg
00023 def tracker_status_callback(msg):
00024     global g_tracker_status_msg
00025     with g_lock:
00026         g_tracker_status_msg = msg
00027 def publish_text(event):
00028     with g_lock:
00029         if g_distance_error_msg and g_angle_error_msg:
00030             text_interface.publish("""RMS Distance Error: {0}
00031 RMS Angular Error: {1}""".format(g_distance_error_msg.data, g_angle_error_msg.data))
00032 
00033         if g_tracker_status_msg:
00034             if g_tracker_status_msg.is_tracking:
00035                 status_interface.publish("Status: Tracking")
00036             else:
00037                 status_interface.publish("Status: Stable")
00038 
00039 
00040 if __name__ == "__main__":
00041     rospy.init_node("tracking_info")
00042     text_interface = OverlayTextInterface("~text")
00043     status_interface = OverlayTextInterface("~status_text")
00044     sub = rospy.Subscriber("~rms_angle_error", Float32, angle_error_callback)
00045     sub2 = rospy.Subscriber("~rms_distance_error", Float32, distance_error_callback)
00046     sub3 = rospy.Subscriber("/particle_filter_tracker/output/tracker_status", TrackerStatus, tracker_status_callback)
00047     rospy.Timer(rospy.Duration(0.1), publish_text)
00048     rospy.spin()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50