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