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
00032
00033
00034
00035 #ifndef SELFTEST_HH
00036 #define SELFTEST_HH
00037
00038 #include <stdexcept>
00039 #include <vector>
00040 #include <string>
00041
00042 #include <boost/thread.hpp>
00043 #include <ros/callback_queue.h>
00044
00045 #include "diagnostic_msgs/DiagnosticStatus.h"
00046 #include "diagnostic_msgs/SelfTest.h"
00047 #include "diagnostic_updater/diagnostic_updater.h"
00048
00049 namespace self_test
00050 {
00051
00052 using namespace diagnostic_updater;
00053
00068 class TestRunner : public DiagnosticTaskVector
00069 {
00070 private:
00071 ros::ServiceServer service_server_;
00072 ros::CallbackQueue self_test_queue_;
00073 ros::NodeHandle node_handle_;
00074 std::string id_;
00075
00076 bool verbose;
00077
00078 public:
00079 using DiagnosticTaskVector::add;
00080
00089 TestRunner(ros::NodeHandle h = ros::NodeHandle()) :
00090 node_handle_(h)
00091 {
00092 ROS_DEBUG("Advertising self_test");
00093 ros::NodeHandle private_node_handle_("~");
00094 private_node_handle_.setCallbackQueue(&self_test_queue_);
00095 service_server_ = private_node_handle_.advertiseService("self_test", &TestRunner::doTest, this);
00096 verbose = true;
00097 }
00098
00104 void checkTest()
00105 {
00106 self_test_queue_.callAvailable(ros::WallDuration(0));
00107 }
00108
00118 void setID(std::string id)
00119 {
00120 id_ = id;
00121 }
00122
00123 private:
00127 bool doTest(diagnostic_msgs::SelfTest::Request &req,
00128 diagnostic_msgs::SelfTest::Response &res)
00129 {
00130 bool retval = false;
00131 bool ignore_set_id_warn = false;
00132
00133 if (node_handle_.ok())
00134 {
00135 const std::string unspecified_id("unspecified");
00136
00137 ROS_INFO("Entering self-test.");
00138
00139 std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
00140
00141 const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
00142 for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
00143 iter != tasks.end(); iter++)
00144 {
00145 if (ros::isShuttingDown())
00146 {
00147 ROS_ERROR("ROS has shut down. Exiting.");
00148 return false;
00149 }
00150
00151 diagnostic_updater::DiagnosticStatusWrapper status;
00152
00153 status.level = 2;
00154 status.message = "No message was set";
00155
00156 try {
00157 ROS_INFO("Starting test: %s", iter->getName().c_str());
00158 iter->run(status);
00159
00160 } catch (std::exception& e)
00161 {
00162 status.level = 2;
00163 status.message = std::string("Uncaught exception: ") + e.what();
00164 ignore_set_id_warn = true;
00165 }
00166
00167 if (status.level >= 1)
00168 if (verbose)
00169 ROS_WARN("Non-zero self-test test status. Name: '%s', status %i: '%s'", status.name.c_str(), status.level, status.message.c_str());
00170
00171 status_vec.push_back(status);
00172 }
00173
00174 if (!ignore_set_id_warn && id_.empty())
00175 ROS_WARN("setID was not called by any self-test. The node author should be notified. If there is no suitable ID for this node, an ID of 'none' should be used.");
00176
00177
00178 res.id = id_;
00179
00180 res.passed = true;
00181 for (std::vector<diagnostic_msgs::DiagnosticStatus>::iterator status_iter = status_vec.begin();
00182 status_iter != status_vec.end();
00183 status_iter++)
00184 {
00185 if (status_iter->level >= 2)
00186 res.passed = false;
00187 }
00188
00189 if (res.passed && id_ == unspecified_id)
00190 ROS_WARN("Self-test passed, but setID was not called. This is a bug in the driver. Please report it.");
00191
00192 res.status = status_vec;
00193
00194 retval = true;
00195
00196 ROS_INFO("Self-test complete.");
00197 }
00198
00199 return retval;
00200
00201 }
00202 };
00203 };
00204
00205 #endif