Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <ros/ros.h>
00032 #include "diagnostic_msgs/SelfTest.h"
00033 #include "self_test/self_test.h"
00034 #include <stdexcept>
00035
00041 class MyNode
00042 {
00043 public:
00044
00045
00046 self_test::TestRunner self_test_;
00047
00048
00049 double some_val;
00050
00051 ros::NodeHandle nh_;
00052
00053 MyNode() : self_test_()
00054 {
00055 self_test_.add("Pretest", this, &MyNode::pretest );
00056
00057 self_test_.add("ID Lookup", this, &MyNode::test1);
00058 self_test_.add("Error test", this, &MyNode::test2);
00059 self_test_.add("Value generating test", this, &MyNode::test3);
00060 self_test_.add("Value testing test", this, &MyNode::test4);
00061
00062 self_test_.add("Posttest", this, &MyNode::pretest );
00063 }
00064
00065 void pretest(diagnostic_updater::DiagnosticStatusWrapper& status)
00066 {
00067 ROS_INFO("Doing preparation stuff before we run our test.\n");
00068 status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Pretest completed successfully.");
00069
00070 some_val = 1.0;
00071 }
00072
00073 void test1(diagnostic_updater::DiagnosticStatusWrapper& status)
00074 {
00075
00076 char ID[] = "12345";
00077 bool lookup_successful = true;
00078
00079 if (lookup_successful)
00080 {
00081 status.summary(diagnostic_msgs::DiagnosticStatus::OK, "ID Lookup successful");
00082 self_test_.setID(ID);
00083 }
00084 else
00085 status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "ID Lookup failed");
00086 }
00087
00088 void test2(diagnostic_updater::DiagnosticStatusWrapper& status)
00089 {
00090
00091 status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Reporting an error");
00092 }
00093
00094 void test3(diagnostic_updater::DiagnosticStatusWrapper& status)
00095 {
00096 some_val += 41.0;
00097
00098 status.add("some value", some_val);
00099 status.summary(diagnostic_msgs::DiagnosticStatus::OK, "We successfully changed the value.");
00100 }
00101
00102 void test4(diagnostic_updater::DiagnosticStatusWrapper& status)
00103 {
00104 if (some_val == 42.0)
00105 {
00106 status.summary(diagnostic_msgs::DiagnosticStatus::OK, "We observed the change in value");
00107 }
00108 else
00109 {
00110 status.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "We failed to observe the change in value, it is currently %f.", some_val);
00111 }
00112 }
00113
00114 void posttest(diagnostic_updater::DiagnosticStatusWrapper& status)
00115 {
00116 ROS_INFO("Doing cleanup stuff after we run our test.\n");
00117 status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Posttest completed successfully.");
00118 }
00119
00120 bool spin()
00121 {
00122 while (nh_.ok())
00123 {
00124 ros::Duration(1).sleep();
00125
00126 self_test_.checkTest();
00127 }
00128 return true;
00129 }
00130 };
00131
00132 int
00133 main(int argc, char** argv)
00134 {
00135 ros::init(argc, argv, "my_node");
00136
00137 MyNode n;
00138
00139 n.spin();
00140
00141 return(0);
00142 }