Program Listing for File diagnostic_status_wrapper.hpp

↰ Return to documentation for file (/tmp/ws/src/diagnostics/diagnostic_updater/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/rclcpp.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_