00001
00032 #include "roch_base/roch_diagnostics.h"
00033
00034
00035 namespace
00036 {
00037 const int UNDERVOLT_ERROR = 18;
00038 const int UNDERVOLT_WARN = 19;
00039 const int OVERVOLT_ERROR = 30;
00040 const int OVERVOLT_WARN = 29;
00041 const int DRIVER_OVERTEMP_ERROR = 50;
00042 const int DRIVER_OVERTEMP_WARN = 30;
00043 const int MOTOR_OVERTEMP_ERROR = 80;
00044 const int MOTOR_OVERTEMP_WARN = 70;
00045 const double LOWPOWER_ERROR = 0.2;
00046 const double LOWPOWER_WARN = 0.3;
00047 const int CONTROLFREQ_WARN = 90;
00048 const unsigned int SAFETY_TIMEOUT = 0x1;
00049 const unsigned int SAFETY_LOCKOUT = 0x2;
00050 const unsigned int SAFETY_ESTOP = 0x8;
00051 const unsigned int SAFETY_CCI = 0x10;
00052 const unsigned int SAFETY_PSU = 0x20;
00053 const unsigned int SAFETY_CURRENT = 0x40;
00054 const unsigned int SAFETY_WARN = (SAFETY_TIMEOUT | SAFETY_CCI | SAFETY_PSU);
00055 const unsigned int SAFETY_ERROR = (SAFETY_LOCKOUT | SAFETY_ESTOP | SAFETY_CURRENT);
00056 }
00057
00058 namespace roch_base
00059 {
00060
00061 template<>
00062 rochHardwareDiagnosticTask<sawyer::DataSystemStatus>::rochHardwareDiagnosticTask(roch_msgs::RochStatus &msg)
00063 :
00064 DiagnosticTask("system_status"),
00065 msg_(msg)
00066 {
00067 }
00068
00069 template<>
00070 void rochHardwareDiagnosticTask<sawyer::DataSystemStatus>::update(
00071 diagnostic_updater::DiagnosticStatusWrapper &stat,
00072 core::Channel<sawyer::DataSystemStatus>::Ptr &system_status)
00073 {
00074 msg_.uptime = system_status->getUptime();
00075
00076 msg_.battery_voltage = system_status->getVoltage(0);
00077 msg_.left_driver_voltage = system_status->getVoltage(1);
00078 msg_.right_driver_voltage = system_status->getVoltage(2);
00079
00080 msg_.mcu_and_user_port_current = system_status->getCurrent(0);
00081 msg_.left_driver_current = system_status->getCurrent(1);
00082 msg_.right_driver_current = system_status->getCurrent(2);
00083
00084 msg_.left_driver_temp = system_status->getTemperature(0);
00085 msg_.right_driver_temp = system_status->getTemperature(1);
00086 msg_.left_motor_temp = system_status->getTemperature(2);
00087 msg_.right_motor_temp = system_status->getTemperature(3);
00088 stat.add("Uptime", msg_.uptime);
00089
00090 stat.add("Battery Voltage", msg_.battery_voltage);
00091 stat.add("Left Motor Driver Voltage", msg_.left_driver_voltage);
00092 stat.add("Right Motor Driver Voltage", msg_.right_driver_voltage);
00093
00094 stat.add("MCU and User Port Current", msg_.mcu_and_user_port_current);
00095 stat.add("Left Motor Driver Current", msg_.left_driver_current);
00096 stat.add("Right Motor Driver Current", msg_.right_driver_current);
00097
00098 stat.add("Left Motor Driver Temp (C)", msg_.left_driver_temp);
00099 stat.add("Right Motor Driver Temp (C)", msg_.right_driver_temp);
00100 stat.add("Left Motor Temp (C)", msg_.left_motor_temp);
00101 stat.add("Right Motor Temp (C)", msg_.right_motor_temp);
00102 ROS_DEBUG_STREAM("Received Uptime:"<<msg_.uptime<<", Battery Voltage:"<<msg_.battery_voltage<<", Left Motor Driver Voltage:"<<msg_.left_driver_voltage<<", Right Motor Driver Voltage:"<<msg_.right_driver_voltage<<", "
00103 "MCU and User Port Current:"<<msg_.mcu_and_user_port_current<<", Left Motor Driver Current:"<<msg_.left_driver_current<<",Right Motor Driver Current:"<<msg_.right_driver_current<<", "
00104 "Left Motor Driver Temp (C):"<<msg_.left_driver_temp<<", Right Motor Driver Temp (C):"<<msg_.right_driver_temp<<", Left Motor Temp (C):"<<msg_.left_motor_temp<<", Right Motor Temp (C):"<<msg_.right_motor_temp<<" .");
00105
00106 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "System Status OK");
00107 if (msg_.battery_voltage > OVERVOLT_ERROR)
00108 {
00109 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Main battery voltage too high");
00110 }
00111 else if (msg_.battery_voltage > OVERVOLT_WARN)
00112 {
00113 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Main battery voltage too high");
00114 }
00115 else if (msg_.battery_voltage < UNDERVOLT_ERROR)
00116 {
00117 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Main battery voltage too low");
00118 }
00119 else if (msg_.battery_voltage < UNDERVOLT_WARN)
00120 {
00121 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Main battery voltage too low");
00122 }
00123 else
00124 {
00125 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::OK, "Voltage OK");
00126 }
00127
00128 if (std::max(msg_.left_driver_temp, msg_.right_driver_temp) > DRIVER_OVERTEMP_ERROR)
00129 {
00130 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Motor drivers too hot");
00131 }
00132 else if (std::max(msg_.left_driver_temp, msg_.right_driver_temp) > DRIVER_OVERTEMP_WARN)
00133 {
00134 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Motor drivers too hot");
00135 }
00136 else if (std::max(msg_.left_motor_temp, msg_.right_motor_temp) > MOTOR_OVERTEMP_ERROR)
00137 {
00138 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Motors too hot");
00139 }
00140 else if (std::max(msg_.left_motor_temp, msg_.right_motor_temp) > MOTOR_OVERTEMP_WARN)
00141 {
00142 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Motors too hot");
00143 }
00144 else
00145 {
00146 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::OK, "Temperature OK");
00147 }
00148 }
00149
00150 template<>
00151 rochHardwareDiagnosticTask<sawyer::DataPowerSystem>::rochHardwareDiagnosticTask(roch_msgs::RochStatus &msg)
00152 :
00153 DiagnosticTask("power_status"),
00154 msg_(msg)
00155 { }
00156
00157 template<>
00158 void rochHardwareDiagnosticTask<sawyer::DataPowerSystem>::update(
00159 diagnostic_updater::DiagnosticStatusWrapper &stat,
00160 core::Channel<sawyer::DataPowerSystem>::Ptr &power_status)
00161 {
00162 msg_.charge_estimate = power_status->getChargeEstimate(0);
00163 msg_.capacity_estimate = power_status->getCapacityEstimate(0);
00164
00165 stat.add("Charge (%)", msg_.charge_estimate);
00166 stat.add("Battery Capacity (Wh)", msg_.capacity_estimate);
00167 ROS_DEBUG_STREAM("Received Charge (%%): "<<msg_.charge_estimate<<", Battery Capacity (Wh): "<<msg_.capacity_estimate<<" .");
00168
00169 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Power System OK");
00170 if (msg_.charge_estimate < LOWPOWER_ERROR)
00171 {
00172 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Low power");
00173 }
00174 else if (msg_.charge_estimate < LOWPOWER_WARN)
00175 {
00176 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Low power");
00177 }
00178 else
00179 {
00180 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::OK, "Charge OK");
00181 }
00182 }
00183
00184 template<>
00185 rochHardwareDiagnosticTask<sawyer::DataSafetySystemStatus>::rochHardwareDiagnosticTask(
00186 roch_msgs::RochStatus &msg)
00187 :
00188 DiagnosticTask("safety_status"),
00189 msg_(msg)
00190 { }
00191
00192 template<>
00193 void rochHardwareDiagnosticTask<sawyer::DataSafetySystemStatus>::update(
00194 diagnostic_updater::DiagnosticStatusWrapper &stat,
00195 core::Channel<sawyer::DataSafetySystemStatus>::Ptr &safety_status)
00196 {
00197 uint16_t flags = safety_status->getFlags();
00198 msg_.timeout = (flags & SAFETY_TIMEOUT) > 0;
00199 msg_.lockout = (flags & SAFETY_LOCKOUT) > 0;
00200 msg_.e_stop = (flags & SAFETY_ESTOP) > 0;
00201 msg_.ros_pause = (flags & SAFETY_CCI) > 0;
00202 msg_.no_battery = (flags & SAFETY_PSU) > 0;
00203 msg_.current_limit = (flags & SAFETY_CURRENT) > 0;
00204
00205 stat.add("Timeout", static_cast<bool>(msg_.timeout));
00206 stat.add("Lockout", static_cast<bool>(msg_.lockout));
00207 stat.add("Emergency Stop", static_cast<bool>(msg_.e_stop));
00208 stat.add("ROS Pause", static_cast<bool>(msg_.ros_pause));
00209 stat.add("No battery", static_cast<bool>(msg_.no_battery));
00210 stat.add("Current limit", static_cast<bool>(msg_.current_limit));
00211 ROS_DEBUG_STREAM("Received Safety System Status, Timeout:"<<msg_.timeout<<", Lockout:"<<msg_.lockout<<", Emergency Stop:"<<msg_.e_stop<<", ROS Pause:"<<msg_.ros_pause<<", No battery:"<<msg_.no_battery<<", Current limit:"<<msg_.current_limit<<" .");
00212
00213 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Safety System OK");
00214 if ((flags & SAFETY_ERROR) > 0)
00215 {
00216 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Safety System Error");
00217 }
00218 else if ((flags & SAFETY_WARN) > 0)
00219 {
00220 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Safety System Warning");
00221 }
00222 }
00223
00224 rochSoftwareDiagnosticTask::rochSoftwareDiagnosticTask(roch_msgs::RochStatus &msg, double target_control_freq)
00225 :
00226 DiagnosticTask("software_status"),
00227 msg_(msg),
00228 target_control_freq_(target_control_freq)
00229 {
00230 reset();
00231 }
00232
00233 void rochSoftwareDiagnosticTask::updateControlFrequency(double frequency)
00234 {
00235
00236 control_freq_ = std::min(control_freq_, frequency);
00237 }
00238
00239 void rochSoftwareDiagnosticTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
00240 {
00241 msg_.ros_control_loop_freq = control_freq_;
00242 stat.add("ROS Control Loop Frequency", msg_.ros_control_loop_freq);
00243
00244 double margin = control_freq_ / target_control_freq_ * 100;
00245
00246 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Software OK");
00247 if (margin < CONTROLFREQ_WARN)
00248 {
00249 std::ostringstream message;
00250 message << "Control loop executing " << 100 - static_cast<int>(margin) << "% slower than desired";
00251 stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, message.str());
00252 }
00253
00254 reset();
00255 }
00256
00257 void rochSoftwareDiagnosticTask::reset()
00258 {
00259 control_freq_ = std::numeric_limits<double>::infinity();
00260 target_control_freq_ = 0;
00261 }
00262 }