void diagnoseConnections(diagnostic_updater::DiagnosticStatusWrapper &status) {
if (!fadeToColorClient)
status.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR,
"Not all required services are connected.");
else
status.summaryf(diagnostic_msgs::DiagnosticStatus::OK,
"All required services are connected.");
}
void CANPriusNode::diagnoseCANConnection(
diagnostic_updater::DiagnosticStatusWrapper& status) {
if (_canConnection && _canConnection->isOpen())
status.summaryf(diagnostic_msgs::DiagnosticStatus::OK,
"CAN connection opened on %s.",
_canConnection->getDevicePathStr().c_str());
else
status.summaryf(diagnostic_msgs::DiagnosticStatus::ERROR,
"CAN connection closed on %s.", _canDeviceStr.c_str());
}
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 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.");
}
}
/**
* 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);
}
}
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 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 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 AnalogInputTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "[%d, %d, %d, %d]",
values[0], values[1], values[2], values[3]);
}
void DigitalInputTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "[%d, %d, %d, %d]",
status & 0x08?1:0, status & 0x04?1:0,
status & 0x02?1:0, status & 0x01?1:0);
}
void GyroSensorTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
// Raw data angles are in hundredths of degree
stat.summaryf(diagnostic_msgs::DiagnosticStatus::OK, "Heading: %.2f degrees", heading/100.0);
}
