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"); } }