DebugDataPublisher.h

Go to the documentation of this file.

//
// Created by wk on 2026/3/2.
//
#pragma once
#include <ros/ros.h>
#include <rm_msgs/DebugData.h>
#include <unordered_map>
#include <string>
 
class DebugDataPublisher
{
public:
  explicit DebugDataPublisher(ros::NodeHandle& nh, const std::string& topic = "debug_data", int queue_size = 5)
  {
    debug_data_pub_ = nh.advertise<rm_msgs::DebugData>(topic, queue_size);
  }
 
  void add(const std::string& name, double value)
  {
    auto it = index_map_.find(name);
    if (it != index_map_.end())
    {
      // Update existing entry
      debug_data_.stamp[it->second] = ros::Time::now();
      debug_data_.value[it->second] = value;
    }
    else
    {
      // Insert new entry
      index_map_[name] = debug_data_.name.size();
      debug_data_.stamp.push_back(ros::Time::now());
      debug_data_.name.push_back(name);
      debug_data_.value.push_back(value);
    }
  }
 
  void add(const std::string& name, double value, const ros::Time& stamp)
  {
    auto it = index_map_.find(name);
    if (it != index_map_.end())
    {
      debug_data_.stamp[it->second] = stamp;
      debug_data_.value[it->second] = value;
    }
    else
    {
      index_map_[name] = debug_data_.name.size();
      debug_data_.stamp.push_back(stamp);
      debug_data_.name.push_back(name);
      debug_data_.value.push_back(value);
    }
  }
 
  void publish()
  {
    if (!debug_data_.name.empty())
    {
      debug_data_pub_.publish(debug_data_);
    }
    clear();
  }
 
  void publishAndKeep()
  {
    if (!debug_data_.name.empty())
    {
      debug_data_pub_.publish(debug_data_);
    }
  }
 
  void clear()
  {
    debug_data_.stamp.clear();
    debug_data_.name.clear();
    debug_data_.value.clear();
    index_map_.clear();
  }
 
private:
  ros::Publisher debug_data_pub_;                      
  rm_msgs::DebugData debug_data_;                      
  std::unordered_map<std::string, size_t> index_map_;  
};