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

  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 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 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 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 HerculesSoftwareDiagnosticTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
  {
    msg_.ros_control_loop_freq = control_freq_;
    stat.add("ROS Control Loop Frequency", msg_.ros_control_loop_freq);

    double margin = control_freq_ / target_control_freq_ * 100;

    stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Software OK");
    if (margin < CONTROLFREQ_WARN)
    {
      std::ostringstream message;
      message << "Control loop executing " << 100 - static_cast<int>(margin) << "% slower than desired";
      stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, message.str());
    }

    reset();
  }

  void packetStatistics(diagnostic_updater::DiagnosticStatusWrapper& status)
  {
    // Get stats from camera driver
    unsigned long received, missed, requested, resent;
    cam_->getAttribute("StatPacketsReceived", received);
    cam_->getAttribute("StatPacketsMissed", missed);
    cam_->getAttribute("StatPacketsRequested", requested);
    cam_->getAttribute("StatPacketsResent", resent);

    // Compute rolling totals, percentages
    packets_received_acc_.add(received - packets_received_total_);
    packets_received_total_ = received;
    unsigned long received_recent = packets_received_acc_.sum();

    packets_missed_acc_.add(missed - packets_missed_total_);
    packets_missed_total_ = missed;
    unsigned long missed_recent = packets_missed_acc_.sum();

    float recent_ratio = float(received_recent) / (received_recent + missed_recent);
    float total_ratio = float(received) / (received + missed);

    if (missed_recent == 0) {
      status.summary(0, "No missed packets");
    }
    else if (recent_ratio > 0.99f) {
      status.summary(1, "Some missed packets");
    }
    else {
      status.summary(2, "Excessive proportion of missed packets");
    }

    // Adjust data rate
    unsigned long data_rate = 0;
    cam_->getAttribute("StreamBytesPerSecond", data_rate);
    unsigned long max_data_rate = cam_->getMaxDataRate();

    if (auto_adjust_stream_bytes_per_second_)
    {
      if (max_data_rate < prosilica::Camera::GIGE_MAX_DATA_RATE)
        status.mergeSummary(1, "Max data rate is lower than expected for a GigE port");

      try
      {
        /// @todo Something that doesn't oscillate
        float multiplier = 1.0f;
        if (recent_ratio == 1.0f)
        {
          multiplier = 1.1f;
	    }
        else if (recent_ratio < 0.99f)
        {
          multiplier = 0.9f;
        }
        if (multiplier != 1.0f)
        {
          unsigned long new_data_rate = std::min((unsigned long)(multiplier * data_rate + 0.5), max_data_rate);
          new_data_rate = std::max(new_data_rate, max_data_rate/1000);
          if (data_rate != new_data_rate)
          {
            data_rate = new_data_rate;
            cam_->setAttribute("StreamBytesPerSecond", data_rate);
            ROS_DEBUG("Changed data rate to %lu bytes per second", data_rate);
          }
        }
      }
      catch (prosilica::ProsilicaException &e)
      {
        if (e.error_code == ePvErrUnplugged)
          throw;
        ROS_ERROR("Exception occurred: '%s'\n"
                  "Possible network issue. Attempting to reset data rate to the current maximum.",
                  e.what());
        data_rate = max_data_rate;
        cam_->setAttribute("StreamBytesPerSecond", data_rate);
      }
    }

    status.add("Recent % Packets Received", recent_ratio * 100.0f);
    status.add("Overall % Packets Received", total_ratio * 100.0f);
    status.add("Received Packets", received);
    status.add("Missed Packets", missed);
    status.add("Requested Packets", requested);
    status.add("Resent Packets", resent);
    status.add("Data Rate (bytes/s)", data_rate);
    status.add("Max Data Rate (bytes/s)", max_data_rate);
  }

void WG06::diagnosticsFT(diagnostic_updater::DiagnosticStatusWrapper &d, WG06StatusWithAccelAndFT *status)
{
  stringstream str;
  str << "Force/Torque sensor (" << 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();

  ros::Time current_time(ros::Time::now());
  double sample_frequency = 0.0;
  if (!first_publish_)
  {
    sample_frequency = double(ft_sample_count_ - diag_last_ft_sample_count_) / (current_time - last_publish_time_).toSec();
  }
  diag_last_ft_sample_count_ = ft_sample_count_;

  //d.addf("F/T sample count", "%llu", ft_sample_count_);
  d.addf("F/T sample frequency", "%.2f (Hz)", sample_frequency);
  d.addf("F/T missed samples", "%llu", ft_missed_samples_);
  std::stringstream ss;
  const FTDataSample &sample(status->ft_samples_[0]);  //use newest data sample
  for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
  {
    ss.str(""); ss << "Ch"<< (i);
    d.addf(ss.str(), "%d", int(sample.data_[i]));
  }
  d.addf("FT Vhalf", "%d", int(sample.vhalf_));

  if (ft_overload_flags_ != 0)
  {
    d.mergeSummary(d.ERROR, "Sensor overloaded");
    ss.str("");
    for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
    {
      ss << "Ch" << i << " ";
    }
  }
  else 
  {
    ss.str("None");
  }
  d.add("Overload Channels", ss.str());

  if (ft_sampling_rate_error_)
  {
    d.mergeSummary(d.ERROR, "Sampling rate error");
  }

  if (ft_disconnected_)
  {
    d.mergeSummary(d.ERROR, "Amplifier disconnected");
  }
  else if (ft_vhalf_error_)
  {
    d.mergeSummary(d.ERROR, "Vhalf error, amplifier circuity may be damaged"); 
  }

  const std::vector<double> &ft_data( ft_analog_in_.state_.state_ );
  if (ft_data.size() == 6)
  {
    d.addf("Force X",  "%f", ft_data[0]);
    d.addf("Force Y",  "%f", ft_data[1]);
    d.addf("Force Z",  "%f", ft_data[2]);
    d.addf("Torque X", "%f", ft_data[3]);
    d.addf("Torque Y", "%f", ft_data[4]);
    d.addf("Torque Z", "%f", ft_data[5]);
  }
}

void WG06::diagnosticsPressure(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
{
  int status_bytes = 
    has_accel_and_ft_  ? sizeof(WG06StatusWithAccelAndFT) :  // Has FT sensor and accelerometer
    accel_publisher_   ? sizeof(WG06StatusWithAccel) : 
                         sizeof(WG0XStatus);
  WG06Pressure *pressure = (WG06Pressure *)(buffer + command_size_ + status_bytes);

  stringstream str;
  str << "Pressure sensors (" << 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.clear();

  if (enable_pressure_sensor_)
  {    
    d.summary(d.OK, "OK");
  }
  else
  {
    d.summary(d.OK, "Pressure sensor disabled by user");
  }

  if (pressure_checksum_error_)
  {
    d.mergeSummary(d.ERROR, "Checksum error on pressure data");
  }
    
  if (enable_pressure_sensor_)
  {
    // look at pressure sensor value to detect any damaged cables, or possibly missing sensor
    unsigned l_finger_good_count = 0;
    unsigned r_finger_good_count = 0;

    for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
    {
      uint16_t l_data = pressure->l_finger_tip_[region_num];
      if ((l_data != 0xFFFF) && (l_data != 0x0000))
      {
        ++l_finger_good_count;
      }

      uint16_t r_data = pressure->r_finger_tip_[region_num];
      if ((r_data != 0xFFFF) && (r_data != 0x0000))
      {
        ++r_finger_good_count;
      }
    }
      
    // if no pressure sensor regions are return acceptable data, then its possible that the
    // pressure sensor is not connected at all.  This is just a warning, because on many robots
    // the pressure sensor may not be installed
    if ((l_finger_good_count == 0) && (r_finger_good_count == 0))
    {
      d.mergeSummary(d.WARN, "Pressure sensors may not be connected");
    }
    else 
    {
      // At least one pressure sensor seems to be present ...       
      if (l_finger_good_count == 0)
      {
        d.mergeSummary(d.WARN, "Sensor on left finger may not be connected");
      }
      else if (l_finger_good_count < NUM_PRESSURE_REGIONS)
      {
        d.mergeSummary(d.WARN, "Sensor on left finger may have damaged sensor regions");
      }

      if (r_finger_good_count == 0)
      {
        d.mergeSummary(d.WARN, "Sensor on right finger may not be connected");
      }
      else if (r_finger_good_count < NUM_PRESSURE_REGIONS)
      {
        d.mergeSummary(d.WARN, "Sensor on right finger may have damaged sensor regions");
      }
    } 

    d.addf("Timestamp", "%u", pressure->timestamp_);
    d.addf("Data size", "%u", pressure_size_);
    d.addf("Checksum error count", "%u", pressure_checksum_error_count_);

    { // put right and left finger data in dianostics
      std::stringstream ss;

      for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
      {
        ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0') 
           << pressure->r_finger_tip_[region_num] << " ";
        if (region_num%8 == 7) 
          ss << std::endl;      
      }
      d.add("Right finger data",  ss.str());

      ss.str("");

      for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
      {
//.........这里部分代码省略.........