diagnostics_common.hpp
Go to the documentation of this file.
00001 
00029 #ifndef _DIAGNOSTIC_COMMON_HPP_
00030 #define _DIAGNOSTIC_COMMON_HPP_
00031 
00032 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00033 #include <diagnostic_msgs/DiagnosticArray.h>
00034 #include <diagnostic_msgs/DiagnosticStatus.h>
00035 #include <boost/ptr_container/ptr_vector.hpp>
00036 #include <boost/ptr_container/ptr_map.hpp>
00037 #include <boost/variant.hpp>
00038 #include <sstream>
00039 #include <ros/ros.h>
00040 #include <self_test/self_test.h>
00041 
00042 
00043 namespace shadow_robot
00044 {
00045   typedef boost::variant<int, double> DiagValues;
00046 
00047   class DiagnosticTest
00048   {
00049   public:
00050     DiagnosticTest()
00051     {};
00052     ~DiagnosticTest()
00053     {};
00054 
00055     std::vector<DiagValues> received_values;
00056     std::pair<DiagValues, DiagValues> min_max;
00057   };
00058 
00059   typedef std::map<std::string, DiagnosticTest> DiagMap;
00060 
00065   class VariantParser
00066     : public boost::static_visitor<void>
00067   {
00068   public:
00069     void operator()(int int_val) const
00070     {
00071       values_it->second.received_values.push_back( ::atoi( new_value.c_str() ) );
00072     }
00073 
00074     void operator()(double double_val) const
00075     {
00076       values_it->second.received_values.push_back( ::atof(new_value.c_str() ) );
00077     }
00078 
00079     DiagMap::iterator values_it;
00080     std::string new_value;
00081   };
00082 
00086   class VariantGreaterThan
00087     : public boost::static_visitor<bool>
00088   {
00089   public:
00090     bool operator()(const int value1, const int value2) const
00091     {
00092       return value1 >= value2;
00093     }
00094 
00095     bool operator()(const double value1, const double value2) const
00096     {
00097       return value1 >= value2;
00098     }
00099 
00100     bool operator()(const double value1, const int value2) const
00101     {
00102       return value1 >= value2;
00103     }
00104 
00105     bool operator()(const int value1, const double value2) const
00106     {
00107       return value1 >= value2;
00108     }
00109   };
00110 
00111   class BaseDiagnostics
00112   {
00113   public:
00114     BaseDiagnostics(std::string name, self_test::TestRunner* test_runner)
00115       : name(name), test_runner_(test_runner)
00116     {}
00117 
00118     virtual ~BaseDiagnostics()
00119     {}
00120 
00121     virtual void parse_diagnostics(std::vector<diagnostic_msgs::KeyValue> values,
00122                                    short level, std::string full_name) = 0;
00123 
00124     virtual void add_test()
00125     {
00126       test_runner_->add(full_name, this, &BaseDiagnostics::run_test);
00127     }
00128 
00129     virtual void run_test(diagnostic_updater::DiagnosticStatusWrapper& status)
00130     {
00131       std::pair<bool, std::string> res = to_string_();
00132       if( res.first )
00133         status.summary( diagnostic_msgs::DiagnosticStatus::OK, res.second );
00134       else
00135         status.summary( diagnostic_msgs::DiagnosticStatus::ERROR, res.second );
00136     };
00137 
00138     virtual std::auto_ptr<BaseDiagnostics> shallow_clone(std::string name) = 0;
00139 
00140     std::string name;
00141     std::string full_name;
00142 
00143   protected:
00144     self_test::TestRunner* test_runner_;
00145 
00146     virtual std::pair<bool, std::string> to_string_() = 0;
00147   };
00148 
00149   class MinMaxDiagnostics
00150     : public BaseDiagnostics
00151   {
00152   public:
00153     MinMaxDiagnostics(std::string name, self_test::TestRunner* test_runner)
00154       : BaseDiagnostics(name, test_runner)
00155     {};
00156 
00157     ~MinMaxDiagnostics()
00158     {};
00159 
00160     virtual void parse_diagnostics(std::vector<diagnostic_msgs::KeyValue> values,
00161                                    short level, std::string full_name)
00162     {
00163       this->full_name = full_name;
00164 
00165       for( size_t values_i = 0; values_i < values.size(); ++values_i )
00166       {
00167         DiagMap::iterator values_it;
00168         for(values_it = values_->begin(); values_it != values_->end(); ++values_it)
00169         {
00170           if( values[values_i].key.compare(values_it->first) == 0 )
00171           {
00172             VariantParser parser;
00173             parser.new_value = values[values_i].value;
00174             parser.values_it = values_it;
00175             boost::apply_visitor( parser, values_it->second.min_max.first);
00176           }
00177         }
00178       }
00179     }
00180 
00181     virtual std::auto_ptr<BaseDiagnostics> shallow_clone(std::string name)
00182     {
00183       std::auto_ptr<BaseDiagnostics> tmp( new MinMaxDiagnostics(name, test_runner_) );
00184       return tmp;
00185     };
00186 
00187   protected:
00188     boost::shared_ptr< DiagMap > values_;
00189 
00190     virtual std::pair<bool, std::string> to_string_()
00191     {
00192       std::stringstream ss;
00193       bool ok = true;
00194       DiagValues out_of_range_value;
00195 
00196       ss << "\nDiagnostics[" << name << "]:";
00197 
00198       DiagMap::iterator values_it;
00199       VariantGreaterThan greater_than;
00200       for(values_it = values_->begin(); values_it != values_->end(); ++values_it)
00201       {
00202         //We're checking all values. If one is out of the specified range -> test fails
00203         for(size_t i = 0; i<values_it->second.received_values.size(); ++i)
00204         {
00205           //is value > min?
00206           bool min_comp = boost::apply_visitor(greater_than, values_it->second.received_values[i], values_it->second.min_max.first);
00207           //is value > max?
00208           bool max_comp = boost::apply_visitor(greater_than, values_it->second.received_values[i], values_it->second.min_max.second);
00209 
00210           // value is in the [min;max] interval
00211           if( min_comp && (!max_comp) )
00212           { }
00213           else
00214           {
00215             ok = false;
00216             out_of_range_value = values_it->second.received_values[i];
00217             break; //test fails no need to go further
00218           }
00219         }
00220         if (ok)
00221         {
00222           ss <<" OK(";
00223         }
00224         else
00225         {
00226           ss <<" ERROR(";
00227         }
00228 
00229         //we're returning the first value that was out of the range
00230         ss << values_it->first << "=" << out_of_range_value <<")";
00231       }
00232 
00233       return std::pair<bool, std::string>(ok, ss.str());
00234     }
00235   };
00236 
00237   class IsOKDiagnostics
00238     : public BaseDiagnostics
00239   {
00240   public:
00241     IsOKDiagnostics(std::string name, self_test::TestRunner* test_runner)
00242       : BaseDiagnostics(name, test_runner)
00243     {};
00244 
00245     ~IsOKDiagnostics()
00246     {};
00247 
00248     virtual void parse_diagnostics(std::vector<diagnostic_msgs::KeyValue> values,
00249                                    short level, std::string full_name)
00250     {
00251       this->full_name = full_name;
00252       level_ = level;
00253     };
00254 
00255     virtual std::auto_ptr<BaseDiagnostics> shallow_clone(std::string name)
00256     {
00257       std::auto_ptr<BaseDiagnostics> tmp( new IsOKDiagnostics(name, test_runner_) );
00258       return tmp;
00259     };
00260 
00261   protected:
00262     short level_;
00263 
00264     virtual std::pair<bool, std::string> to_string_()
00265     {
00266       std::stringstream ss;
00267       bool ok = true;
00268 
00269       ss << "Diagnostics[" << full_name << "]:";
00270 
00271       if( level_ == diagnostic_msgs::DiagnosticStatus::ERROR )
00272       {
00273         ok = false;
00274         ss << " status = ERROR";
00275       }
00276       else if( level_ == diagnostic_msgs::DiagnosticStatus::WARN )
00277       {
00278         ss << " status = WARN";
00279       }
00280       else
00281       {
00282         ss << " status = OK";
00283       }
00284 
00285       return std::pair<bool, std::string>(ok, ss.str());
00286     };
00287   };
00288 }
00289 
00290   /* For the emacs weenies in the crowd.
00291      Local Variables:
00292      c-basic-offset: 2
00293      End:
00294   */
00295 
00296 #endif /* _DIAGNOSTIC_COMMON_HPP_ */


sr_self_test
Author(s): Ugo Cupcic
autogenerated on Fri Aug 21 2015 12:25:57