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

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

	/* -*- 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)
	{
		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 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 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 run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
		lock_guard lock(mutex);

		if (!data_received)
			stat.summary(2, "No data");
		else if (last_rst.rssi < low_rssi)
			stat.summary(1, "Low RSSI");
		else if (last_rst.remrssi < low_rssi)
			stat.summary(1, "Low remote RSSI");
		else
			stat.summary(0, "Normal");

		float rssi_dbm = (last_rst.rssi / 1.9) - 127;
		float remrssi_dbm = (last_rst.remrssi / 1.9) - 127;

		stat.addf("RSSI", "%u", last_rst.rssi);
		stat.addf("RSSI (dBm)", "%.1f", rssi_dbm);
		stat.addf("Remote RSSI", "%u", last_rst.remrssi);
		stat.addf("Remote RSSI (dBm)", "%.1f", remrssi_dbm);
		stat.addf("Tx buffer (%)", "%u", last_rst.txbuf);
		stat.addf("Noice level", "%u", last_rst.noise);
		stat.addf("Remote noice level", "%u", last_rst.remnoise);
		stat.addf("Rx errors", "%u", last_rst.rxerrors);
		stat.addf("Fixed", "%u", last_rst.fixed);
	}

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

/**  \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 run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
		boost::recursive_mutex::scoped_lock lock(mutex);

		if (satellites_visible < 0)
			stat.summary(2, "No satellites");
		else if (fix_type < 2 || fix_type > 3)
			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);
	}

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

  // 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 gps_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
    /**
     * Function that will report the status of the hardware to the diagnostic topic
     */
    {
	if (gps_state == 0)  
		stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "The gps is working");
	else if (gps_state == 1)
	{
		stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Can't intialize");
		stat.addf("Recommendation", "The gps could not be initialized. Please make sure the gps is connected to the motherboard and is configured. You can follow points 1.3 and after in the following tutorial for the configuration: http://ros.org/wiki/gpsd_client/Tutorials/Getting started with gpsd_client");
	}
	else if (gps_state == 2)
	{
		stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "wrong lib gpsd version");
		stat.addf("Recommendation", "Please make sure that gpsd is installed and that you have at least the api major version 3 or after installed.");
	}
    }

void phidget_encoder_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
/**
 * Function that will report the status of the hardware to the diagnostic topic
 */
{
	if (!encoderError)  
		stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "intialized");
	else if(encoderError == 1)
	{
		stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "cannot be attached");
		stat.addf("Recommendation", "Please verify that the robot has a Phidget Encoder board. If present, please unplug and replug the Phidget Spatial Board USB cable from the Motherboard.");
	}
	else if(encoderError == 2)
	{
		stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "cannot read");
		stat.addf("Recommendation", "Please verify that the two encoders are connected to the Phidget Encoder Board.");
	}

}

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