diagnostic_updater_test.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 """
00005 @author Brice Rebsamen <brice [dot] rebsamen [gmail]>
00006 """
00007 
00008 import roslib
00009 roslib.load_manifest('diagnostic_updater')
00010 import rospy
00011 from diagnostic_updater import *
00012 import unittest
00013 import time
00014 
00015 class ClassFunction(DiagnosticTask):
00016     def __init__(self):
00017         DiagnosticTask.__init__(self, "classFunction")
00018 
00019     def run(self, stat):
00020         stat.summary(0, "Test is running")
00021         stat.add("Value", "%f" % 5)
00022         stat.add("String", "Toto")
00023         stat.add("Floating", 5.55)
00024         stat.add("Integer", 5)
00025         stat.add("Formatted", "%s %i", "Hello", 5)
00026         stat.add("Bool", True)
00027         return stat
00028 
00029 class TestClass:
00030     def wrapped(stat):
00031         return stat
00032 
00033 
00034 class TestDiagnosticStatusWrapper(unittest.TestCase):
00035 
00036     def testDiagnosticUpdater(self):
00037         updater = Updater()
00038 
00039         c = TestClass()
00040         updater.add("wrapped", c.wrapped)
00041 
00042         cf = ClassFunction()
00043         updater.add(cf)
00044 
00045 
00046     def testDiagnosticStatusWrapper(self):
00047         stat = DiagnosticStatusWrapper()
00048 
00049         message = "dummy"
00050         level = 1
00051         stat.summary(level, message)
00052         self.assertEqual(message, stat.message, "DiagnosticStatusWrapper::summary failed to set message")
00053         self.assertEqual(level, stat.level, "DiagnosticStatusWrapper::summary failed to set level")
00054 
00055         stat.add("toto", "%.1f" % 5.0)
00056         stat.add("baba", 5)
00057         stat.add("foo", "%05i" % 27)
00058         stat.add("bool", True)
00059         stat.add("bool2", False)
00060 
00061         self.assertEqual("5.0", stat.values[0].value, "Bad value, adding a value with addf")
00062         self.assertEqual("5", stat.values[1].value, "Bad value, adding a string with add")
00063         self.assertEqual("00027", stat.values[2].value, "Bad value, adding a string with addf")
00064         self.assertEqual("toto", stat.values[0].key, "Bad label, adding a value with add")
00065         self.assertEqual("baba", stat.values[1].key, "Bad label, adding a string with add")
00066         self.assertEqual("foo", stat.values[2].key, "Bad label, adding a string with addf")
00067 
00068         self.assertEqual("bool", stat.values[3].key, "Bad label, adding a true bool key with add")
00069         self.assertEqual("True", stat.values[3].value, "Bad value, adding a true bool with add")
00070 
00071         self.assertEqual("bool2", stat.values[4].key, "Bad label, adding a false bool key with add")
00072         self.assertEqual("False", stat.values[4].value, "Bad value, adding a false bool with add")
00073 
00074 
00075     def testFrequencyStatus(self):
00076         freq_bound = {'min': 10, 'max': 20}
00077 
00078         fs = FrequencyStatus(FrequencyStatusParam(freq_bound, 0.5, 2))
00079 
00080         stat = [DiagnosticStatusWrapper() for i in range(5)]
00081         fs.tick()
00082         time.sleep(.02)
00083         stat[0] = fs.run(stat[0]) # Should be too fast, 20 ms for 1 tick, lower limit should be 33ms.
00084         time.sleep(.05)
00085         fs.tick()
00086         stat[1] = fs.run(stat[1]) # Should be good, 70 ms for 2 ticks, lower limit should be 66 ms.
00087         time.sleep(.3)
00088         fs.tick()
00089         stat[2] = fs.run(stat[2]) # Should be good, 350 ms for 2 ticks, upper limit should be 400 ms.
00090         time.sleep(.15)
00091         fs.tick()
00092         stat[3] = fs.run(stat[3]) # Should be too slow, 450 ms for 2 ticks, upper limit should be 400 ms.
00093         fs.clear()
00094         stat[4] = fs.run(stat[4]) # Should be good, just cleared it.
00095 
00096         self.assertEqual(1, stat[0].level, "max frequency exceeded but not reported")
00097         self.assertEqual(0, stat[1].level, "within max frequency but reported error")
00098         self.assertEqual(0, stat[2].level, "within min frequency but reported error")
00099         self.assertEqual(1, stat[3].level, "min frequency exceeded but not reported")
00100         self.assertEqual(2, stat[4].level, "freshly cleared should fail")
00101         self.assertEqual("", stat[0].name, "Name should not be set by FrequencyStatus")
00102         self.assertEqual("Frequency Status", fs.getName(), "Name should be \"Frequency Status\"")
00103 
00104 
00105     def testTimeStampStatus(self):
00106         ts = TimeStampStatus()
00107 
00108         stat = [DiagnosticStatusWrapper() for i in range(5)]
00109         stat[0] = ts.run(stat[0])
00110         ts.tick(rospy.Time.now().to_sec() + 2)
00111         stat[1] = ts.run(stat[1])
00112         ts.tick(rospy.Time.now())
00113         stat[2] = ts.run(stat[2])
00114         ts.tick(rospy.Time.now().to_sec() - 4)
00115         stat[3] = ts.run(stat[3])
00116         ts.tick(rospy.Time.now().to_sec() - 6)
00117         stat[4] = ts.run(stat[4])
00118 
00119         self.assertEqual(1, stat[0].level, "no data should return a warning")
00120         self.assertEqual(2, stat[1].level, "too far future not reported")
00121         self.assertEqual(0, stat[2].level, "now not accepted")
00122         self.assertEqual(0, stat[3].level, "4 seconds ago not accepted")
00123         self.assertEqual(2, stat[4].level, "too far past not reported")
00124         self.assertEqual("", stat[0].name, "Name should not be set by TimeStapmStatus")
00125         self.assertEqual("Timestamp Status", ts.getName(), "Name should be \"Timestamp Status\"")
00126 
00127 
00128 if __name__ == '__main__':
00129     rospy.init_node("test_node")
00130     unittest.main()


diagnostic_updater
Author(s): Kevin Watts, Brice Rebsamen , Jeremy Leibs, Blaise Gassend
autogenerated on Tue Mar 26 2019 03:09:44