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