C++ DiagnosticStatusWrapper::add方法代码示例

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