24 from diagnostic_msgs.msg
import DiagnosticArray, DiagnosticStatus, KeyValue
28 rospy.init_node(
"hz_monitor")
33 self.
topics = rospy.get_param(
'~topics')
35 self.
hz = rospy.get_param(
'~hz')
37 self.
hzerror = rospy.get_param(
'~hzerror')
39 self.
window_size = float(rospy.get_param(
'~window_size', 100))
45 rospy.logerr(
'hztest not initialized properly. Parameter [{}] not set. debug[{}] debug[{}]'.format(e, rospy.get_caller_id(), rospy.resolve_name(e.args[0])))
50 self.
pub_diagnostics = rospy.Publisher(
'~diagnostics', DiagnosticArray, queue_size = 1)
58 msg_class, real_topic, msg_eval = rostopic.get_topic_class(topic, blocking=
False)
61 self.missing_topics.remove(real_topic)
64 rospy.logdebug(
"hz monitor is waiting for type of topics {}.".format(self.
missing_topics))
66 except rospy.exceptions.ROSInterruptException:
73 rospy.Subscriber(topic, rospy.AnyMsg, rt.callback_hz)
75 rospy.loginfo(
"subscribed to [{}]".format(topic))
79 if isinstance(self.
hzerror, float)
or isinstance(self.
hzerror, int):
81 rospy.logerr(
"hzerror cannot be negative")
85 if min_rate < 0
or max_rate < 0:
86 rospy.logerr(
"min_rate/max_rate cannot be negative")
88 min_duration = 1/min_rate
if min_rate > 0
else float(
'inf')
89 max_duration = 1/max_rate
if max_rate > 0
else float(
'inf')
91 rospy.logerr(
"hzerror not float or int")
95 array = DiagnosticArray()
96 array.header.stamp = rospy.Time.now()
97 hz_status = DiagnosticStatus()
99 hz_status.level = DiagnosticStatus.OK
100 hz_status.message =
'all publishing rates are ok' 101 hz_status.values.append(KeyValue(
"topics", str(self.
topics)))
102 hz_status.values.append(KeyValue(
"desired_rate", str(self.
hz)))
103 hz_status.values.append(KeyValue(
"min_rate", str(min_rate)))
104 hz_status.values.append(KeyValue(
"max_rate", str(max_rate)))
105 hz_status.values.append(KeyValue(
"window_size", str(self.
window_size)))
108 consolidated_error_messages = {}
109 consolidated_error_messages.setdefault(
"never received message for topics", [])
110 consolidated_error_messages.setdefault(
"no messages anymore for topics", [])
111 consolidated_error_messages.setdefault(
"publishing rate is too low for topics", [])
112 consolidated_error_messages.setdefault(
"publishing rate is too high for topics", [])
115 hz_status.level = DiagnosticStatus.WARN
116 hz_status.message =
"could not determine type for topics {}. Probably never published on that topics.".format(self.
missing_topics)
117 array.status.append(hz_status)
118 self.pub_diagnostics.publish(array)
122 for topic, rt
in self.rt_HZ_store.items():
130 if not rt
or not rt.times:
131 hz_status.level = DiagnosticStatus.ERROR
133 consolidated_error_messages[
"never received message for topics"].append(topic)
134 elif rt.msg_tn == rt.last_printed_tn \
135 and not (event.current_real.to_sec() < rt.msg_tn + min_duration):
136 hz_status.level = DiagnosticStatus.ERROR
138 consolidated_error_messages[
"no messages anymore for topics"].append(topic)
142 mean = sum(rt.times) / n
143 rate = 1./mean
if mean > 0.
else 0
144 rt.last_printed_tn = rt.msg_tn
145 rates.append(round(rate, 2))
146 if min_rate
and rate < min_rate:
147 hz_status.level = DiagnosticStatus.WARN
148 consolidated_error_messages[
"publishing rate is too low for topics"].append(topic)
150 if max_rate
and rate > max_rate:
151 hz_status.level = DiagnosticStatus.WARN
152 consolidated_error_messages[
"publishing rate is too high for topics"].append(topic)
154 if hz_status.level != DiagnosticStatus.OK:
156 key =
"never received message for topics" 157 if len(consolidated_error_messages[key]) > 0:
158 message += key +
" " + str(consolidated_error_messages[key]) +
", " 159 key =
"no messages anymore for topics" 160 if len(consolidated_error_messages[key]) > 0:
161 message += key +
" " + str(consolidated_error_messages[key]) +
", " 162 key =
"publishing rate is too low for topics" 163 if len(consolidated_error_messages[key]) > 0:
164 message += key +
" " + str(consolidated_error_messages[key]) +
", " 165 key =
"publishing rate is too high for topics" 166 if len(consolidated_error_messages[key]) > 0:
167 message += key +
" " + str(consolidated_error_messages[key])
168 hz_status.message = message
170 hz_status.values.append(KeyValue(
"rates", str(rates)))
171 array.status.append(hz_status)
172 self.pub_diagnostics.publish(array)
174 if __name__ ==
'__main__':
def publish_diagnostics(self, event)