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
00036
00037
00038
00039
00040 #include <stdlib.h>
00041 #include <algorithm>
00042 #include <ros/ros.h>
00043 #include <boost/foreach.hpp>
00044 #include <libsensors_monitor/libsensors_chip.h>
00045 #include <sstream>
00046
00047 #define NAME_BUFFER_SIZE 250
00048
00049 SensorChip::SensorChip(sensors_chip_name const *chip_name,
00050 const std::vector<std::string> &ignore) :
00051 internal_name_(chip_name)
00052 {
00053 char name_buffer[NAME_BUFFER_SIZE];
00054 sensors_snprintf_chip_name(name_buffer, NAME_BUFFER_SIZE, internal_name_);
00055 name_ = name_buffer;
00056
00057 ROS_DEBUG("Found Sensor: %s", getName().c_str());
00058
00059
00060 sensors_feature const *feature;
00061 int number = 0;
00062
00063 while ((feature = sensors_get_features(internal_name_, &number)) != NULL) {
00064 sensors_feature_type type = feature->type;
00065 SensorChipFeaturePtr feature_ptr;
00066 switch(type){
00067 case SENSORS_FEATURE_FAN:
00068 feature_ptr.reset(new FanSensor(*this, feature));
00069 break;
00070 case SENSORS_FEATURE_TEMP:
00071 feature_ptr.reset(new TempSensor(*this, feature));
00072 break;
00073 case SENSORS_FEATURE_IN:
00074 feature_ptr.reset(new VoltageSensor(*this, feature));
00075 break;
00076 default:
00077 feature_ptr.reset(new OtherSensor(*this, feature));
00078 break;
00079 }
00080
00081 if( std::count(ignore.begin(), ignore.end(),
00082 feature_ptr->getFullName()) > 0 ) {
00083 ROS_INFO_STREAM("Ignoring sensor " << feature_ptr->getFullName());
00084 } else {
00085 features_.push_back(feature_ptr);
00086 }
00087 }
00088
00089 std::stringstream info_msg;
00090 info_msg << "Found sensor " << getName();
00091 if( features_.size() > 0 ) {
00092 info_msg << " with features: ";
00093 size_t remain = features_.size();
00094 BOOST_FOREACH(const SensorChipFeaturePtr & feature, features_) {
00095 remain--;
00096 info_msg << feature->getFeatureName();
00097 if( remain > 0 ) info_msg << ", ";
00098 }
00099 } else {
00100 info_msg << " with no features?";
00101 }
00102 ROS_INFO_STREAM(info_msg.str());
00103 }
00104
00105
00106
00107 SensorChipFeature::SensorChipFeature(const SensorChip& chip,
00108 sensors_feature const *feature) :
00109 chip_(chip),
00110 feature_(feature)
00111 {
00112 name_ = feature_->name;
00113 char* label_c_str = sensors_get_label(chip_.internal_name_, feature_);
00114 if( label_c_str == NULL ) {
00115 ROS_WARN("Could not get label for %s", name_.c_str());
00116 label_ = "(None)";
00117 } else {
00118 label_ = label_c_str;
00119 free(label_c_str);
00120 }
00121 full_name_ = getChipName()+"/"+getFeatureName();
00122 full_label_ = getChipName()+"/"+getFeatureLabel();
00123
00124
00125 ROS_DEBUG("\tFound Feature: %s(%s)[%d]", getFullLabel().c_str(),
00126 getFullName().c_str(), feature_->type);
00127
00128 enumerate_subfeatures();
00129 }
00130
00131
00132 void SensorChipFeature::enumerate_subfeatures(){
00133 sensors_subfeature const *subfeature;
00134 int number = 0;
00135
00136 while ((subfeature = sensors_get_all_subfeatures(chip_.internal_name_, feature_, &number)) != NULL) {
00137 SensorChipSubFeaturePtr subfeature_ptr(new SensorChipSubFeature(*this,
00138 subfeature));
00139 sub_features_.push_back(subfeature_ptr);
00140 }
00141 }
00142
00143
00144 SensorChipSubFeaturePtr SensorChipFeature::getSubFeatureByType(sensors_subfeature_type type){
00145 BOOST_FOREACH(SensorChipSubFeaturePtr subfeature, sub_features_){
00146 if(subfeature->getType()==type)
00147 return subfeature;
00148 }
00149 return SensorChipSubFeaturePtr();
00150 }
00151
00152
00153 SensorChipSubFeature::SensorChipSubFeature(const SensorChipFeature& feature,
00154 sensors_subfeature const *subfeature) :
00155 feature_(feature),
00156 subfeature_(subfeature),
00157 name_(subfeature->name)
00158 {
00159 ROS_DEBUG("\t\tFound Sub-Feature: %s[%d] = %f", getName().c_str(),
00160 subfeature_->type, getValue());
00161 }
00162
00163
00164 double SensorChipSubFeature::getValue(){
00165 double value;
00166 if( sensors_get_value(feature_.chip_.internal_name_, subfeature_->number,
00167 &value) != 0 ) {
00168 ROS_WARN_STREAM("Failed to get value for " << feature_.getFullName() <<
00169 " " << getName());
00170 }
00171 return value;
00172 }
00173
00174 void FanSensor::buildStatus(diagnostic_updater::DiagnosticStatusWrapper &stat){
00175 SensorChipSubFeaturePtr speed = getSubFeatureByType(SENSORS_SUBFEATURE_FAN_INPUT);
00176 if(speed){
00177 double speed_val = speed->getValue();
00178 if(speed_val==0)
00179 stat.summary(diagnostic_msgs::DiagnosticStatus::WARN,
00180 "Fan stalled or not connected");
00181 else
00182 stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "Fan OK (%.2f RPM)",
00183 speed_val);
00184 stat.add("Fan Speed (RPM)", speed_val);
00185 }
00186 else
00187 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No Fan Input!!!");
00188 }
00189
00190
00191 void TempSensor::buildStatus(diagnostic_updater::DiagnosticStatusWrapper &stat){
00192 SensorChipSubFeaturePtr temp = getSubFeatureByType(SENSORS_SUBFEATURE_TEMP_INPUT);
00193 SensorChipSubFeaturePtr max_temp = getSubFeatureByType(SENSORS_SUBFEATURE_TEMP_MAX);
00194 SensorChipSubFeaturePtr temp_crit = getSubFeatureByType(SENSORS_SUBFEATURE_TEMP_CRIT);
00195 SensorChipSubFeaturePtr temp_crit_alarm = getSubFeatureByType(SENSORS_SUBFEATURE_TEMP_CRIT_ALARM);
00196 if(temp){
00197 double temp_val = temp->getValue();
00198 stat.add("Temperature (C)", temp_val);
00199
00200 if(max_temp && max_temp->getValue()!=0)
00201 stat.add("Max Temperature (C)", max_temp->getValue());
00202 if(temp_crit && temp_crit->getValue()!=0)
00203 stat.add("Temperature Critical (C)", temp_crit->getValue());
00204
00205 if(temp_crit_alarm && temp_crit_alarm->getValue()!=0)
00206 stat.summaryf(diagnostic_msgs::DiagnosticStatus::WARN,
00207 "Critical Temp Alarm (%.2f C)", temp_val);
00208 else if(temp_crit && temp_crit->getValue()!=0 &&
00209 temp_val > temp_crit->getValue())
00210 stat.summaryf(diagnostic_msgs::DiagnosticStatus::WARN,
00211 "Critical Temp Alarm (%.2f C)", temp_val);
00212 else if(max_temp && max_temp->getValue()!=0 &&
00213 temp_val > max_temp->getValue())
00214 stat.summaryf(diagnostic_msgs::DiagnosticStatus::WARN,
00215 "Max Temp Alarm (%.2f C)", temp_val);
00216 else
00217 stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK,
00218 "Temp OK (%.2f C)", temp_val);
00219 }
00220 else
00221 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "NO TEMP Input!!!");
00222 }
00223
00224 void VoltageSensor::buildStatus(diagnostic_updater::DiagnosticStatusWrapper &stat){
00225 SensorChipSubFeaturePtr voltage = getSubFeatureByType(
00226 SENSORS_SUBFEATURE_IN_INPUT);
00227 SensorChipSubFeaturePtr min = getSubFeatureByType(
00228 SENSORS_SUBFEATURE_IN_MIN);
00229 SensorChipSubFeaturePtr max = getSubFeatureByType(
00230 SENSORS_SUBFEATURE_IN_MAX);
00231 SensorChipSubFeaturePtr lcrit = getSubFeatureByType(
00232 SENSORS_SUBFEATURE_IN_LCRIT);
00233 SensorChipSubFeaturePtr crit = getSubFeatureByType(
00234 SENSORS_SUBFEATURE_IN_CRIT);
00235
00236
00237 SensorChipSubFeaturePtr alarm = getSubFeatureByType(
00238 SENSORS_SUBFEATURE_IN_ALARM);
00239 SensorChipSubFeaturePtr min_alarm = getSubFeatureByType(
00240 SENSORS_SUBFEATURE_IN_MIN_ALARM);
00241 SensorChipSubFeaturePtr max_alarm = getSubFeatureByType(
00242 SENSORS_SUBFEATURE_IN_MAX_ALARM);
00243 SensorChipSubFeaturePtr lcrit_alarm = getSubFeatureByType(
00244 SENSORS_SUBFEATURE_IN_LCRIT_ALARM);
00245 SensorChipSubFeaturePtr crit_alarm = getSubFeatureByType(
00246 SENSORS_SUBFEATURE_IN_CRIT_ALARM);
00247
00248 if(voltage){
00249 double voltage_val = voltage->getValue();
00250 stat.add("Voltage (V)", voltage_val);
00251
00252 bool low_warn = false;
00253 bool low_err = false;
00254 bool high_warn = false;
00255 bool high_err = false;
00256
00257
00258 if(max) {
00259 stat.add("Max Voltage (V)", max->getValue());
00260 if(max_alarm && max_alarm->getValue()!=0) {
00261 high_warn = true;
00262 } else if(voltage_val >= max->getValue()) {
00263 high_warn = true;
00264 }
00265 }
00266
00267
00268 if(min) {
00269 stat.add("Min Voltage (V)", min->getValue());
00270 if(min_alarm && min_alarm->getValue()!=0) {
00271 low_warn = true;
00272 } else if(voltage_val <= min->getValue()) {
00273 low_warn = true;
00274 }
00275 }
00276
00277 if(crit) {
00278 stat.add("Critical Max Voltage (V)", crit->getValue());
00279 if(crit_alarm && crit_alarm->getValue()!=0) {
00280 high_err = true;
00281 } else if(voltage_val >= crit->getValue()) {
00282 high_err = true;
00283 }
00284 }
00285
00286 if(lcrit) {
00287 stat.add("Critical Min Voltage (V)", lcrit->getValue());
00288 if(lcrit_alarm && lcrit_alarm->getValue()!=0) {
00289 low_err = true;
00290 } else if(voltage_val <= lcrit->getValue()) {
00291 low_err = true;
00292 }
00293 }
00294
00295
00296 if(high_err) {
00297 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR,
00298 "High Voltage CRITICAL (%.2f V)", voltage_val);
00299 } else if(low_err) {
00300 stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR,
00301 "Low Voltage CRITICAL (%.2f V)", voltage_val);
00302 } else if(high_warn) {
00303 stat.summaryf(diagnostic_msgs::DiagnosticStatus::WARN,
00304 "High Voltage ALARM (%.2f V)", voltage_val);
00305 } else if(low_warn) {
00306 stat.summaryf(diagnostic_msgs::DiagnosticStatus::WARN,
00307 "Low Voltage ALARM (%.2f V)", voltage_val);
00308 } else {
00309 stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK,
00310 "Voltage OK (%.2f V)", voltage_val);
00311 }
00312 } else {
00313 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
00314 "NO Voltage Input!!!");
00315 }
00316 }
00317
00318 void OtherSensor::buildStatus(diagnostic_updater::DiagnosticStatusWrapper &stat){
00319 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Unkown sensor type");
00320 BOOST_FOREACH(SensorChipSubFeaturePtr subfeature, sub_features_){
00321 stat.add(subfeature->getName(), subfeature->getValue());
00322 }
00323 }