diagnostics.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010  ** Includes
00011  *****************************************************************************/
00012 
00013 #include "../../include/kobuki_node/diagnostics.hpp"
00014 
00015 /*****************************************************************************
00016 ** Namespaces
00017 *****************************************************************************/
00018 
00019 namespace kobuki {
00020 
00021 /*****************************************************************************
00022 ** Implementation
00023 *****************************************************************************/
00024 
00025 void BatteryTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00026   switch ( status.level() ) {
00027     case ( Battery::Maximum ) : {
00028       stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Maximum");
00029       break;
00030     }
00031     case ( Battery::Healthy ) : {
00032       stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Healthy");
00033       break;
00034     }
00035     case ( Battery::Low ) : {
00036       stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Low");
00037       break;
00038     }
00039     case ( Battery::Dangerous ) : {
00040       stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Dangerous");
00041       break;
00042     }
00043   }
00044 
00045   stat.add("Voltage (V)", status.voltage);
00046   stat.add("Percent", status.percent());
00047   stat.add("Charge (Ah)", (2.2*status.percent())/100.0);
00048   stat.add("Capacity (Ah)", 2.2); // TODO: how can we tell which battery is in use?
00049 
00050   switch (status.charging_source ) {
00051     case(Battery::None) : {
00052       stat.add("Source", "None");
00053       break;
00054     }
00055     case(Battery::Adapter) : {
00056       stat.add("Source", "Adapter");
00057       break;
00058     }
00059     case(Battery::Dock) : {
00060       stat.add("Source", "Dock");
00061       break;
00062     }
00063   }
00064   switch ( status.charging_state ) {
00065     case ( Battery::Charged ) : {
00066       stat.add("Charging State", "Trickle Charging"); // i.e. fully charged
00067       stat.add("Current (A)", 3.14); // TODO: what's the real value for our charger?
00068       break;
00069     }
00070     case ( Battery::Charging ) : {
00071       stat.add("Charging State", "Full Charging");
00072       stat.add("Current (A)", 3.14); // TODO: what's the real value for our charger?
00073       break;
00074     }
00075     case ( Battery::Discharging ) : {
00076       stat.add("Charging State", "Not Charging");
00077       stat.add("Current (A)", 0.0);
00078       break;
00079     }
00080     default: break;
00081   }
00082 }
00083 
00084 void WatchdogTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00085   if ( alive ) {
00086     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Alive");
00087   } else {
00088     stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No Signal");
00089   }
00090 }
00091 
00092 void CliffSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00093   if ( status ) {
00094     stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Cliff Detected!");
00095   } else {
00096     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
00097   }
00098 
00099   stat.addf("Left",   "Reading: %d  Cliff: %s", values.bottom[0], status & CoreSensors::Flags::LeftCliff?"YES":"NO");
00100   stat.addf("Center", "Reading: %d  Cliff: %s", values.bottom[1], status & CoreSensors::Flags::CenterCliff?"YES":"NO");
00101   stat.addf("Right",  "Reading: %d  Cliff: %s", values.bottom[2], status & CoreSensors::Flags::RightCliff?"YES":"NO");
00102 }
00103 
00104 void WallSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00105   if ( status ) {
00106     stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wall Hit!");
00107   } else {
00108     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
00109   }
00110 
00111   stat.addf("Left",   status & CoreSensors::Flags::LeftBumper?"YES":"NO");
00112   stat.addf("Center", status & CoreSensors::Flags::CenterBumper?"YES":"NO");
00113   stat.addf("Right",  status & CoreSensors::Flags::RightBumper?"YES":"NO");
00114 }
00115 
00116 void WheelDropTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00117   if ( status ) {
00118     stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wheel Drop!");
00119   } else {
00120     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
00121   }
00122 
00123   stat.addf("Left",   status & CoreSensors::Flags::LeftWheel?"YES":"NO");
00124   stat.addf("Right",  status & CoreSensors::Flags::RightWheel?"YES":"NO");
00125 }
00126 
00127 void MotorCurrentTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00128   if ( std::max(values[0], values[1]) > 6 ) { // TODO not sure about this threshold; should be a parameter?
00129     stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Is robot stalled? Motors current is very high");
00130   } else {
00131     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
00132   }
00133 
00134   stat.addf("Left",  "%d", values[0]);
00135   stat.addf("Right", "%d", values[1]);
00136 }
00137 
00138 void MotorStateTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00139   if ( state == true ) {
00140     stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Motors Enabled");
00141   } else {
00142     stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Motors Disabled");
00143   }
00144 
00145   stat.addf("State", "%d", int(state));
00146 }
00147 
00148 void GyroSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00149   // Raw data angles are in hundredths of degree
00150   stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "Heading: %.2f degrees", heading/100.0);
00151 }
00152 
00153 void DigitalInputTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00154   stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "[%d, %d, %d, %d]",
00155                 status & 0x08?1:0, status & 0x04?1:0,
00156                 status & 0x02?1:0, status & 0x01?1:0);
00157 }
00158 
00159 void AnalogInputTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00160   stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "[%d, %d, %d, %d]",
00161                 values[0], values[1], values[2], values[3]);
00162 }
00163 
00164 } // namespace kobuki


kobuki_node
Author(s): Daniel Stonier, Younghun Ju, Jorge Santos Simon
autogenerated on Thu Jun 6 2019 17:37:58