C++ DiagnosticStatusWrapper::summary方法代码示例

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