Program Listing for File diagnostic_status_wrapper.hpp
↰ Return to documentation for file (include/diagnostic_updater/diagnostic_status_wrapper.hpp
)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef DIAGNOSTIC_UPDATER__DIAGNOSTIC_STATUS_WRAPPER_HPP_
#define DIAGNOSTIC_UPDATER__DIAGNOSTIC_STATUS_WRAPPER_HPP_
#include <stdarg.h>
#include <cstdio>
#include <sstream>
#include <string>
#include <vector>
#include "diagnostic_msgs/msg/diagnostic_status.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/logging.hpp"
namespace diagnostic_updater
{
class DiagnosticStatusWrapper : public diagnostic_msgs::msg::DiagnosticStatus
{
public:
DiagnosticStatusWrapper()
: debug_logger_(rclcpp::get_logger("diagnostics_wrapper_logger"))
{}
explicit DiagnosticStatusWrapper(const DiagnosticStatusWrapper & other) = delete;
void summary(unsigned char lvl, const std::string s)
{
level = lvl;
message = s;
}
void mergeSummary(unsigned char lvl, const std::string s)
{
if ((lvl > 0) && (level > 0)) {
if (!message.empty()) {
message += "; ";
message += s;
}
} else if (lvl > level) {
message = s;
}
if (lvl > level) {
level = lvl;
}
}
void mergeSummary(const diagnostic_msgs::msg::DiagnosticStatus & src)
{
mergeSummary(src.level, src.message);
}
void mergeSummaryf(unsigned char lvl, const char * format, ...)
{
va_list va;
const int kBufferSize = 1000;
char buff[kBufferSize]; // @todo This could be done more elegantly.
va_start(va, format);
if (vsnprintf(buff, sizeof(buff), format, va) >= kBufferSize) {
RCLCPP_DEBUG(
debug_logger_,
"Really long string in DiagnosticStatusWrapper::mergeSummaryf, it was truncated.");
}
std::string value = std::string(buff);
mergeSummary(lvl, value);
va_end(va);
}
void summaryf(unsigned char lvl, const char * format, ...)
{
va_list va;
const int kBufferSize = 1000;
char buff[kBufferSize];
va_start(va, format);
if (vsnprintf(buff, sizeof(buff), format, va) >= kBufferSize) {
RCLCPP_DEBUG(
debug_logger_,
"Really long string in DiagnosticStatusWrapper::summaryf, it was truncated.");
}
std::string value = std::string(buff);
summary(lvl, value);
va_end(va);
}
void clearSummary() {summary(0, "");}
void summary(const diagnostic_msgs::msg::DiagnosticStatus & src)
{
summary(src.level, src.message);
}
template<class T>
void add(const std::string & key, const T & val)
{
std::stringstream ss;
ss << val;
std::string sval = ss.str();
add(key, sval);
}
void addf(const std::string & key, const char * format, ...);
void clear() {values.clear();}
private:
rclcpp::Logger debug_logger_;
};
template<>
inline void DiagnosticStatusWrapper::add<std::string>(
const std::string & key,
const std::string & s)
{
diagnostic_msgs::msg::KeyValue ds;
ds.key = key;
ds.value = s;
values.push_back(ds);
}
// /\brief For bool, diagnostic value is "True" or "False"
template<>
inline void DiagnosticStatusWrapper::add<bool>(
const std::string & key,
const bool & b)
{
diagnostic_msgs::msg::KeyValue ds;
ds.key = key;
ds.value = b ? "True" : "False";
values.push_back(ds);
}
// Need to place addf after DiagnosticStatusWrapper::add<std::string> or
// gcc complains that the specialization occurs after instantiation.
inline void
DiagnosticStatusWrapper::addf(
const std::string & key, const char * format, ...) // In practice format will always be a char *
{
va_list va;
const int kBufferSize = 1000;
char buff[kBufferSize]; // @todo This could be done more elegantly.
va_start(va, format);
if (vsnprintf(buff, sizeof(buff), format, va) >= kBufferSize) {
RCLCPP_DEBUG(
debug_logger_,
"Really long string in DiagnosticStatusWrapper::addf, it was truncated.");
}
std::string value = std::string(buff);
add(key, value);
va_end(va);
}
} // namespace diagnostic_updater
#endif // DIAGNOSTIC_UPDATER__DIAGNOSTIC_STATUS_WRAPPER_HPP_