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 WatchdogTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
if ( alive ) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Alive");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No Signal");
}
}
void TiltLaserListener::updateDiagnostic(
diagnostic_updater::DiagnosticStatusWrapper &stat)
{
boost::mutex::scoped_lock lock(cloud_mutex_);
if (vital_checker_->isAlive()) {
if (not_use_laser_assembler_service_ &&
use_laser_assembler_) {
if (cloud_vital_checker_->isAlive()) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
getName() + " running");
}
else {
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"~input/cloud is not activate");
}
stat.add("scan queue", cloud_buffer_.size());
}
else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
getName() + " running");
}
}
else {
jsk_topic_tools::addDiagnosticErrorSummary(
name_, vital_checker_, stat);
}
}
// 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 (!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 DiagnosticNodelet::updateDiagnostic(
diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if (connection_status_ == SUBSCRIBED) {
if (vital_checker_->isAlive()) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
getName() + " running");
}
else {
jsk_topic_tools::addDiagnosticErrorSummary(
name_, vital_checker_, stat, diagnostic_error_level_);
}
}
else {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
getName() + " is not subscribed");
}
std::stringstream topic_names;
for (size_t i = 0; i < publishers_.size(); i++) {
if (i == publishers_.size() - 1) {
topic_names << publishers_[i].getTopic();
}
else {
topic_names << publishers_[i].getTopic() << ", ";
}
}
stat.add("watched topics", topic_names.str());
for (size_t i = 0; i < publishers_.size(); i++) {
stat.add(publishers_[i].getTopic(),
(boost::format("%d subscribers") %
publishers_[i].getNumSubscribers()).str());
}
}
void PIKSI::DiagCB( diagnostic_updater::DiagnosticStatusWrapper &stat )
{
boost::mutex::scoped_lock lock( cmd_lock );
if( piksid < 0 && !PIKSIOpenNoLock( ) )
{
stat.summary( diagnostic_msgs::DiagnosticStatus::ERROR, "Disconnected" );
return;
}
stat.summary( diagnostic_msgs::DiagnosticStatus::OK, "PIKSI status OK" );
int ret;
static unsigned int last_io_failure_count = io_failure_count;
if( io_failure_count > last_io_failure_count )
stat.summary( diagnostic_msgs::DiagnosticStatus::WARN, "I/O Failure Count Increase" );
stat.add( "io_failure_count", io_failure_count );
last_io_failure_count = io_failure_count;
static unsigned int last_open_failure_count = open_failure_count;
if( open_failure_count > last_open_failure_count )
stat.summary( diagnostic_msgs::DiagnosticStatus::WARN, "Open Failure Count Increase" );
stat.add( "open_failure_count", open_failure_count );
last_open_failure_count = open_failure_count;
}
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");
}
}
/* -*- 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) {
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));
}
void WalrusPodHW::roboteq_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper &stat, int controller)
{
if (controllers[controller]->is_connected())
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
else
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not Connected");
stat.add("Error Count", error_cnt);
}
void diag_init(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if(isInitialized_)
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "");
else
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "");
stat.add("Initialized", isInitialized_);
}
void BatteryTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
switch ( status.level() ) {
case ( Battery::Maximum ) : {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Maximum");
break;
}
case ( Battery::Healthy ) : {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Healthy");
break;
}
case ( Battery::Low ) : {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Low");
break;
}
case ( Battery::Dangerous ) : {
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Dangerous");
break;
}
}
stat.add("Voltage (V)", status.voltage);
stat.add("Percent", status.percent());
stat.add("Charge (Ah)", (2.2*status.percent())/100.0);
stat.add("Capacity (Ah)", 2.2); // TODO: how can we tell which battery is in use?
switch (status.charging_source ) {
case(Battery::None) : {
stat.add("Source", "None");
break;
}
case(Battery::Adapter) : {
stat.add("Source", "Adapter");
break;
}
case(Battery::Dock) : {
stat.add("Source", "Dock");
break;
}
}
switch ( status.charging_state ) {
case ( Battery::Charged ) : {
stat.add("State", "Charged");
stat.add("Current (A)", 3.14); // TODO: what's the real value for our charger?
break;
}
case ( Battery::Charging ) : {
stat.add("State", "Charging");
stat.add("Current (A)", 3.14); // TODO: what's the real value for our charger?
break;
}
case ( Battery::Discharging ) : {
stat.add("State", "Discharging");
stat.add("Current (A)", 0.0);
break;
}
default: break;
}
}
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 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 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 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 packetErrorStatus(diagnostic_updater::DiagnosticStatusWrapper& status)
{
unsigned long erroneous;
cam_->getAttribute("StatPacketsErroneous", erroneous);
if (erroneous == 0) {
status.summary(0, "No erroneous packets");
} else {
status.summary(2, "Possible camera hardware failure");
}
status.add("Erroneous Packets", erroneous);
}
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_);
}
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 produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if (_connected)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Laser OK");
}
else
{
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Laser not connected!");
}
stat.add("Retries", _retries);
stat.add("Scans", _scans);
}
/*
* \brief Checks that the robot is receiving at a correct frequency the command messages. Diagnostics
*
*/
void check_command_subscriber(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
ros::Time current_time = ros::Time::now();
double diff = (current_time - last_command_time_).toSec();
if(diff > 1.0){
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Topic is not receiving commands");
//ROS_INFO("check_command_subscriber: %lf seconds without commands", diff);
// TODO: Set Speed References to 0
}else{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Topic receiving commands");
}
}
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);
}
void CrioReceiver::checkGPSValues(diagnostic_updater::DiagnosticStatusWrapper &stat) {
stat.add("Latitude", gps_packet_.latitude);
stat.add("Longitude", gps_packet_.longitude);
stat.add("Lat Std Dev", gps_packet_.lat_std_dev);
stat.add("Long Std Dev", gps_packet_.long_std_dev);
stat.add("Solution Status", gps_packet_.solution_status);
stat.add("Position Type", gps_packet_.position_type);
stat.add("Differential Age", gps_packet_.differential_age);
stat.add("Solution Age", gps_packet_.solution_age);
//Need to cast the following... if they are uint8_t (chars) they get interpreted by the runtime monitor as characters and, when 0, can mess up the runtime_monitor
stat.add("Satellites Tracked", (uint16_t) gps_packet_.satellites_tracked);
stat.add("Satellites Computed", (uint16_t) gps_packet_.satellites_computed);
std::string status_msg;
status_msg += "No ranges specified for diagnostics checks. Values must be eyeballed";
unsigned char status_lvl = diagnostic_msgs::DiagnosticStatus::OK;
/*if (fabs(pose_packet_.yaw_bias) > 1.) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "Yaw sensor bias has diverged; ";
} else if (fabs(pose_packet_.yaw_bias) > 0.4 && fabs(pose_packet_.yaw_bias) < 1.) {
status_lvl = diagnostic_msgs::DiagnosticStatus::WARN;
status_msg += "Yaw sensor bias is outside of expected bounds; ";
} else {
status_msg += "Yaw sensor bias is okay; ";
}
if ((diagnostics_info_.YawSwing_mV / 1000.0) < 0.1) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "Yaw sensor seems disconnected; ";
} else {
status_msg += "Yaw sensor is connected; ";
}*/
stat.summary(status_lvl, status_msg);
}
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 OrganizedPassThrough::updateDiagnostic(
diagnostic_updater::DiagnosticStatusWrapper &stat)
{
if (vital_checker_->isAlive()) {
stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
name_ + " running");
stat.add("Filtered points (Avg.)", filtered_points_counter_.mean());
if (filter_field_ == FIELD_X) {
stat.add("filter field", "x");
}
else if (filter_field_ == FIELD_Y) {
stat.add("filter field", "y");
}
stat.add("min index", min_index_);
stat.add("max index", max_index_);
jsk_topic_tools::addDiagnosticBooleanStat("keep organized",
keep_organized_,
stat);
jsk_topic_tools::addDiagnosticBooleanStat("filter_limit_negative",
filter_limit_negative_,
stat);
}
else {
jsk_recognition_utils::addDiagnosticErrorSummary(
"ClusterPointIndicesDecomposer", vital_checker_, stat);
}
}
void CrioReceiver::checkYawSensor(diagnostic_updater::DiagnosticStatusWrapper &stat) {
stat.add("Yaw Rate", diagnostics_info_.YawRate_mV / 1000.0);
stat.add("Yaw Swing", diagnostics_info_.YawSwing_mV / 1000.0);
stat.add("Yaw Temp", diagnostics_info_.YawTemp_mV / 1000.0);
stat.add("Yaw Ref", diagnostics_info_.YawRef_mV / 1000.0);
stat.add("Yaw Bias", pose_packet_.yaw_bias);
stat.add("Yaw Bias Variance", pose_packet_.yaw_bias_variance);
std::string status_msg;
unsigned char status_lvl = diagnostic_msgs::DiagnosticStatus::OK;
if (fabs(pose_packet_.yaw_bias) > 1.) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "Yaw sensor bias has diverged; ";
} else if (fabs(pose_packet_.yaw_bias) > 0.4 && fabs(pose_packet_.yaw_bias) < 1.) {
status_lvl = diagnostic_msgs::DiagnosticStatus::WARN;
status_msg += "Yaw sensor bias is outside of expected bounds; ";
} else {
status_msg += "Yaw sensor bias is okay; ";
}
if ((diagnostics_info_.YawSwing_mV / 1000.0) < 0.1) {
status_lvl = diagnostic_msgs::DiagnosticStatus::ERROR;
status_msg += "Yaw sensor seems disconnected; ";
} else {
status_msg += "Yaw sensor is connected; ";
}
stat.summary(status_lvl, status_msg);
}
// Try to load camera name, etc. Should catch gross communication failure.
void infoTest(diagnostic_updater::DiagnosticStatusWrapper& status)
{
status.name = "Info Test";
self_test_->setID(hw_id_);
std::string camera_name;
try {
cam_->getAttribute("CameraName", camera_name);
status.summary(0, "Connected to Camera");
status.add("Camera Name", camera_name);
}
catch (prosilica::ProsilicaException& e) {
status.summary(2, e.what());
}
}
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);
}
