void DiagnosticNodelet::updateDiagnostic(
diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if (connection_status_ == SUBSCRIBED) {
if (vital_checker_->isAlive()) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
getName() + " running");
}
else {
jsk_topic_tools::addDiagnosticErrorSummary(
name_, vital_checker_, stat, diagnostic_error_level_);
}
}
else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
getName() + " is not subscribed");
}
std::stringstream topic_names;
for (size_t i = 0; i < publishers_.size(); i++) {
if (i == publishers_.size() - 1) {
topic_names << publishers_[i].getTopic();
}
else {
topic_names << publishers_[i].getTopic() << ", ";
}
}
stat.add("watched topics", topic_names.str());
for (size_t i = 0; i < publishers_.size(); i++) {
stat.add(publishers_[i].getTopic(),
(boost::format("%d subscribers") %
publishers_[i].getNumSubscribers()).str());
}
}
// Test validity of all attribute values.
void attributeTest(diagnostic_updater::DiagnosticStatusWrapper& status)
{
status.name = "Attribute Test";
tPvAttrListPtr list_ptr;
unsigned long list_length;
if (PvAttrList(cam_->handle(), &list_ptr, &list_length) == ePvErrSuccess) {
status.summary(0, "All attributes in valid range");
for (unsigned int i = 0; i < list_length; ++i) {
const char* attribute = list_ptr[i];
tPvErr e = PvAttrIsValid(cam_->handle(), attribute);
if (e != ePvErrSuccess) {
status.summary(2, "One or more invalid attributes");
if (e == ePvErrOutOfRange)
status.add(attribute, "Out of range");
else if (e == ePvErrNotFound)
status.add(attribute, "Does not exist");
else
status.addf(attribute, "Unexpected error code %u", e);
}
}
}
else {
status.summary(2, "Unable to retrieve attribute list");
}
}
void PIKSI::DiagCB( diagnostic_updater::DiagnosticStatusWrapper &stat )
{
boost::mutex::scoped_lock lock( cmd_lock );
if( piksid < 0 && !PIKSIOpenNoLock( ) )
{
stat.summary( diagnostic_msgs::DiagnosticStatus::ERROR, "Disconnected" );
return;
}
stat.summary( diagnostic_msgs::DiagnosticStatus::OK, "PIKSI status OK" );
int ret;
static unsigned int last_io_failure_count = io_failure_count;
if( io_failure_count > last_io_failure_count )
stat.summary( diagnostic_msgs::DiagnosticStatus::WARN, "I/O Failure Count Increase" );
stat.add( "io_failure_count", io_failure_count );
last_io_failure_count = io_failure_count;
static unsigned int last_open_failure_count = open_failure_count;
if( open_failure_count > last_open_failure_count )
stat.summary( diagnostic_msgs::DiagnosticStatus::WARN, "Open Failure Count Increase" );
stat.add( "open_failure_count", open_failure_count );
last_open_failure_count = open_failure_count;
}
void LimitedCounter::valueDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) {
bool active = ((ros::Time::now() - counter.getLastActivity()) < secondsConsideredActive);
if (counter.getValue() > maxValue) {
if ((ros::Time::now() - counter.getLastActivity()) > ros::Duration(5)) {
counter.setValue(0);
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "There are lost messages. Reseting counter");
} else
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "There are lost messages");
}
else if (counter.getValue() < minValue) {
if ((ros::Time::now() - counter.getLastActivity()) > ros::Duration(5)) {
counter.setValue(0);
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Invalid calls. Reseting counter.");
} else
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Invalid calls");
}
else {
if (active) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Active. No lost messages.");
stat.add("New Event: ", _name);
}
else {
if ((ros::Time::now() - counter.getLastActivity()) > ros::Duration(5)) {
counter.setValue(0);
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Inactive. Counter value turned to zero.");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Inactive. No lost messages");
}
}
}
stat.add("Counter Value ", counter.getValue());
stat.add("Max Value Accepted ", maxValue);
stat.add("Counter Active", active);
}
/* -*- diagnostics -*- */
void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
int fix_type, satellites_visible;
float eph, epv;
uas->get_gps_epts(eph, epv, fix_type, satellites_visible);
if (satellites_visible <= 0)
stat.summary(2, "No satellites");
else if (fix_type < 2)
stat.summary(1, "No fix");
else if (fix_type == 2)
stat.summary(0, "2D fix");
else if (fix_type >= 3)
stat.summary(0, "3D fix");
stat.addf("Satellites visible", "%zd", satellites_visible);
stat.addf("Fix type", "%d", fix_type);
if (!isnan(eph))
stat.addf("EPH (m)", "%.2f", eph);
else
stat.add("EPH (m)", "Unknown");
if (!isnan(epv))
stat.addf("EPV (m)", "%.2f", epv);
else
stat.add("EPV (m)", "Unknown");
}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
lock_guard lock(mutex);
ros::Time curtime = ros::Time::now();
int curseq = count_;
int events = curseq - seq_nums_[hist_indx_];
double window = (curtime - times_[hist_indx_]).toSec();
double freq = events / window;
seq_nums_[hist_indx_] = curseq;
times_[hist_indx_] = curtime;
hist_indx_ = (hist_indx_ + 1) % window_size_;
if (events == 0) {
stat.summary(2, "No events recorded.");
}
else if (freq < min_freq_ * (1 - tolerance_)) {
stat.summary(1, "Frequency too low.");
}
else if (freq > max_freq_ * (1 + tolerance_)) {
stat.summary(1, "Frequency too high.");
}
else {
stat.summary(0, "Normal");
}
stat.addf("Heartbeats since startup", "%d", count_);
stat.addf("Frequency (Hz)", "%f", freq);
stat.add("Vehicle type", mavros::UAS::str_type(type));
stat.add("Autopilot type", mavros::UAS::str_autopilot(autopilot));
stat.add("Mode", mode);
stat.add("System status", mavros::UAS::str_system_status(system_status));
}
void OrganizedPassThrough::updateDiagnostic(
diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if (vital_checker_->isAlive()) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
name_ + " running");
stat.add("Filtered points (Avg.)", filtered_points_counter_.mean());
if (filter_field_ == FIELD_X) {
stat.add("filter field", "x");
}
else if (filter_field_ == FIELD_Y) {
stat.add("filter field", "y");
}
stat.add("min index", min_index_);
stat.add("max index", max_index_);
jsk_topic_tools::addDiagnosticBooleanStat("keep organized",
keep_organized_,
stat);
jsk_topic_tools::addDiagnosticBooleanStat("filter_limit_negative",
filter_limit_negative_,
stat);
}
else {
jsk_recognition_utils::addDiagnosticErrorSummary(
"ClusterPointIndicesDecomposer", vital_checker_, stat);
}
}
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 CrioReceiver::checkGPSValues(diagnostic_updater::DiagnosticStatusWrapper &stat) {
stat.add("Latitude", gps_packet_.latitude);
stat.add("Longitude", gps_packet_.longitude);
stat.add("Lat Std Dev", gps_packet_.lat_std_dev);
stat.add("Long Std Dev", gps_packet_.long_std_dev);
stat.add("Solution Status", gps_packet_.solution_status);
stat.add("Position Type", gps_packet_.position_type);
stat.add("Differential Age", gps_packet_.differential_age);
stat.add("Solution Age", gps_packet_.solution_age);
//Need to cast the following... if they are uint8_t (chars) they get interpreted by the runtime monitor as characters and, when 0, can mess up the runtime_monitor
stat.add("Satellites Tracked", (uint16_t) gps_packet_.satellites_tracked);
stat.add("Satellites Computed", (uint16_t) gps_packet_.satellites_computed);
std::string status_msg;
status_msg += "No ranges specified for diagnostics checks. Values must be eyeballed";
unsigned char status_lvl = diagnostic_msgs::DiagnosticStatus::OK;
/*if (fabs(pose_packet_.yaw_bias) > 1.) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "Yaw sensor bias has diverged; ";
} else if (fabs(pose_packet_.yaw_bias) > 0.4 && fabs(pose_packet_.yaw_bias) < 1.) {
status_lvl = diagnostic_msgs::DiagnosticStatus::WARN;
status_msg += "Yaw sensor bias is outside of expected bounds; ";
} else {
status_msg += "Yaw sensor bias is okay; ";
}
if ((diagnostics_info_.YawSwing_mV / 1000.0) < 0.1) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "Yaw sensor seems disconnected; ";
} else {
status_msg += "Yaw sensor is connected; ";
}*/
stat.summary(status_lvl, status_msg);
}
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
stat.add("latest VICON frame number", lastFrameNumber);
stat.add("dropped frames", droppedFrameCount);
stat.add("framecount", frameCount);
stat.add("# markers", n_markers);
stat.add("# unlabeled markers", n_unlabeled_markers);
}
void diag_init(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(isInitialized_ && isDSAInitialized_)
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "");
else
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "");
stat.add("Hand initialized", isInitialized_);
stat.add("Tactile iInitialized", isDSAInitialized_);
}
void AudioProcessor::diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
stat.add("max period between updates", max_period_between_updates_);
stat.add("latest callback runtime", last_callback_duration_);
stat.add("latest VICON frame number", last_frame_number_);
stat.add("dropped frames", dropped_frame_count_);
stat.add("framecount", frame_count_);
}
///\brief Publishes diagnostics and status
void ControlMode::diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
{
int status;
status = ((state != ControlModeTypes::ERROR) ? diagnostic_msgs::DiagnosticStatus::OK
: diagnostic_msgs::DiagnosticStatus::ERROR);
stat.summary(status, stateToString(state));
stat.add("ready", ready);
stat.add("info", info);
}
void addDiagnosticBooleanStat(
const std::string& string_prefix,
const bool value,
diagnostic_updater::DiagnosticStatusWrapper& stat)
{
if (value) {
stat.add(string_prefix, "True");
}
else {
stat.add(string_prefix, "False");
}
}
void WG021::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
{
WG021Status *status = (WG021Status *)(buffer + command_size_);
stringstream str;
str << "EtherCAT Device (" << 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();
d.add("Configuration", config_info_.configuration_status_ ? "good" : "error loading configuration");
d.add("Name", actuator_info_.name_);
d.addf("Position", "%02d", sh_->get_ring_position());
d.addf("Product code",
"WG021 (%d) Firmware Revision %d.%02d, PCB Revision %c.%02d",
sh_->get_product_code(), fw_major_, fw_minor_,
'A' + board_major_, board_minor_);
d.add("Robot", actuator_info_.robot_name_);
d.add("Serial Number", serial);
d.addf("Nominal Current Scale", "%f", config_info_.nominal_current_scale_);
d.addf("Nominal Voltage Scale", "%f", config_info_.nominal_voltage_scale_);
d.addf("HW Max Current", "%f", config_info_.absolute_current_limit_ * config_info_.nominal_current_scale_);
d.addf("SW Max Current", "%f", actuator_info_.max_current_);
publishGeneralDiagnostics(d);
mailbox_.publishMailboxDiagnostics(d);
d.add("Mode", modeString(status->mode_));
d.addf("Digital out", "%d", status->digital_out_);
d.addf("Programmed current", "%f", status->programmed_current_ * config_info_.nominal_current_scale_);
d.addf("Measured current", "%f", status->measured_current_ * config_info_.nominal_current_scale_);
d.addf("Timestamp", "%u", status->timestamp_);
d.addf("Config 0", "%#02x", status->config0_);
d.addf("Config 1", "%#02x", status->config1_);
d.addf("Config 2", "%#02x", status->config2_);
d.addf("Output Status", "%#02x", status->output_status_);
d.addf("Output Start Timestamp", "%u", status->output_start_timestamp_);
d.addf("Output Stop Timestamp", "%u", status->output_stop_timestamp_);
d.addf("Board temperature", "%f", 0.0078125 * status->board_temperature_);
d.addf("Max board temperature", "%f", 0.0078125 * max_board_temperature_);
d.addf("Bridge temperature", "%f", 0.0078125 * status->bridge_temperature_);
d.addf("Max bridge temperature", "%f", 0.0078125 * max_bridge_temperature_);
d.addf("Supply voltage", "%f", status->supply_voltage_ * config_info_.nominal_voltage_scale_);
d.addf("LED voltage", "%f", status->led_voltage_ * config_info_.nominal_voltage_scale_);
d.addf("Packet count", "%d", status->packet_count_);
EthercatDevice::ethercatDiagnostics(d, 2);
}
void addDiagnosticInformation(
const std::string& string_prefix,
jsk_topic_tools::TimeAccumulator& accumulator,
diagnostic_updater::DiagnosticStatusWrapper& stat)
{
stat.add(string_prefix + " (Avg.)", accumulator.mean());
if (accumulator.mean() != 0.0) {
stat.add(string_prefix + " (Avg., fps)", 1.0 / accumulator.mean());
}
stat.add(string_prefix + " (Max)", accumulator.max());
stat.add(string_prefix + " (Min)", accumulator.min());
stat.add(string_prefix + " (Var.)", accumulator.variance());
}
void CrioReceiver::checkVoltageLevels(diagnostic_updater::DiagnosticStatusWrapper &stat) {
stat.add("24V", diagnostics_info_.VMonitor_24V_mV / 1000.0);
stat.add("13.8V", diagnostics_info_.VMonitor_13V_mV / 1000.0);
stat.add("5V", diagnostics_info_.VMonitor_5V_mV / 1000.0);
stat.add("cRIO", diagnostics_info_.VMonitor_cRIO_mV / 1000.0);
stat.add("eStop", diagnostics_info_.VMonitor_eStop_mV / 1000.0);
std::string status_msg;
unsigned char status_lvl = diagnostic_msgs::DiagnosticStatus::OK;
if (diagnostics_info_.VMonitor_cRIO_mV < 18 * 1000.0) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "cRIO voltage below 18V; ";
} else if (diagnostics_info_.VMonitor_cRIO_mV < 20 * 1000.0) {
status_lvl = diagnostic_msgs::DiagnosticStatus::WARN;
status_msg += "cRIO voltage below 20V; ";
}
if (diagnostics_info_.VMonitor_24V_mV < 21 * 1000.0) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "24V line voltage below 21V. Charge the robot immediately; ";
} else if (diagnostics_info_.VMonitor_24V_mV < 24 * 1000.0) {
if (status_lvl < diagnostic_msgs::DiagnosticStatus::WARN) {
status_lvl = diagnostic_msgs::DiagnosticStatus::WARN;
}
status_msg += "24V line voltage below 24V; ";
}
int16_t v13_diff = std::abs(diagnostics_info_.VMonitor_13V_mV - 13800);
if (v13_diff > 400) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "13.8V line voltage is more than 400mv from 13.8V; ";
} else if (v13_diff > 200) {
if (status_lvl < diagnostic_msgs::DiagnosticStatus::WARN) {
status_lvl = diagnostic_msgs::DiagnosticStatus::WARN;
}
status_msg += "13.8V line voltage is more than 200mv from 13.8V; ";
}
int16_t v5_diff = std::abs(diagnostics_info_.VMonitor_5V_mV - 5000);
if (v5_diff > 300) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "5V line voltage is more than 300mv from 5V; ";
} else if (v5_diff > 150) {
if (status_lvl < diagnostic_msgs::DiagnosticStatus::WARN) {
status_lvl = diagnostic_msgs::DiagnosticStatus::WARN;
}
status_msg += "5V line voltage is more than 150mv from 5V; ";
}
stat.summary(status_lvl, status_msg);
}
void produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if (_connected)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Laser OK");
}
else
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Laser not connected!");
}
stat.add("Retries", _retries);
stat.add("Scans", _scans);
}
void TiltLaserListener::updateDiagnostic(
diagnostic_updater::DiagnosticStatusWrapper &stat)
{
boost::mutex::scoped_lock lock(cloud_mutex_);
if (vital_checker_->isAlive()) {
if (not_use_laser_assembler_service_ &&
use_laser_assembler_) {
if (cloud_vital_checker_->isAlive()) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
getName() + " running");
}
else {
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"~input/cloud is not activate");
}
stat.add("scan queue", cloud_buffer_.size());
}
else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
getName() + " running");
}
}
else {
jsk_topic_tools::addDiagnosticErrorSummary(
name_, vital_checker_, stat);
}
}
void diag_init(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(is_initialized_bool_)
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "");
else
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "");
stat.add("Initialized", is_initialized_bool_);
}
void WalrusPodHW::roboteq_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper &stat, int controller)
{
if (controllers[controller]->is_connected())
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
else
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not Connected");
stat.add("Error Count", error_cnt);
}
void freqStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
{
double freq = (double)(count_)/diagnostic_.getPeriod();
if (freq < (.9*desired_freq_))
{
status.summary(2, "Desired frequency not met");
}
else
{
status.summary(0, "Desired frequency met");
}
status.add("Images in interval", count_);
status.add("Desired frequency", desired_freq_);
status.add("Actual frequency", freq);
count_ = 0;
}
void get_pid_diag_status(diagnostic_updater::DiagnosticStatusWrapper& pid_diag_status)
{
pid_diag_status.summary(diag_status);
pid_diag_status.add("Setpoint", setpoint);
pid_diag_status.add("Plant State", plant_state);
pid_diag_status.add("Control Error", error.at(0));
pid_diag_status.add("Control output effort", control_effort);
pid_diag_status.add("Proportional effort", proportional);
pid_diag_status.add("Integral effort", integral);
pid_diag_status.add("Derivative effort", derivative);
pid_diag_status.add("Measurements received", measurements_received);
}
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 ImuRosI::phidgetsDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if (is_connected_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "The Phidget is connected.");
stat.add("Device Serial Number", getDeviceSerialNumber());
stat.add("Device Name", getDeviceName());
stat.add("Device Type", getDeviceType());
}
else
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "The Phidget is not connected. Check the USB.");
}
if (error_number_ != 0)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "The Phidget reports error.");
stat.add("Error Number", error_number_);
stat.add("Error message", getErrorDescription(error_number_));
}
}
void CrioReceiver::checkYawSensor(diagnostic_updater::DiagnosticStatusWrapper &stat) {
stat.add("Yaw Rate", diagnostics_info_.YawRate_mV / 1000.0);
stat.add("Yaw Swing", diagnostics_info_.YawSwing_mV / 1000.0);
stat.add("Yaw Temp", diagnostics_info_.YawTemp_mV / 1000.0);
stat.add("Yaw Ref", diagnostics_info_.YawRef_mV / 1000.0);
stat.add("Yaw Bias", pose_packet_.yaw_bias);
stat.add("Yaw Bias Variance", pose_packet_.yaw_bias_variance);
std::string status_msg;
unsigned char status_lvl = diagnostic_msgs::DiagnosticStatus::OK;
if (fabs(pose_packet_.yaw_bias) > 1.) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "Yaw sensor bias has diverged; ";
} else if (fabs(pose_packet_.yaw_bias) > 0.4 && fabs(pose_packet_.yaw_bias) < 1.) {
status_lvl = diagnostic_msgs::DiagnosticStatus::WARN;
status_msg += "Yaw sensor bias is outside of expected bounds; ";
} else {
status_msg += "Yaw sensor bias is okay; ";
}
if ((diagnostics_info_.YawSwing_mV / 1000.0) < 0.1) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "Yaw sensor seems disconnected; ";
} else {
status_msg += "Yaw sensor is connected; ";
}
stat.summary(status_lvl, status_msg);
}
void WalrusPodHW::pod_control_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper &stat, int pod)
{
bool position_valid = pm_feedback_timeout > (ros::Time::now() - last_pm_feedback);
bool current_valid = controllers[pod & CONTROLLER_MASK]->is_connected();
bool warning = false;
bool error = false;
string msg = "";
if (!position_valid)
{
error = true;
msg += "No position data. ";
}
if (!current_valid)
{
error = true;
msg += "No current data. ";
}
else if (POD_MOTOR_CURRENT_HIGH_ABOVE > 0 && pod_current[pod] > POD_MOTOR_CURRENT_HIGH_ABOVE)
{
warning = true;
msg += "Motor current high. ";
}
uint8_t level = diagnostic_msgs::DiagnosticStatus::OK;
if (error)
level = diagnostic_msgs::DiagnosticStatus::ERROR;
else if (warning)
level = diagnostic_msgs::DiagnosticStatus::WARN;
else
msg = "Everything OK";
stat.summary(level, msg);
stat.add("Velocity", position_valid ? formatDouble(pod_velocity[pod], 0) + " rpm" : "No Position Data");
stat.add("Position", position_valid ? formatDouble((pod_position[pod]*180/M_PI), 2) + "\xc2\xb0" : "No Positon Data");
stat.add("Current", current_valid ? formatDouble(pod_current[pod], 3) + " A" : "No Current Data");
}
void packetErrorStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
{
unsigned long erroneous;
cam_->getAttribute("StatPacketsErroneous", erroneous);
if (erroneous == 0) {
status.summary(0, "No erroneous packets");
} else {
status.summary(2, "Possible camera hardware failure");
}
status.add("Erroneous Packets", erroneous);
}
void frameStatistics(diagnostic_updater::DiagnosticStatusWrapper& status)
{
// Get stats from camera driver
float frame_rate;
unsigned long completed, dropped;
cam_->getAttribute("StatFrameRate", frame_rate);
cam_->getAttribute("StatFramesCompleted", completed);
cam_->getAttribute("StatFramesDropped", dropped);
// Compute rolling totals, percentages
frames_completed_acc_.add(completed - frames_completed_total_);
frames_completed_total_ = completed;
unsigned long completed_recent = frames_completed_acc_.sum();
frames_dropped_acc_.add(dropped - frames_dropped_total_);
frames_dropped_total_ = dropped;
unsigned long dropped_recent = frames_dropped_acc_.sum();
float recent_ratio = float(completed_recent) / (completed_recent + dropped_recent);
float total_ratio = float(completed) / (completed + dropped);
// Set level based on recent % completed frames
if (dropped_recent == 0) {
status.summary(0, "No dropped frames");
}
else if (recent_ratio > 0.8f) {
status.summary(1, "Some dropped frames");
}
else {
status.summary(2, "Excessive proportion of dropped frames");
}
status.add("Camera Frame Rate", frame_rate);
status.add("Recent % Frames Completed", recent_ratio * 100.0f);
status.add("Overall % Frames Completed", total_ratio * 100.0f);
status.add("Frames Completed", completed);
status.add("Frames Dropped", dropped);
}
void EuclideanClustering::updateDiagnostic(
diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if (vital_checker_->isAlive()) {
stat.summary(
diagnostic_msgs::DiagnosticStatus::OK,
"EuclideanSegmentation running");
jsk_recognition_utils::addDiagnosticInformation(
"Kdtree Construction", kdtree_acc_, stat);
jsk_recognition_utils::addDiagnosticInformation(
"Euclidean Segmentation", segmentation_acc_, stat);
stat.add("Cluster Num (Avg.)", cluster_counter_.mean());
stat.add("Max Size of the cluster", maxsize_);
stat.add("Min Size of the cluster", minsize_);
stat.add("Cluster tolerance", tolerance);
stat.add("Tracking tolerance", label_tracking_tolerance);
}
else {
jsk_recognition_utils::addDiagnosticErrorSummary(
"EuclideanClustering", vital_checker_, stat);
}
}
