35 #include <gtest/gtest.h> 46 void unwrapped(diagnostic_msgs::DiagnosticStatus &s)
55 TEST(DiagnosticUpdater, testDiagnosticUpdater)
65 s.
summary(0,
"Test is running");
66 s.
addf(
"Value",
"%f", 5);
67 s.
add(
"String",
"Toto");
68 s.
add(
"Floating", 5.55);
70 s.
addf(
"Formatted %s %i",
"Hello", 5);
86 TEST(DiagnosticUpdater, testDiagnosticStatusWrapperKeyValuePairs)
90 const char *message =
"dummy";
93 EXPECT_STREQ(message, stat.message.c_str()) <<
"DiagnosticStatusWrapper::summary failed to set message";
94 EXPECT_EQ(level, stat.level) <<
"DiagnosticStatusWrapper::summary failed to set level";
96 stat.
addf(
"toto",
"%.1f", 5.0);
98 stat.
addf(
"foo",
"%05i", 27);
100 stat.
add(
"bool",
true);
101 stat.
add(
"bool2",
false);
103 EXPECT_STREQ(
"5.0", stat.values[0].value.c_str()) <<
"Bad value, adding a value with addf";
104 EXPECT_STREQ(
"5", stat.values[1].value.c_str()) <<
"Bad value, adding a string with add";
105 EXPECT_STREQ(
"00027", stat.values[2].value.c_str()) <<
"Bad value, adding a string with addf";
106 EXPECT_STREQ(
"toto", stat.values[0].key.c_str()) <<
"Bad label, adding a value with add";
107 EXPECT_STREQ(
"baba", stat.values[1].key.c_str()) <<
"Bad label, adding a string with add";
108 EXPECT_STREQ(
"foo", stat.values[2].key.c_str()) <<
"Bad label, adding a string with addf";
110 EXPECT_STREQ(
"bool", stat.values[3].key.c_str()) <<
"Bad label, adding a true bool key with add";
111 EXPECT_STREQ(
"True", stat.values[3].value.c_str()) <<
"Bad label, adding a true bool with add";
113 EXPECT_STREQ(
"bool2", stat.values[4].key.c_str()) <<
"Bad label, adding a false bool key with add";
114 EXPECT_STREQ(
"False", stat.values[4].value.c_str()) <<
"Bad label, adding a false bool with add";
117 TEST(DiagnosticUpdater, testDiagnosticStatusWrapperMergeSummary)
124 EXPECT_STREQ(
"Old", stat.message.c_str()) <<
"Bad summary, merging levels (OK,OK)";
129 EXPECT_STREQ(
"New", stat.message.c_str()) <<
"Bad summary, merging levels (OK,WARN)";
134 EXPECT_STREQ(
"Old; New", stat.message.c_str()) <<
"Bad summary, merging levels (WARN,WARN)";
139 EXPECT_STREQ(
"Old; New", stat.message.c_str()) <<
"Bad summary, merging levels (WARN,ERROR)";
142 TEST(DiagnosticUpdater, testFrequencyStatus)
165 EXPECT_EQ(1, stat[0].level) <<
"max frequency exceeded but not reported";
166 EXPECT_EQ(0, stat[1].level) <<
"within max frequency but reported error";
167 EXPECT_EQ(0, stat[2].level) <<
"within min frequency but reported error";
168 EXPECT_EQ(1, stat[3].level) <<
"min frequency exceeded but not reported";
169 EXPECT_EQ(2, stat[4].level) <<
"freshly cleared should fail";
170 EXPECT_STREQ(
"", stat[0].name.c_str()) <<
"Name should not be set by FrequencyStatus";
171 EXPECT_STREQ(
"Frequency Status", fs.
getName().c_str()) <<
"Name should be \"Frequency Status\"";
174 TEST(DiagnosticUpdater, testTimeStampStatus)
189 EXPECT_EQ(1, stat[0].level) <<
"no data should return a warning";
190 EXPECT_EQ(2, stat[1].level) <<
"too far future not reported";
191 EXPECT_EQ(0, stat[2].level) <<
"now not accepted";
192 EXPECT_EQ(0, stat[3].level) <<
"4 seconds ago not accepted";
193 EXPECT_EQ(2, stat[4].level) <<
"too far past not reported";
194 EXPECT_STREQ(
"", stat[0].name.c_str()) <<
"Name should not be set by TimeStapmStatus";
195 EXPECT_STREQ(
"Timestamp Status", ts.
getName().c_str()) <<
"Name should be \"Timestamp Status\"";
198 int main(
int argc,
char **argv){
200 testing::InitGoogleTest(&argc, argv);
201 return RUN_ALL_TESTS();
static TimeStampStatusParam DefaultTimeStampStatusParam
Default TimeStampStatusParam. This is like calling the constructor with no arguments.
void unwrapped(diagnostic_msgs::DiagnosticStatus &s)
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Fills out this Task's DiagnosticStatusWrapper.
virtual void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Fills out this Task's DiagnosticStatusWrapper.
void summary(unsigned char lvl, const std::string s)
Fills out the level and message fields of the DiagnosticStatus.
A structure that holds the constructor parameters for the FrequencyStatus class.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void add(const std::string &name, TaskFunction f)
Add a DiagnosticTask embodied by a name and function to the DiagnosticTaskVector. ...
int main(int argc, char **argv)
void addf(const std::string &key, const char *format,...)
Add a key-value pair using a format string.
Manages a list of diagnostic tasks, and calls them in a rate-limited manner.
A diagnostic task that monitors the frequency of an event.
void wrapped(DiagnosticStatusWrapper &s)
void tick()
Signals that an event has occurred.
Diagnostic task to monitor the interval between events.
void clear()
Resets the statistics.
void mergeSummary(unsigned char lvl, const std::string s)
Merges a level and message with the existing ones.
const std::string & getName()
Returns the name of the DiagnosticTask.
TEST(DiagnosticUpdater, testDiagnosticUpdater)
DiagnosticTask is an abstract base class for collecting diagnostic data.
void add(const std::string &key, const T &val)
Add a key-value pair.
void tick(double stamp)
Signals an event. Timestamp stored as a double.
Wrapper for the diagnostic_msgs::DiagnosticStatus message that makes it easier to update...