multi_topic_msgs = dict()
class MultiTopicCallback():
def __init__(self, topic):
self.topic = topic
global multi_topic_msgs
multi_topic_msgs[self.topic] = None
def callback(self, msg):
global multi_topic_msgs
with g_lock:
multi_topic_msgs[self.topic] = msg
if __name__ == "__main__":
rospy.init_node("string_to_overlay_text")
text_interface = OverlayTextInterface("~output")
multi_topics = rospy.get_param("~multi_topics", [])
g_format = rospy.get_param("~format", "{0}")
if multi_topics:
subs = []
for topic in multi_topics:
callback = MultiTopicCallback(topic)
subs.append(rospy.Subscriber(topic, String, callback.callback))
rospy.Timer(rospy.Duration(0.1), publish_text_multi)
else:
sub = rospy.Subscriber("~input", String, callback)
rospy.Timer(rospy.Duration(0.1), publish_text)
rospy.spin()
global g_tracker_status_msg
with g_lock:
g_tracker_status_msg = msg
def publish_text(event):
with g_lock:
if g_distance_error_msg and g_angle_error_msg:
text_interface.publish("""RMS Distance Error: {0}
RMS Angular Error: {1}""".format(g_distance_error_msg.data,
g_angle_error_msg.data))
if g_tracker_status_msg:
if g_tracker_status_msg.is_tracking:
status_interface.publish("Status: Tracking")
else:
status_interface.publish("Status: Stable")
if __name__ == "__main__":
rospy.init_node("tracking_info")
text_interface = OverlayTextInterface("~text")
status_interface = OverlayTextInterface("~status_text")
sub = rospy.Subscriber("~rms_angle_error", Float32, angle_error_callback)
sub2 = rospy.Subscriber("~rms_distance_error", Float32,
distance_error_callback)
sub3 = rospy.Subscriber("/particle_filter_tracker/output/tracker_status",
TrackerStatus, tracker_status_callback)
rospy.Timer(rospy.Duration(0.1), publish_text)
rospy.spin()
with g_resolution_lock:
g_resolution_msg = msg
def cloud_callback(msg):
global g_cloud_msg, g_cloud_lock
with g_cloud_lock:
g_cloud_msg = msg
def publish_text(event):
global g_resolution_msg, g_resolution_lock, g_cloud_msg, g_cloud_lock
with g_resolution_lock:
with g_cloud_lock:
if not g_cloud_msg or not g_resolution_msg:
return
text_interface.publish("""PointCloud Resolution: {0}
Number of Points: {1}""".format(g_resolution_msg.data,
g_cloud_msg.width * g_cloud_msg.height))
if __name__ == "__main__":
rospy.init_node("octree_info")
# pub = rospy.Publisher("~text", OverlayText)
text_interface = OverlayTextInterface("~text")
sub_resolution = rospy.Subscriber("~resolution", Float32,
resolution_callback)
sub_cloud = rospy.Subscriber("~cloud", PointCloud2, cloud_callback)
rospy.Timer(rospy.Duration(0.1), publish_text)
rospy.spin()