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 WatchdogTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) { if ( alive ) { stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Alive"); } else { stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No Signal"); } }
/* -*- 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("Timesyncs since startup", "%d", count_); stat.addf("Frequency (Hz)", "%f", freq); stat.addf("Last dt (ms)", "%0.6f", last_dt / 1e6); stat.addf("Mean dt (ms)", "%0.6f", (count_) ? dt_sum / count_ / 1e6 : 0.0); stat.addf("Last system time (s)", "%0.9f", last_ts / 1e9); stat.addf("Time offset (s)", "%0.9f", offset / 1e9); }
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 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 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()); } }
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 diagnoseConnections(diagnostic_updater::DiagnosticStatusWrapper &status) { if (!fadeToColorClient) status.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Not all required services are connected."); else status.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "All required services are connected."); }
void diag_init(diagnostic_updater::DiagnosticStatusWrapper &stat) { if(isInitialized_) stat.summary(diagnostic_msgs::DiagnosticStatus::OK, ""); else stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, ""); stat.add("Initialized", isInitialized_); }
///\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 WheelDropTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) { if ( status ) { stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wheel Drop!"); } else { stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right"); } stat.addf("Left", status & CoreSensors::Flags::LeftWheel?"YES":"NO"); stat.addf("Right", status & CoreSensors::Flags::RightWheel?"YES":"NO"); }
void MotorCurrentTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) { if ( std::max(values[0], values[1]) > 6 ) { // TODO not sure about this threshold; should be a parameter? stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Is robot stalled? Motors current is very high"); } else { stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right"); } stat.addf("Left", "%d", values[0]); stat.addf("Right", "%d", values[1]); }
void CANPriusNode::diagnoseCANConnection( diagnostic_updater::DiagnosticStatusWrapper& status) { if (_canConnection && _canConnection->isOpen()) status.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "CAN connection opened on %s.", _canConnection->getDevicePathStr().c_str()); else status.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "CAN connection closed on %s.", _canDeviceStr.c_str()); }
void run(diagnostic_updater::DiagnosticStatusWrapper &stat) { if (boost::shared_ptr<MAVConnInterface> link = weak_link.lock()) { mavlink_status_t mav_status = link->get_status(); stat.addf("Received packets:", "%u", mav_status.packet_rx_success_count); stat.addf("Dropped packets:", "%u", mav_status.packet_rx_drop_count); stat.addf("Buffer overruns:", "%u", mav_status.buffer_overrun); stat.addf("Parse errors:", "%u", mav_status.parse_error); stat.addf("Rx sequence number:", "%u", mav_status.current_rx_seq); stat.addf("Tx sequence number:", "%u", mav_status.current_tx_seq); if (mav_status.packet_rx_drop_count > last_drop_count) stat.summaryf(1, "%d packeges dropped since last report", mav_status.packet_rx_drop_count - last_drop_count); else if (is_connected) stat.summary(0, "connected"); else // link operational, but not connected stat.summary(1, "not connected"); last_drop_count = mav_status.packet_rx_drop_count; } else { stat.summary(2, "not connected"); } }
void WallSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) { if ( status ) { stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wall Hit!"); } else { stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right"); } stat.addf("Left", status & CoreSensors::Flags::LeftBumper?"YES":"NO"); stat.addf("Center", status & CoreSensors::Flags::CenterBumper?"YES":"NO"); stat.addf("Right", status & CoreSensors::Flags::RightBumper?"YES":"NO"); }
void CliffSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) { if ( status ) { stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Cliff Detected!"); } else { stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right"); } stat.addf("Left", "Reading: %d Cliff: %s", values.bottom[0], status & CoreSensors::Flags::LeftCliff?"YES":"NO"); stat.addf("Center", "Reading: %d Cliff: %s", values.bottom[1], status & CoreSensors::Flags::CenterCliff?"YES":"NO"); stat.addf("Right", "Reading: %d Cliff: %s", values.bottom[2], status & CoreSensors::Flags::RightCliff?"YES":"NO"); }
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 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 phidget_ik_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) /** * Function that will report the status of the hardware to the diagnostic topic */ { if (!interfaceKitError) stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "initialized"); else { stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "cannot be attached"); stat.addf("Recommendation", "Please unplug and replug the Phidget Interface Kit Board USB cable from the Motherboard."); } }
void phidget_spatial_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) /** * Function that will report the status of the hardware to the diagnostic topic */ { if (!spatialError) stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "initialized"); else { stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "cannot be attached"); stat.addf("Recommendation", "Please verify that the robot has a Phidget Spatial board. If present, please unplug and replug the Phidget Spatial Board USB cable from the Motherboard."); } }
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 phidget_spatial_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) /** * Function that will report the status of the hardware to the diagnostic topic */ { if (!spatialError) stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "initialized"); else { stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "IMU cannot be initialized"); stat.addf("Recommendation", PhidgetIMU_INIT_ERROR); } }
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 run(diagnostic_updater::DiagnosticStatusWrapper &stat) { ssize_t freemem_ = freemem; uint16_t brkval_ = brkval; if (freemem < 0) stat.summary(2, "No data"); else if (freemem < 200) stat.summary(1, "Low mem"); else stat.summary(0, "Normal"); stat.addf("Free memory (B)", "%zd", freemem_); stat.addf("Heap top", "0x%04X", brkval_); }
void run(diagnostic_updater::DiagnosticStatusWrapper &stat) { lock_guard lock(mutex); if (voltage < 0) stat.summary(2, "No data"); else if (voltage < min_voltage) stat.summary(1, "Low voltage"); else stat.summary(0, "Normal"); stat.addf("Voltage", "%.2f", voltage); stat.addf("Current", "%.1f", current); stat.addf("Remaining", "%.1f", remaining * 100); }
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); }
/* * \brief Checks that the robot is receiving at a correct frequency the command messages. Diagnostics * */ void check_command_subscriber(diagnostic_updater::DiagnosticStatusWrapper &stat) { ros::Time current_time = ros::Time::now(); double diff = (current_time - last_command_time_).toSec(); if(diff > 1.0){ stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Topic is not receiving commands"); //ROS_INFO("check_command_subscriber: %lf seconds without commands", diff); // TODO: Set Speed References to 0 }else{ stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Topic receiving commands"); } }
void VitalCheckerNodelet::updateDiagnostic( diagnostic_updater::DiagnosticStatusWrapper &stat) { boost::mutex::scoped_lock lock(mutex_); if (vital_checker_->isAlive()) { stat.summary(diagnostic_msgs::DiagnosticStatus::OK, title_ + " is running"); stat.add("last alive time", vital_checker_->lastAliveTimeRelative()); } else { addDiagnosticErrorSummary( title_, vital_checker_, stat); } }
/** * Function that will report the status of the hardware to the diagnostic topic */ void ssc32_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat) { if (!ssc32Error) stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "initialized"); else if (ssc32Error == 1) { stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "ssc32 cannot be initialized"); stat.addf("Recommendation",SSC32_ERROR_CONNECTION); } else if (ssc32Error == 2) { stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "cannot move arm"); stat.addf("Recommendation", ERROR_MOVING_ARM); } }