Example #1
0
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); 
}
Example #2
0
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_);                                   
}
Example #3
0
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]);
  }
}
Example #4
0
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)
      {
        ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0') 
           << pressure->l_finger_tip_[region_num] << " ";
        if (region_num%8 == 7) ss << std::endl;      
      }
      d.add("Left finger data",  ss.str());
    }
  }

}