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 ros::NodeHandle private_node_handle_;
00075 std::string id_;
00076
00077 bool verbose;
00078
00079 public:
00080 using DiagnosticTaskVector::add;
00081
00090 TestRunner(ros::NodeHandle h = ros::NodeHandle(), ros::NodeHandle ph = ros::NodeHandle("~")) :
00091 node_handle_(h),
00092 private_node_handle_(ph)
00093 {
00094 ROS_DEBUG("Advertising self_test");
00095 ros::AdvertiseServiceOptions ops;
00096 ops.init<diagnostic_msgs::SelfTest::Request, diagnostic_msgs::SelfTest::Response>("self_test", boost::bind(&TestRunner::doTest, this, _1, _2));
00097 ops.callback_queue = &self_test_queue_;
00098 service_server_ = private_node_handle_.advertiseService(ops);
00099 verbose = true;
00100 }
00101
00107 void checkTest()
00108 {
00109 self_test_queue_.callAvailable(ros::WallDuration(0));
00110 }
00111
00121 void setID(std::string id)
00122 {
00123 id_ = id;
00124 }
00125
00126 private:
00130 bool doTest(diagnostic_msgs::SelfTest::Request &req,
00131 diagnostic_msgs::SelfTest::Response &res)
00132 {
00133 bool retval = false;
00134 bool ignore_set_id_warn = false;
00135
00136 if (node_handle_.ok())
00137 {
00138 const std::string unspecified_id("unspecified");
00139
00140 ROS_INFO("Entering self-test.");
00141
00142 std::vector<diagnostic_msgs::DiagnosticStatus> status_vec;
00143
00144 const std::vector<DiagnosticTaskInternal> &tasks = getTasks();
00145 for (std::vector<DiagnosticTaskInternal>::const_iterator iter = tasks.begin();
00146 iter != tasks.end(); iter++)
00147 {
00148 if (ros::isShuttingDown())
00149 {
00150 ROS_ERROR("ROS has shut down. Exiting.");
00151 return false;
00152 }
00153
00154 diagnostic_updater::DiagnosticStatusWrapper status;
00155
00156 status.level = 2;
00157 status.message = "No message was set";
00158
00159 try {
00160 ROS_INFO("Starting test: %s", iter->getName().c_str());
00161 iter->run(status);
00162
00163 } catch (std::exception& e)
00164 {
00165 status.level = 2;
00166 status.message = std::string("Uncaught exception: ") + e.what();
00167 ignore_set_id_warn = true;
00168 }
00169
00170 if (status.level >= 1)
00171 if (verbose)
00172 ROS_WARN("Non-zero self-test test status. Name: '%s', status %i: '%s'", status.name.c_str(), status.level, status.message.c_str());
00173
00174 status_vec.push_back(status);
00175 }
00176
00177 if (!ignore_set_id_warn && id_.empty())
00178 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.");
00179
00180
00181 res.id = id_;
00182
00183 res.passed = true;
00184 for (std::vector<diagnostic_msgs::DiagnosticStatus>::iterator status_iter = status_vec.begin();
00185 status_iter != status_vec.end();
00186 status_iter++)
00187 {
00188 if (status_iter->level >= 2)
00189 res.passed = false;
00190 }
00191
00192 if (res.passed && id_ == unspecified_id)
00193 ROS_WARN("Self-test passed, but setID was not called. This is a bug in the driver. Please report it.");
00194
00195 res.status = status_vec;
00196
00197 retval = true;
00198
00199 ROS_INFO("Self-test complete.");
00200 }
00201
00202 return retval;
00203
00204 }
00205 };
00206 };
00207
00208 #endif