error_publisher.h

#pragma once
 
#include <type_traits>
 
#include <rclcpp/rclcpp.hpp>
 
#include <mrs_lib/errorgraph/node_id.h>
#include <mrs_msgs/msg/errorgraph_element.hpp>
#include <mrs_lib/timer_handler.h>
 
namespace mrs_lib
{
  namespace errorgraph
  {
 
    class ErrorPublisher
    {
    public:
      using error_id_t = uint16_t;
 
    private:
      struct error_wrapper_t
      {
        std::optional<error_id_t> id;
        mrs_msgs::msg::ErrorgraphError msg;
      };
 
    public:
      ErrorPublisher(const rclcpp::Node::SharedPtr node, const rclcpp::Clock::SharedPtr clock, const std::string& node_name, const std::string& component_name,
                     const rclcpp::Rate& publish_period = rclcpp::Rate(1.0));
 
      void flushAndShutdown();
 
      void addGeneralError(const error_id_t id, const std::string& description);
 
      template <typename enum_T>
        requires(std::is_enum_v<enum_T> && sizeof(std::underlying_type_t<enum_T>) <= sizeof(error_id_t))
      void addGeneralError(const enum_T id, const std::string& description)
      {
        addGeneralError(static_cast<error_id_t>(id), description);
      }
 
      void addOneshotError(const std::string& description);
 
      void addWaitingForNodeError(const node_id_t& node_id);
 
      void addWaitingForTopicError(const std::string& topic_name);
 
    private:
      rclcpp::Node::SharedPtr node_;
      rclcpp::Clock::SharedPtr clock_;
      std::string node_name_;
      std::string component_name_;
 
      std::mutex errors_mtx_;
      std::vector<error_wrapper_t> errors_;
 
      rclcpp::CallbackGroup::SharedPtr cbkgrp_timers_;
 
      rclcpp::Publisher<mrs_msgs::msg::ErrorgraphElement>::SharedPtr publisher_;
 
      std::unique_ptr<mrs_lib::MRSTimer> timer_publisher_;
 
      void publishErrors();
    };
 
  } // namespace errorgraph
} // namespace mrs_lib