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