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 CliffSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( status ) {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Cliff Detected!");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
}
stat.addf("Left", "Reading: %d Cliff: %s", values.bottom[0], status & CoreSensors::Flags::LeftCliff?"YES":"NO");
stat.addf("Center", "Reading: %d Cliff: %s", values.bottom[1], status & CoreSensors::Flags::CenterCliff?"YES":"NO");
stat.addf("Right", "Reading: %d Cliff: %s", values.bottom[2], status & CoreSensors::Flags::RightCliff?"YES":"NO");
}
void WallSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( status ) {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Wall Hit!");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
}
stat.addf("Left", status & CoreSensors::Flags::LeftBumper?"YES":"NO");
stat.addf("Center", status & CoreSensors::Flags::CenterBumper?"YES":"NO");
stat.addf("Right", status & CoreSensors::Flags::RightBumper?"YES":"NO");
}
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 run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
lock_guard lock(mutex);
if (voltage < 0)
stat.summary(2, "No data");
else if (voltage < min_voltage)
stat.summary(1, "Low voltage");
else
stat.summary(0, "Normal");
stat.addf("Voltage", "%.2f", voltage);
stat.addf("Current", "%.1f", current);
stat.addf("Remaining", "%.1f", remaining * 100);
}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
ssize_t freemem_ = freemem;
uint16_t brkval_ = brkval;
if (freemem < 0)
stat.summary(2, "No data");
else if (freemem < 200)
stat.summary(1, "Low mem");
else
stat.summary(0, "Normal");
stat.addf("Free memory (B)", "%zd", freemem_);
stat.addf("Heap top", "0x%04X", brkval_);
}
/** \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 run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
lock_guard lock(mutex);
if (vcc < 0)
stat.summary(2, "No data");
else if (vcc < 4.5)
stat.summary(1, "Low voltage");
else if (i2cerr != i2cerr_last) {
i2cerr_last = i2cerr;
stat.summary(1, "New I2C error");
}
else
stat.summary(0, "Normal");
stat.addf("Core voltage", "%f", vcc);
stat.addf("I2C errors", "%zu", i2cerr);
}
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 MotorCurrentTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( std::max(values[0], values[1]) > 6 ) { // TODO not sure about this threshold; should be a parameter?
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Is robot stalled? Motors current is very high");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "All right");
}
stat.addf("Left", "%d", values[0]);
stat.addf("Right", "%d", values[1]);
}
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);
}
void phidget_spatial_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
/**
* Function that will report the status of the hardware to the diagnostic topic
*/
{
if (!spatialError)
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "initialized");
else
{
stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "cannot be attached");
stat.addf("Recommendation", "Please verify that the robot has a Phidget Spatial board. If present, please unplug and replug the Phidget Spatial Board USB cable from the Motherboard.");
}
}
void phidget_ik_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
/**
* Function that will report the status of the hardware to the diagnostic topic
*/
{
if (!interfaceKitError)
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "initialized");
else
{
stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "cannot be attached");
stat.addf("Recommendation", "Please unplug and replug the Phidget Interface Kit Board USB cable from the Motherboard.");
}
}
void phidget_spatial_diagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
/**
* Function that will report the status of the hardware to the diagnostic topic
*/
{
if (!spatialError)
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "initialized");
else
{
stat.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "IMU cannot be initialized");
stat.addf("Recommendation", PhidgetIMU_INIT_ERROR);
}
}
void run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
lock_guard lock(mutex);
if ((last_st.onboard_control_sensors_health & last_st.onboard_control_sensors_enabled)
!= last_st.onboard_control_sensors_enabled)
stat.summary(2, "Sensor helth");
else
stat.summary(0, "Normal");
stat.addf("Sensor present", "0x%08X", last_st.onboard_control_sensors_present);
stat.addf("Sensor enabled", "0x%08X", last_st.onboard_control_sensors_enabled);
stat.addf("Sensor helth", "0x%08X", last_st.onboard_control_sensors_health);
// decode sensor health mask
#define STAT_ADD_SENSOR(msg, sensor_mask) \
if (last_st.onboard_control_sensors_enabled & sensor_mask) \
stat.add(msg, (last_st.onboard_control_sensors_health & sensor_mask) ? "Ok" : "Fail")
STAT_ADD_SENSOR("Sensor 3D Gyro", MAV_SYS_STATUS_SENSOR_3D_GYRO);
STAT_ADD_SENSOR("Sensor 3D Accel", MAV_SYS_STATUS_SENSOR_3D_ACCEL);
STAT_ADD_SENSOR("Sensor 3D Mag", MAV_SYS_STATUS_SENSOR_3D_MAG);
STAT_ADD_SENSOR("Sensor Abs Pressure", MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE);
STAT_ADD_SENSOR("Sensor Diff Pressure", MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE);
STAT_ADD_SENSOR("Sensor GPS", MAV_SYS_STATUS_SENSOR_GPS);
STAT_ADD_SENSOR("Sensor Optical Flow", MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW);
STAT_ADD_SENSOR("Sensor Vision Position", MAV_SYS_STATUS_SENSOR_VISION_POSITION);
STAT_ADD_SENSOR("Sensor Laser Position", MAV_SYS_STATUS_SENSOR_LASER_POSITION);
STAT_ADD_SENSOR("Sensor Ext Grount Truth", MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH);
STAT_ADD_SENSOR("Sensor Ang Rate Control", MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL);
STAT_ADD_SENSOR("Sensor Attitude Stab", MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION);
STAT_ADD_SENSOR("Sensor Yaw Position", MAV_SYS_STATUS_SENSOR_YAW_POSITION);
STAT_ADD_SENSOR("Sensor Z/Alt Control", MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL);
STAT_ADD_SENSOR("Sensor X/Y Pos Control", MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL);
STAT_ADD_SENSOR("Sensor Motor Outputs", MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS);
STAT_ADD_SENSOR("Sensor RC Receiver", MAV_SYS_STATUS_SENSOR_RC_RECEIVER);
STAT_ADD_SENSOR("Sensor 3D Gyro 2", MAV_SYS_STATUS_SENSOR_3D_GYRO2);
STAT_ADD_SENSOR("Sensor 3D Accel 2", MAV_SYS_STATUS_SENSOR_3D_ACCEL2);
STAT_ADD_SENSOR("Sensor 3D Mag 2", MAV_SYS_STATUS_SENSOR_3D_MAG2);
STAT_ADD_SENSOR("Geofence health", MAV_SYS_STATUS_GEOFENCE);
STAT_ADD_SENSOR("AHRS health", MAV_SYS_STATUS_AHRS);
STAT_ADD_SENSOR("Terrain health", MAV_SYS_STATUS_TERRAIN);
#undef STAT_ADD_SENSOR
stat.addf("CPU Load (%)", "%.1f", last_st.load / 10.0);
stat.addf("Drop rate (%)", "%.1f", last_st.drop_rate_comm / 10.0);
stat.addf("Errors comm", "%d", last_st.errors_comm);
stat.addf("Errors count #1", "%d", last_st.errors_count1);
stat.addf("Errors count #2", "%d", last_st.errors_count2);
stat.addf("Errors count #3", "%d", last_st.errors_count3);
stat.addf("Errors count #4", "%d", last_st.errors_count4);
}
void diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
lock_guard lock(diag_mutex);
if (!last_status) {
stat.summary(2, "No data");
return;
}
else if (last_status->rssi < low_rssi)
stat.summary(1, "Low RSSI");
else if (last_status->remrssi < low_rssi)
stat.summary(1, "Low remote RSSI");
else
stat.summary(0, "Normal");
stat.addf("RSSI", "%u", last_status->rssi);
stat.addf("RSSI (dBm)", "%.1f", last_status->rssi_dbm);
stat.addf("Remote RSSI", "%u", last_status->remrssi);
stat.addf("Remote RSSI (dBm)", "%.1f", last_status->remrssi_dbm);
stat.addf("Tx buffer (%)", "%u", last_status->txbuf);
stat.addf("Noice level", "%u", last_status->noise);
stat.addf("Remote noice level", "%u", last_status->remnoise);
stat.addf("Rx errors", "%u", last_status->rxerrors);
stat.addf("Fixed", "%u", last_status->fixed);
}
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::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());
}
}
}
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]);
}
}
