void HerculesHardwareDiagnosticTask<clearpath::DataSafetySystemStatus>::update(
diagnostic_updater::DiagnosticStatusWrapper &stat, horizon_legacy::Channel<clearpath::DataSafetySystemStatus>::Ptr &safety_status)
{
uint16_t flags = safety_status->getFlags();
msg_.timeout = (flags & SAFETY_TIMEOUT) > 0;
msg_.lockout = (flags & SAFETY_LOCKOUT) > 0;
msg_.e_stop = (flags & SAFETY_ESTOP) > 0;
msg_.ros_pause = (flags & SAFETY_CCI) > 0;
msg_.no_battery = (flags & SAFETY_PSU) > 0;
msg_.current_limit = (flags & SAFETY_CURRENT) > 0;
stat.add("Timeout", static_cast<bool>(msg_.timeout));
stat.add("Lockout", static_cast<bool>(msg_.lockout));
stat.add("Emergency Stop", static_cast<bool>(msg_.e_stop));
stat.add("ROS Pause", static_cast<bool>(msg_.ros_pause));
stat.add("No battery", static_cast<bool>(msg_.no_battery));
stat.add("Current limit", static_cast<bool>(msg_.current_limit));
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Safety System OK");
if ((flags & SAFETY_ERROR) > 0)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Safety System Error");
}
else if ((flags & SAFETY_WARN) > 0)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Safety System Warning");
}
}
void EthernetInterfaceInfo::publishDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
{
d.add("Interface", interface_);
// TODO : collect and publish information on whether interface is up/running
InterfaceState state;
if (getInterfaceState(state))
{
if (!state.running_ && last_state_.running_)
{
++lost_link_count_;
}
if (state.up_ && !state.running_)
{
d.mergeSummary(d.ERROR, "No link");
}
else if (!state.up_)
{
d.mergeSummary(d.ERROR, "Interface down");
}
d.addf("Interface State", "%s UP, %s RUNNING", state.up_?"":"NOT", state.running_?"":"NOT");
last_state_ = state;
}
else
{
d.add("Iface State", "ERROR");
}
d.add("Lost Links", lost_link_count_);
EthtoolStats stats;
bool have_stats = getEthtoolStats(stats);
stats-=orig_stats_; //subtract off orignal counter values
if (have_stats && (rx_error_index_>=0))
d.addf("RX Errors", "%llu", stats.rx_errors_);
else
d.add( "RX Errors", "N/A");
if (have_stats && (rx_crc_error_index_>=0))
d.addf("RX CRC Errors", "%llu", stats.rx_crc_errors_);
else
d.add( "RX CRC Errors", "N/A");
if (have_stats && (rx_frame_error_index_>=0))
d.addf("RX Frame Errors", "%llu", stats.rx_frame_errors_);
else
d.add( "RX Frame Errors", "N/A");
if (have_stats && (rx_align_error_index_>=0))
d.addf("RX Align Errors", "%llu", stats.rx_align_errors_);
else
d.add( "RX Align Errors", "N/A");
}
void manualDiagnostics(du::DiagnosticStatusWrapper& status) // NOLINT
{
status.summary(DS::OK, "No errors reported.");
// The swri::Subscriber provides methods to access all of the
// statistics and status information. These are suitable for
// making control decisions or report diagnostics. In this
// example, we access the data directly to update a diagnostic
// status on our own. This is useful if you want to apply custom
// summary semantics or change how the key/values are included.
if (sub_.messageCount() == 0) {
status.mergeSummary(DS::WARN, "No messages received.");
} else if (sub_.inTimeout()) {
status.mergeSummary(DS::ERROR, "Topic has timed out.");
} else if (sub_.timeoutCount() > 0) {
status.mergeSummary(DS::WARN, "Timeouts have occurred.");
}
if (sub_.mappedTopic() == sub_.unmappedTopic()) {
status.addf("Topic Name", "%s", sub_.mappedTopic().c_str());
} else {
status.addf("Topic Name", "%s -> %s",
sub_.unmappedTopic().c_str(),
sub_.mappedTopic().c_str());
}
status.addf("Number of publishers", "%d", sub_.numPublishers());
status.addf("Mean Latency [us]", "%f", sub_.meanLatencyMicroseconds());
status.addf("Min Latency [us]", "%f", sub_.minLatencyMicroseconds());
status.addf("Max Latency [us]", "%f", sub_.maxLatencyMicroseconds());
status.addf("Mean Frequency [Hz]", "%f", sub_.meanFrequencyHz());
status.addf("Mean Period [ms]", "%f", sub_.meanPeriodMilliseconds());
status.addf("Min Period [ms]", "%f", sub_.minPeriodMilliseconds());
status.addf("Max Period [ms]", "%f", sub_.maxPeriodMilliseconds());
if (sub_.timeoutEnabled()) {
status.addf("Timed Out Count", "%d [> %f ms]",
sub_.timeoutCount(),
sub_.timeoutMilliseconds());
} else {
status.add("Timed Out Count", "N/A");
}
}
void WG06::diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
{
stringstream str;
str << "Accelerometer (" << actuator_info_.name_ << ")";
d.name = str.str();
char serial[32];
snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
d.hardware_id = serial;
d.summary(d.OK, "OK");
d.clear();
pr2_hardware_interface::AccelerometerCommand acmd(accelerometer_.command_);
const char * range_str =
(acmd.range_ == 0) ? "+/-2G" :
(acmd.range_ == 1) ? "+/-4G" :
(acmd.range_ == 2) ? "+/-8G" :
"INVALID";
const char * bandwidth_str =
(acmd.bandwidth_ == 6) ? "1500Hz" :
(acmd.bandwidth_ == 5) ? "750Hz" :
(acmd.bandwidth_ == 4) ? "375Hz" :
(acmd.bandwidth_ == 3) ? "190Hz" :
(acmd.bandwidth_ == 2) ? "100Hz" :
(acmd.bandwidth_ == 1) ? "50Hz" :
(acmd.bandwidth_ == 0) ? "25Hz" :
"INVALID";
// Board revB=1 and revA=0 does not have accelerometer
bool has_accelerometer = (board_major_ >= 2);
double sample_frequency = 0.0;
ros::Time current_time(ros::Time::now());
if (!first_publish_)
{
sample_frequency = double(accelerometer_samples_) / (current_time - last_publish_time_).toSec();
{
if (((sample_frequency < 2000) || (sample_frequency > 4000)) && has_accelerometer)
{
d.mergeSummary(d.WARN, "Bad accelerometer sampling frequency");
}
}
}
accelerometer_samples_ = 0;
d.addf("Accelerometer", "%s", accelerometer_.state_.samples_.size() > 0 ? "Ok" : "Not Present");
d.addf("Accelerometer range", "%s (%d)", range_str, acmd.range_);
d.addf("Accelerometer bandwidth", "%s (%d)", bandwidth_str, acmd.bandwidth_);
d.addf("Accelerometer sample frequency", "%f", sample_frequency);
d.addf("Accelerometer missed samples", "%d", accelerometer_missed_samples_);
}
void HerculesHardwareDiagnosticTask<clearpath::DataPowerSystem>::update(diagnostic_updater::DiagnosticStatusWrapper &stat,
horizon_legacy::Channel<clearpath::DataPowerSystem>::Ptr &power_status)
{
msg_.charge_estimate = power_status->getChargeEstimate(0);
msg_.capacity_estimate = power_status->getCapacityEstimate(0);
stat.add("Charge (%)", msg_.charge_estimate);
stat.add("Battery Capacity (Wh)", msg_.capacity_estimate);
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Power System OK");
if (msg_.charge_estimate < LOWPOWER_ERROR)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Low power");
}
else if (msg_.charge_estimate < LOWPOWER_WARN)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Low power");
}
else
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::OK, "Charge OK");
}
}
/** \brief Collects and publishes device diagnostics
*/
void MotorModel::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
{
// Should perform locking.. publishing of diagnostics is done from separate thread
double motor_voltage_error;
double motor_voltage_error_max;
double abs_motor_voltage_error;
double abs_motor_voltage_error_max;
double current_error;
double current_error_max;
double abs_current_error;
double abs_current_error_max;
double est_motor_resistance;
std::string reason;
int level;
diagnostics_mutex_.lock();
{
motor_voltage_error = motor_voltage_error_.filter();
motor_voltage_error_max = motor_voltage_error_.filter_max();
abs_motor_voltage_error = abs_motor_voltage_error_.filter();
abs_motor_voltage_error_max = abs_motor_voltage_error_.filter_max();
current_error = current_error_.filter();
current_error_max = current_error_.filter_max();
abs_current_error = abs_current_error_.filter();
abs_current_error_max = abs_current_error_.filter_max();
est_motor_resistance = motor_resistance_.filter();
reason = diagnostics_reason_;
level = diagnostics_level_;
}
diagnostics_mutex_.unlock();
if (level > 0)
d.mergeSummary(level, reason);
d.addf("Motor Voltage Error %", "%f", 100.0 * motor_voltage_error);
d.addf("Max Motor Voltage Error %", "%f", 100.0 * motor_voltage_error_max);
d.addf("Abs Filtered Voltage Error %", "%f", 100.0 * abs_motor_voltage_error);
d.addf("Max Abs Filtered Voltage Error %", "%f", 100.0 * abs_motor_voltage_error_max);
// TODO change names
d.addf("Current Error", "%f", current_error);
d.addf("Max Current Error", "%f", current_error_max);
d.addf("Abs Filtered Current Error", "%f", abs_current_error);
d.addf("Max Abs Filtered Current Error", "%f", abs_current_error_max);
d.addf("Motor Resistance Estimate", "%f", est_motor_resistance);
d.addf("# Published traces", "%d", published_traces_);
}
void HerculesSoftwareDiagnosticTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
msg_.ros_control_loop_freq = control_freq_;
stat.add("ROS Control Loop Frequency", msg_.ros_control_loop_freq);
double margin = control_freq_ / target_control_freq_ * 100;
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Software OK");
if (margin < CONTROLFREQ_WARN)
{
std::ostringstream message;
message << "Control loop executing " << 100 - static_cast<int>(margin) << "% slower than desired";
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, message.str());
}
reset();
}
void packetStatistics(diagnostic_updater::DiagnosticStatusWrapper& status)
{
// Get stats from camera driver
unsigned long received, missed, requested, resent;
cam_->getAttribute("StatPacketsReceived", received);
cam_->getAttribute("StatPacketsMissed", missed);
cam_->getAttribute("StatPacketsRequested", requested);
cam_->getAttribute("StatPacketsResent", resent);
// Compute rolling totals, percentages
packets_received_acc_.add(received - packets_received_total_);
packets_received_total_ = received;
unsigned long received_recent = packets_received_acc_.sum();
packets_missed_acc_.add(missed - packets_missed_total_);
packets_missed_total_ = missed;
unsigned long missed_recent = packets_missed_acc_.sum();
float recent_ratio = float(received_recent) / (received_recent + missed_recent);
float total_ratio = float(received) / (received + missed);
if (missed_recent == 0) {
status.summary(0, "No missed packets");
}
else if (recent_ratio > 0.99f) {
status.summary(1, "Some missed packets");
}
else {
status.summary(2, "Excessive proportion of missed packets");
}
// Adjust data rate
unsigned long data_rate = 0;
cam_->getAttribute("StreamBytesPerSecond", data_rate);
unsigned long max_data_rate = cam_->getMaxDataRate();
if (auto_adjust_stream_bytes_per_second_)
{
if (max_data_rate < prosilica::Camera::GIGE_MAX_DATA_RATE)
status.mergeSummary(1, "Max data rate is lower than expected for a GigE port");
try
{
/// @todo Something that doesn't oscillate
float multiplier = 1.0f;
if (recent_ratio == 1.0f)
{
multiplier = 1.1f;
}
else if (recent_ratio < 0.99f)
{
multiplier = 0.9f;
}
if (multiplier != 1.0f)
{
unsigned long new_data_rate = std::min((unsigned long)(multiplier * data_rate + 0.5), max_data_rate);
new_data_rate = std::max(new_data_rate, max_data_rate/1000);
if (data_rate != new_data_rate)
{
data_rate = new_data_rate;
cam_->setAttribute("StreamBytesPerSecond", data_rate);
ROS_DEBUG("Changed data rate to %lu bytes per second", data_rate);
}
}
}
catch (prosilica::ProsilicaException &e)
{
if (e.error_code == ePvErrUnplugged)
throw;
ROS_ERROR("Exception occurred: '%s'\n"
"Possible network issue. Attempting to reset data rate to the current maximum.",
e.what());
data_rate = max_data_rate;
cam_->setAttribute("StreamBytesPerSecond", data_rate);
}
}
status.add("Recent % Packets Received", recent_ratio * 100.0f);
status.add("Overall % Packets Received", total_ratio * 100.0f);
status.add("Received Packets", received);
status.add("Missed Packets", missed);
status.add("Requested Packets", requested);
status.add("Resent Packets", resent);
status.add("Data Rate (bytes/s)", data_rate);
status.add("Max Data Rate (bytes/s)", max_data_rate);
}
void WG06::diagnosticsFT(diagnostic_updater::DiagnosticStatusWrapper &d, WG06StatusWithAccelAndFT *status)
{
stringstream str;
str << "Force/Torque sensor (" << actuator_info_.name_ << ")";
d.name = str.str();
char serial[32];
snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
d.hardware_id = serial;
d.summary(d.OK, "OK");
d.clear();
ros::Time current_time(ros::Time::now());
double sample_frequency = 0.0;
if (!first_publish_)
{
sample_frequency = double(ft_sample_count_ - diag_last_ft_sample_count_) / (current_time - last_publish_time_).toSec();
}
diag_last_ft_sample_count_ = ft_sample_count_;
//d.addf("F/T sample count", "%llu", ft_sample_count_);
d.addf("F/T sample frequency", "%.2f (Hz)", sample_frequency);
d.addf("F/T missed samples", "%llu", ft_missed_samples_);
std::stringstream ss;
const FTDataSample &sample(status->ft_samples_[0]); //use newest data sample
for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
{
ss.str(""); ss << "Ch"<< (i);
d.addf(ss.str(), "%d", int(sample.data_[i]));
}
d.addf("FT Vhalf", "%d", int(sample.vhalf_));
if (ft_overload_flags_ != 0)
{
d.mergeSummary(d.ERROR, "Sensor overloaded");
ss.str("");
for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
{
ss << "Ch" << i << " ";
}
}
else
{
ss.str("None");
}
d.add("Overload Channels", ss.str());
if (ft_sampling_rate_error_)
{
d.mergeSummary(d.ERROR, "Sampling rate error");
}
if (ft_disconnected_)
{
d.mergeSummary(d.ERROR, "Amplifier disconnected");
}
else if (ft_vhalf_error_)
{
d.mergeSummary(d.ERROR, "Vhalf error, amplifier circuity may be damaged");
}
const std::vector<double> &ft_data( ft_analog_in_.state_.state_ );
if (ft_data.size() == 6)
{
d.addf("Force X", "%f", ft_data[0]);
d.addf("Force Y", "%f", ft_data[1]);
d.addf("Force Z", "%f", ft_data[2]);
d.addf("Torque X", "%f", ft_data[3]);
d.addf("Torque Y", "%f", ft_data[4]);
d.addf("Torque Z", "%f", ft_data[5]);
}
}
void WG06::diagnosticsPressure(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
{
int status_bytes =
has_accel_and_ft_ ? sizeof(WG06StatusWithAccelAndFT) : // Has FT sensor and accelerometer
accel_publisher_ ? sizeof(WG06StatusWithAccel) :
sizeof(WG0XStatus);
WG06Pressure *pressure = (WG06Pressure *)(buffer + command_size_ + status_bytes);
stringstream str;
str << "Pressure sensors (" << actuator_info_.name_ << ")";
d.name = str.str();
char serial[32];
snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
d.hardware_id = serial;
d.clear();
if (enable_pressure_sensor_)
{
d.summary(d.OK, "OK");
}
else
{
d.summary(d.OK, "Pressure sensor disabled by user");
}
if (pressure_checksum_error_)
{
d.mergeSummary(d.ERROR, "Checksum error on pressure data");
}
if (enable_pressure_sensor_)
{
// look at pressure sensor value to detect any damaged cables, or possibly missing sensor
unsigned l_finger_good_count = 0;
unsigned r_finger_good_count = 0;
for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
{
uint16_t l_data = pressure->l_finger_tip_[region_num];
if ((l_data != 0xFFFF) && (l_data != 0x0000))
{
++l_finger_good_count;
}
uint16_t r_data = pressure->r_finger_tip_[region_num];
if ((r_data != 0xFFFF) && (r_data != 0x0000))
{
++r_finger_good_count;
}
}
// if no pressure sensor regions are return acceptable data, then its possible that the
// pressure sensor is not connected at all. This is just a warning, because on many robots
// the pressure sensor may not be installed
if ((l_finger_good_count == 0) && (r_finger_good_count == 0))
{
d.mergeSummary(d.WARN, "Pressure sensors may not be connected");
}
else
{
// At least one pressure sensor seems to be present ...
if (l_finger_good_count == 0)
{
d.mergeSummary(d.WARN, "Sensor on left finger may not be connected");
}
else if (l_finger_good_count < NUM_PRESSURE_REGIONS)
{
d.mergeSummary(d.WARN, "Sensor on left finger may have damaged sensor regions");
}
if (r_finger_good_count == 0)
{
d.mergeSummary(d.WARN, "Sensor on right finger may not be connected");
}
else if (r_finger_good_count < NUM_PRESSURE_REGIONS)
{
d.mergeSummary(d.WARN, "Sensor on right finger may have damaged sensor regions");
}
}
d.addf("Timestamp", "%u", pressure->timestamp_);
d.addf("Data size", "%u", pressure_size_);
d.addf("Checksum error count", "%u", pressure_checksum_error_count_);
{ // put right and left finger data in dianostics
std::stringstream ss;
for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
{
ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
<< pressure->r_finger_tip_[region_num] << " ";
if (region_num%8 == 7)
ss << std::endl;
}
d.add("Right finger data", ss.str());
ss.str("");
for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
{
ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
<< pressure->l_finger_tip_[region_num] << " ";
if (region_num%8 == 7) ss << std::endl;
}
d.add("Left finger data", ss.str());
}
}
}
void HerculesHardwareDiagnosticTask<clearpath::DataSystemStatus>::update(diagnostic_updater::DiagnosticStatusWrapper &stat,
horizon_legacy::Channel<clearpath::DataSystemStatus>::Ptr &system_status)
{
msg_.uptime = system_status->getUptime();
msg_.battery_voltage = system_status->getVoltage(0);
msg_.left_driver_voltage = system_status->getVoltage(1);
msg_.right_driver_voltage = system_status->getVoltage(2);
msg_.mcu_and_user_port_current = system_status->getCurrent(0);
msg_.left_driver_current = system_status->getCurrent(1);
msg_.right_driver_current = system_status->getCurrent(2);
msg_.left_driver_temp = system_status->getTemperature(0);
msg_.right_driver_temp = system_status->getTemperature(1);
msg_.left_motor_temp = system_status->getTemperature(2);
msg_.right_motor_temp = system_status->getTemperature(3);
stat.add("Uptime", msg_.uptime);
stat.add("Battery Voltage", msg_.battery_voltage);
stat.add("Left Motor Driver Voltage", msg_.left_driver_voltage);
stat.add("Right Motor Driver Voltage", msg_.right_driver_voltage);
stat.add("MCU and User Port Current", msg_.mcu_and_user_port_current);
stat.add("Left Motor Driver Current", msg_.left_driver_current);
stat.add("Right Motor Driver Current", msg_.right_driver_current);
stat.add("Left Motor Driver Temp (C)", msg_.left_driver_temp);
stat.add("Right Motor Driver Temp (C)", msg_.right_driver_temp);
stat.add("Left Motor Temp (C)", msg_.left_motor_temp);
stat.add("Right Motor Temp (C)", msg_.right_motor_temp);
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "System Status OK");
if (msg_.battery_voltage > OVERVOLT_ERROR)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Main battery voltage too high");
}
else if (msg_.battery_voltage > OVERVOLT_WARN)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Main battery voltage too high");
}
else if (msg_.battery_voltage < UNDERVOLT_ERROR)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Main battery voltage too low");
}
else if (msg_.battery_voltage < UNDERVOLT_WARN)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Main battery voltage too low");
}
else
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::OK, "Voltage OK");
}
if (std::max(msg_.left_driver_temp, msg_.right_driver_temp) > DRIVER_OVERTEMP_ERROR)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Motor drivers too hot");
}
else if (std::max(msg_.left_driver_temp, msg_.right_driver_temp) > DRIVER_OVERTEMP_WARN)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Motor drivers too hot");
}
else if (std::max(msg_.left_motor_temp, msg_.right_motor_temp) > MOTOR_OVERTEMP_ERROR)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Motors too hot");
}
else if (std::max(msg_.left_motor_temp, msg_.right_motor_temp) > MOTOR_OVERTEMP_WARN)
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Motors too hot");
}
else
{
stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::OK, "Temperature OK");
}
}
