Program Listing for File status_item.hpp

↰ Return to documentation for file (/tmp/ws/src/diagnostics/diagnostic_aggregator/include/diagnostic_aggregator/status_item.hpp)

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, 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_AGGREGATOR__STATUS_ITEM_HPP_
#define DIAGNOSTIC_AGGREGATOR__STATUS_ITEM_HPP_

#include <map>
#include <memory>
#include <string>
#include <vector>

#include "diagnostic_aggregator/visibility_control.hpp"

#include "diagnostic_msgs/msg/diagnostic_status.hpp"
#include "diagnostic_msgs/msg/key_value.hpp"

#include "rclcpp/rclcpp.hpp"

namespace diagnostic_aggregator
{
inline std::string getOutputName(const std::string & item_name)
{
  std::string output_name = item_name;
  std::string slash_str = "/";
  std::string::size_type pos = 0;
  while ((pos = output_name.find(slash_str, pos)) != std::string::npos) {
    output_name.replace(pos, slash_str.size(), " ");
    pos++;
  }

  return output_name;
}

enum DiagnosticLevel
{
  Level_OK = diagnostic_msgs::msg::DiagnosticStatus::OK,
  Level_Warn = diagnostic_msgs::msg::DiagnosticStatus::WARN,
  Level_Error = diagnostic_msgs::msg::DiagnosticStatus::ERROR,
  Level_Stale = diagnostic_msgs::msg::DiagnosticStatus::STALE
};

inline DiagnosticLevel valToLevel(const int val)
{
  if (val == diagnostic_msgs::msg::DiagnosticStatus::OK) {
    return Level_OK;
  }
  if (val == diagnostic_msgs::msg::DiagnosticStatus::WARN) {
    return Level_Warn;
  }
  if (val == diagnostic_msgs::msg::DiagnosticStatus::ERROR) {
    return Level_Error;
  }
  if (val == diagnostic_msgs::msg::DiagnosticStatus::STALE) {
    return Level_Stale;
  }

  RCLCPP_ERROR(
    rclcpp::get_logger(
      "generic_analyzer_base"),
    R"(Attempting to convert %d into DiagnosticLevel.
    Values are: {0: OK, 1: Warning, 2: Error, 3: Stale})",
    val);
  return Level_Error;
}

inline std::string valToMsg(const int val)
{
  if (val == diagnostic_msgs::msg::DiagnosticStatus::OK) {
    return "OK";
  }
  if (val == diagnostic_msgs::msg::DiagnosticStatus::WARN) {
    return "Warning";
  }
  if (val == diagnostic_msgs::msg::DiagnosticStatus::ERROR) {
    return "Error";
  }
  if (val == diagnostic_msgs::msg::DiagnosticStatus::STALE) {
    return "Stale";
  }

  RCLCPP_ERROR(
    rclcpp::get_logger(
      "generic_analyzer_base"),
    R"(Attempting to convert diagnostic level %d into string.
    Values are: {0: \"OK\", 1: \"Warning\", 2: \"Error\", 3: \"Stale\"})",
    val);
  return "Error";
}

inline std::string removeLeadingNameChaff(const std::string & input_name, const std::string & chaff)
{
  std::string output_name = input_name;

  if (chaff.empty()) {
    return output_name;
  }

  // Remove start name from all output names
  // Turns "/PREFIX/base_hokuyo_node: Connection Status" to "/PREFIX/Connection Status"
  std::size_t last_slash = output_name.rfind("/");
  std::string start_of_name = output_name.substr(0, last_slash) + std::string("/");

  if (output_name.find(chaff) == last_slash + 1) {
    output_name.replace(last_slash + 1, chaff.size(), "");
  }

  if (output_name.find(":", last_slash) == last_slash + 1) {
    output_name = start_of_name + output_name.substr(last_slash + 2);
  }

  while (output_name.find(" ", last_slash) == last_slash + 1) {
    output_name = start_of_name + output_name.substr(last_slash + 2);
  }

  return output_name;
}

class StatusItem
{
public:
  DIAGNOSTIC_AGGREGATOR_PUBLIC
  explicit StatusItem(const diagnostic_msgs::msg::DiagnosticStatus * status);

  DIAGNOSTIC_AGGREGATOR_PUBLIC
  StatusItem(
    const std::string item_name, const std::string message = "Missing",
    const DiagnosticLevel level = Level_Stale);

  DIAGNOSTIC_AGGREGATOR_PUBLIC
  ~StatusItem();

  DIAGNOSTIC_AGGREGATOR_PUBLIC
  bool update(const diagnostic_msgs::msg::DiagnosticStatus * status);

  DIAGNOSTIC_AGGREGATOR_PUBLIC
  std::shared_ptr<diagnostic_msgs::msg::DiagnosticStatus> toStatusMsg(
    const std::string & path, const bool stale = false) const;

  /*
   *\brief Returns level of DiagnosticStatus message
   */
  DiagnosticLevel getLevel() const {return level_;}

  std::string getMessage() const {return message_;}

  std::string getName() const {return name_;}

  std::string getHwId() const {return hw_id_;}

  const rclcpp::Time getLastUpdateTime() const {return update_time_;}

  bool hasKey(const std::string & key) const
  {
    for (unsigned int i = 0; i < values_.size(); ++i) {
      if (values_[i].key == key) {
        return true;
      }
    }

    return false;
  }

  std::string getValue(const std::string & key) const
  {
    for (unsigned int i = 0; i < values_.size(); ++i) {
      if (values_[i].key == key) {
        return values_[i].value;
      }
    }

    return std::string("");
  }

private:
  rclcpp::Time update_time_;
  rclcpp::Clock::SharedPtr clock_;

  DiagnosticLevel level_;
  std::string output_name_;
  std::string name_;
  std::string message_;
  std::string hw_id_;
  std::vector<diagnostic_msgs::msg::KeyValue> values_;
};

}  // namespace diagnostic_aggregator

#endif  // DIAGNOSTIC_AGGREGATOR__STATUS_ITEM_HPP_