예제 #1
0
	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);
	}
예제 #2
0
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);
   }
 }
예제 #4
0
  // 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");
    }
  }
예제 #5
0
파일: 3dr_radio.cpp 프로젝트: JeffsanC/uavs
	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);
	}
예제 #6
0
 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());
   }
 }
예제 #7
0
	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;
	}
예제 #8
0
	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");
		}
	}
예제 #9
0
	/* -*- 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");
	}
예제 #10
0
    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));
    }
예제 #11
0
    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);
    }
예제 #12
0
 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_);
 }
예제 #13
0
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;
  }
}
예제 #14
0
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]);
}
예제 #15
0
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");
}
예제 #16
0
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");
}
예제 #17
0
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");
}
예제 #18
0
    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);
    }
예제 #19
0
  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);
  }
예제 #20
0
    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_);
    }
예제 #21
0
    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");
	}
}
예제 #24
0
파일: gps.cpp 프로젝트: BeaglePilot/mavros
	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);
	}
예제 #25
0
 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);
 }
예제 #26
0
  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);
   }
 }
예제 #28
0
 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);
 }
예제 #29
0
  // 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());
    }
  }
예제 #30
0
    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);
    }