node.cpp

Go to the documentation of this file.

#include <memory>
#include <numeric>
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int64.h>
#include <std_msgs/Int8.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/UInt64.h>
#include <std_msgs/UInt8.h>

std::unique_ptr<ros::NodeHandle> nh;
std::unique_ptr<ros::Publisher> pub;

ros::Duration window_length;
std::vector<std::pair<ros::Time, int64_t>> time_values_i;
std::vector<std::pair<ros::Time, uint64_t>> time_values_u;
std::vector<std::pair<ros::Time, double>> time_values_d;

template <class T, class U>
void callbackI(const T &msg)
{
  const ros::Time now(ros::Time::now());
  // Add new value
  time_values_d.emplace_back(now, msg->data);

  // Remove values that are too old
  for (auto it(time_values_d.begin()); it != time_values_d.end();)
  {
    ros::Duration delta(now - it->first);
    if (delta > window_length)
      time_values_d.erase(it);
    else
      break;
  }

  // Compute and publish mean
  long long int sum(0);
  for (auto tv : time_values_d)
    sum += tv.second;
  U mean;
  mean.data = sum / time_values_d.size();
  pub->publish(mean);
}

void callbackInt8(const std_msgs::Int8ConstPtr &msg)
{
  callbackI<std_msgs::Int8ConstPtr, std_msgs::Int8>(msg);
}

void callbackInt16(const std_msgs::Int16ConstPtr &msg)
{
  callbackI<std_msgs::Int16ConstPtr, std_msgs::Int16>(msg);
}

void callbackInt32(const std_msgs::Int32ConstPtr &msg)
{
  callbackI<std_msgs::Int32ConstPtr, std_msgs::Int32>(msg);
}

void callbackInt64(const std_msgs::Int64ConstPtr &msg)
{
  callbackI<std_msgs::Int64ConstPtr, std_msgs::Int64>(msg);
}

template <class T, class U>
void callbackU(const T &msg)
{
  const ros::Time now(ros::Time::now());
  // Add new value
  time_values_d.emplace_back(now, msg->data);

  // Remove values that are too old
  for (auto it(time_values_d.begin()); it != time_values_d.end();)
  {
    ros::Duration delta(now - it->first);
    if (delta > window_length)
      time_values_d.erase(it);
    else
      break;
  }

  // Compute and publish mean
  long long unsigned sum(0);
  for (auto tv : time_values_d)
    sum += tv.second;
  U mean;
  mean.data = sum / time_values_d.size();
  pub->publish(mean);
}

void callbackUInt8(const std_msgs::UInt8ConstPtr &msg)
{
  callbackU<std_msgs::UInt8ConstPtr, std_msgs::UInt8>(msg);
}

void callbackUInt16(const std_msgs::UInt16ConstPtr &msg)
{
  callbackU<std_msgs::UInt16ConstPtr, std_msgs::UInt16>(msg);
}

void callbackUInt32(const std_msgs::UInt32ConstPtr &msg)
{
  callbackU<std_msgs::UInt32ConstPtr, std_msgs::UInt32>(msg);
}

void callbackUInt64(const std_msgs::UInt64ConstPtr &msg)
{
  callbackU<std_msgs::UInt64ConstPtr, std_msgs::UInt64>(msg);
}

template <class T, class U>
void callbackD(const T &msg)
{
  const ros::Time now(ros::Time::now());

  // Add new value
  if (!std::isnan(msg->data))
    time_values_d.emplace_back(now, msg->data);

  // Remove values that are too old
  for (auto it(time_values_d.begin()); it != time_values_d.end();)
  {
    ros::Duration delta(now - it->first);
    if (delta > window_length)
      time_values_d.erase(it);
    else
      break;
  }

  // Compute and publish mean
  long double sum(0);
  for (auto tv : time_values_d)
    sum += tv.second;
  U mean;
  mean.data = sum / time_values_d.size();
  pub->publish(mean);
}

void callbackFloat32(const std_msgs::Float32ConstPtr &msg)
{
  callbackD<std_msgs::Float32ConstPtr, std_msgs::Float32>(msg);
}

void callbackFloat64(const std_msgs::Float64ConstPtr &msg)
{
  callbackD<std_msgs::Float64ConstPtr, std_msgs::Float64>(msg);
}

int main(int argc,
         char *argv[])
{
  // FIXME Check if overflow happened and warn user

  ros::init(argc, argv, "republish");
  nh = std::make_unique<ros::NodeHandle>("~");

  std::string in_topic, out_topic;
  nh->getParam("in_topic", in_topic);
  nh->getParam("out_topic", out_topic);

  {
    double window_length_tmp(2);
    nh->getParam("window_length", window_length_tmp);
    window_length.sec = (int) window_length_tmp;
    window_length.nsec = (window_length_tmp - (int) window_length_tmp) * 1e9;
  }

  ROS_DEBUG_STREAM("Subscriber topic = " << in_topic);
  ROS_DEBUG_STREAM("Publisher topic = " << out_topic);
  ROS_DEBUG_STREAM("Window length = " << window_length.toSec() << " seconds");

  if (in_topic.empty())
  {
    ROS_ERROR_STREAM("in_topic must be specified");
    return 1;
  }

  if (out_topic.empty())
  {
    ROS_ERROR_STREAM("out_topic must be specified");
    return 1;
  }

  std::string topic_type;
  while (1)
  {
    ros::master::V_TopicInfo topics;
    if (!ros::master::getTopics(topics))
    {
      ROS_ERROR_STREAM("Error getting topics: Could not retrieve the topics names");
      return 1;
    }

    ros::master::V_TopicInfo::iterator topic =
    std::find_if(topics.begin(), topics.end(), [=](const ros::master::TopicInfo &ti)
    {
      return ti.name == in_topic;
    });

    if (topic != topics.end())
    {
      topic_type = topic->datatype;
      ROS_DEBUG_STREAM("Topic " << in_topic << " " << "type is " << topic_type);
      break;
    }

    ROS_WARN_STREAM_THROTTLE(2, "Waiting for topic " << in_topic << " to be available");
    ros::Duration(0.1).sleep();

    if (!nh->ok())
      return 0;
  }

  ros::Subscriber sub;
  if (topic_type == "std_msgs/Float32")
  {
    sub = nh->subscribe(in_topic, 5, &callbackFloat32);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::Float32>(out_topic, 5));
  }
  else if (topic_type == "std_msgs/Float64")
  {
    sub = nh->subscribe(in_topic, 5, &callbackFloat64);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::Float64>(out_topic, 5));
  }
  else if (topic_type == "std_msgs/Int8")
  {
    sub = nh->subscribe(in_topic, 5, &callbackInt8);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::Int8>(out_topic, 5));
  }
  else if (topic_type == "std_msgs/Int16")
  {
    sub = nh->subscribe(in_topic, 5, &callbackInt16);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::Int16>(out_topic, 5));
  }
  else if (topic_type == "std_msgs/Int32")
  {
    sub = nh->subscribe(in_topic, 5, &callbackInt32);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::Int32>(out_topic, 5));
  }
  else if (topic_type == "std_msgs/Int64")
  {
    sub = nh->subscribe(in_topic, 5, &callbackInt64);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::Int64>(out_topic, 5));
  }
  else if (topic_type == "std_msgs/UInt8")
  {
    sub = nh->subscribe(in_topic, 5, &callbackUInt8);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::UInt8>(out_topic, 5));
  }
  else if (topic_type == "std_msgs/UInt16")
  {
    sub = nh->subscribe(in_topic, 5, &callbackUInt16);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::UInt16>(out_topic, 5));
  }
  else if (topic_type == "std_msgs/UInt32")
  {
    sub = nh->subscribe(in_topic, 5, &callbackUInt32);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::UInt32>(out_topic, 5));
  }
  else if (topic_type == "std_msgs/UInt64")
  {
    sub = nh->subscribe(in_topic, 5, &callbackUInt64);
    pub = std::make_unique<ros::Publisher>(nh->advertise<std_msgs::UInt64>(out_topic, 5));
  }
  else
  {
    ROS_ERROR_STREAM("Topic type " << topic_type << " is not supported");
    return 1;
  }

  ros::AsyncSpinner spinner(1);
  spinner.start();
  ros::waitForShutdown();
  return 0;
}