5 @author Brice Rebsamen <brice [dot] rebsamen [gmail]> 9 roslib.load_manifest(
'diagnostic_updater')
11 from diagnostic_updater
import *
17 DiagnosticTask.__init__(self,
"classFunction")
20 stat.summary(0,
"Test is running")
21 stat.add(
"Value",
"%f" % 5)
22 stat.add(
"String",
"Toto")
23 stat.add(
"Floating", 5.55)
24 stat.add(
"Integer", 5)
25 stat.add(
"Formatted",
"%s %i",
"Hello", 5)
26 stat.add(
"Bool",
True)
40 updater.add(
"wrapped", c.wrapped)
51 stat.summary(level, message)
52 self.assertEqual(message, stat.message,
"DiagnosticStatusWrapper::summary failed to set message")
53 self.assertEqual(level, stat.level,
"DiagnosticStatusWrapper::summary failed to set level")
55 stat.add(
"toto",
"%.1f" % 5.0)
57 stat.add(
"foo",
"%05i" % 27)
58 stat.add(
"bool",
True)
59 stat.add(
"bool2",
False)
61 self.assertEqual(
"5.0", stat.values[0].value,
"Bad value, adding a value with addf")
62 self.assertEqual(
"5", stat.values[1].value,
"Bad value, adding a string with add")
63 self.assertEqual(
"00027", stat.values[2].value,
"Bad value, adding a string with addf")
64 self.assertEqual(
"toto", stat.values[0].key,
"Bad label, adding a value with add")
65 self.assertEqual(
"baba", stat.values[1].key,
"Bad label, adding a string with add")
66 self.assertEqual(
"foo", stat.values[2].key,
"Bad label, adding a string with addf")
68 self.assertEqual(
"bool", stat.values[3].key,
"Bad label, adding a true bool key with add")
69 self.assertEqual(
"True", stat.values[3].value,
"Bad value, adding a true bool with add")
71 self.assertEqual(
"bool2", stat.values[4].key,
"Bad label, adding a false bool key with add")
72 self.assertEqual(
"False", stat.values[4].value,
"Bad value, adding a false bool with add")
76 freq_bound = {
'min': 10,
'max': 20}
83 stat[0] = fs.run(stat[0])
86 stat[1] = fs.run(stat[1])
89 stat[2] = fs.run(stat[2])
92 stat[3] = fs.run(stat[3])
94 stat[4] = fs.run(stat[4])
96 self.assertEqual(1, stat[0].level,
"max frequency exceeded but not reported")
97 self.assertEqual(0, stat[1].level,
"within max frequency but reported error")
98 self.assertEqual(0, stat[2].level,
"within min frequency but reported error")
99 self.assertEqual(1, stat[3].level,
"min frequency exceeded but not reported")
100 self.assertEqual(2, stat[4].level,
"freshly cleared should fail")
101 self.assertEqual(
"", stat[0].name,
"Name should not be set by FrequencyStatus")
102 self.assertEqual(
"Frequency Status", fs.getName(),
"Name should be \"Frequency Status\"")
109 stat[0] = ts.run(stat[0])
110 ts.tick(rospy.Time.now().to_sec() + 2)
111 stat[1] = ts.run(stat[1])
112 ts.tick(rospy.Time.now())
113 stat[2] = ts.run(stat[2])
114 ts.tick(rospy.Time.now().to_sec() - 4)
115 stat[3] = ts.run(stat[3])
116 ts.tick(rospy.Time.now().to_sec() - 6)
117 stat[4] = ts.run(stat[4])
119 self.assertEqual(1, stat[0].level,
"no data should return a warning")
120 self.assertEqual(2, stat[1].level,
"too far future not reported")
121 self.assertEqual(0, stat[2].level,
"now not accepted")
122 self.assertEqual(0, stat[3].level,
"4 seconds ago not accepted")
123 self.assertEqual(2, stat[4].level,
"too far past not reported")
124 self.assertEqual(
"", stat[0].name,
"Name should not be set by TimeStapmStatus")
125 self.assertEqual(
"Timestamp Status", ts.getName(),
"Name should be \"Timestamp Status\"")
128 if __name__ ==
'__main__':
129 rospy.init_node(
"test_node")
A structure that holds the constructor parameters for the FrequencyStatus class.
def testDiagnosticUpdater(self)
def testTimeStampStatus(self)
def testDiagnosticStatusWrapper(self)
Manages a list of diagnostic tasks, and calls them in a rate-limited manner.
A diagnostic task that monitors the frequency of an event.
def testFrequencyStatus(self)
Diagnostic task to monitor the interval between events.
DiagnosticTask is an abstract base class for collecting diagnostic data.
Wrapper for the diagnostic_msgs::DiagnosticStatus message that makes it easier to update...