76 lines
2.6 KiB
C++
Executable File
76 lines
2.6 KiB
C++
Executable File
#include <diagnostic_msgs/DiagnosticArray.h>
|
|
#include <diagnostic_msgs/DiagnosticStatus.h>
|
|
#include "md_controller/diagnostic.h"
|
|
|
|
|
|
/**
|
|
* g_diagnostic_handler: singleton, implements the diagnostic for sick_line_guidance
|
|
*/
|
|
MD::Diagnostic::DiagnosticImpl MD::Diagnostic::g_diagnostic_handler;
|
|
|
|
|
|
/*
|
|
* Constructor.
|
|
*/
|
|
MD::Diagnostic::DiagnosticImpl::DiagnosticImpl() : m_diagnostic_initialized(false), m_diagnostic_component("")
|
|
{
|
|
}
|
|
|
|
/*
|
|
* Initialization.
|
|
*
|
|
* @param[in] nh ros::NodeHandle
|
|
* @param[in] publish_topic ros topic to publish diagnostic messages
|
|
* @param[in] component description of the component reporting
|
|
*/
|
|
void MD::Diagnostic::DiagnosticImpl::init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component)
|
|
{
|
|
m_diagnostic_publisher = nh.advertise<diagnostic_msgs::DiagnosticArray>(publish_topic, 1);
|
|
m_diagnostic_component = component;
|
|
m_diagnostic_initialized = true;
|
|
}
|
|
|
|
/*
|
|
* Updates and reports the current status.
|
|
*
|
|
* @param[in] status current status (OK, ERROR, ...)
|
|
* @param[in] message optional diagnostic message
|
|
*/
|
|
void MD::Diagnostic::DiagnosticImpl::update(DIAGNOSTIC_STATUS status, const std::string &message)
|
|
{
|
|
if (m_diagnostic_initialized)
|
|
{
|
|
static std::map<DIAGNOSTIC_STATUS, std::string> status_description = {
|
|
{DIAGNOSTIC_STATUS::OK, "OK"},
|
|
{DIAGNOSTIC_STATUS::EXIT, "EXIT"},
|
|
{DIAGNOSTIC_STATUS::ERROR_STATUS, "ERROR_STATUS"},
|
|
{DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "CONFIGURATION_ERROR"},
|
|
{DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "INITIALIZATION_ERROR"},
|
|
{DIAGNOSTIC_STATUS::INTERNAL_ERROR, "INTERNAL_ERROR"}
|
|
};
|
|
|
|
// create DiagnosticStatus
|
|
diagnostic_msgs::DiagnosticStatus msg;
|
|
msg.level = (status == DIAGNOSTIC_STATUS::OK) ? (diagnostic_msgs::DiagnosticStatus::OK) : (diagnostic_msgs::DiagnosticStatus::ERROR); // Level of operation
|
|
msg.name = m_diagnostic_component; // description of the test/component reporting
|
|
msg.hardware_id = ""; // hardware unique string (tbd)
|
|
msg.values.clear(); // array of values associated with the status
|
|
// description of the status
|
|
msg.message = status_description[status];
|
|
if(msg.message.empty())
|
|
{
|
|
msg.message = "ERROR";
|
|
}
|
|
if (!message.empty())
|
|
{
|
|
msg.message = msg.message + ": " + message;
|
|
}
|
|
// publish DiagnosticStatus in DiagnosticArray
|
|
diagnostic_msgs::DiagnosticArray msg_array;
|
|
msg_array.header.stamp = ros::Time::now();
|
|
msg_array.header.frame_id = "";
|
|
msg_array.status.clear();
|
|
msg_array.status.push_back(msg);
|
|
m_diagnostic_publisher.publish(msg_array);
|
|
}
|
|
} |