pfuclt_aux.h

Go to the documentation of this file.

#ifndef PFUCLT_AUX_H
#define PFUCLT_AUX_H

#include <ros/ros.h>
#include <vector>
#include <fstream>
#include <boost/ref.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

namespace pfuclt_omni_dataset
{
enum ORDER_TYPE
{
  ASC,
  DESC
};

typedef struct landmark_s
{
  int serial;
  float x, y;
} Landmark;

std::vector<Landmark> getLandmarks(const char* filename);

template <typename T> T calc_stdDev(const std::vector<T>& vec)
{
  using namespace boost::accumulators;

  accumulator_set<T, stats<tag::variance> > acc;
  std::for_each(vec.begin(), vec.end(), boost::bind<void>(boost::ref(acc), _1));
  return (T)sqrt(extract::variance(acc));
}

template <typename T>
std::vector<unsigned int> order_index(std::vector<T> const& values,
                                      const ORDER_TYPE order = DESC)
{
  // from http://stackoverflow.com/a/10585614 and modified
  // return sorted indices of vector values

  using namespace boost::phoenix;
  using namespace boost::phoenix::arg_names;

  std::vector<unsigned int> indices(values.size());
  int i = 0;
  std::transform(values.begin(), values.end(), indices.begin(), ref(i)++);
  if (order == DESC)
  {
    std::sort(indices.begin(), indices.end(),
              ref(values)[arg1] > ref(values)[arg2]);
  }
  else // ASC
  {
    std::sort(indices.begin(), indices.end(),
              ref(values)[arg1] < ref(values)[arg2]);
  }

  return indices;
}

template <typename T>
bool readParam(ros::NodeHandle& nh, const std::string name, T& variable)
{
  std::ostringstream oss;
  if (nh.getParam(name, variable))
  {
    oss << "Received parameter " << name << "=" << variable;
    ROS_INFO("%s", oss.str().c_str());
    return true;
  }
  else
    ROS_ERROR("Failed to receive parameter %s", name.c_str());

  return false;
}

/* @brief readParam (vectors) - reads and returns a parameter from the ROS
 * parameter
 * server
 * @param nh - reference to the ROS nodehandle that will perform the parameter
 * reading task
 * @param name - the parameter's name
 * @param variable - reference to the location where the parameter should be
 * stored as type std::vector<T>
 * @remark this function is overloaded from readParam for use with vectors
 * @return
 */
template <typename T>
bool readParam(ros::NodeHandle& nh, const std::string name,
               std::vector<T>& variable)
{
  std::ostringstream oss;
  if (nh.getParam(name, variable))
  {
    oss << "Received parameter " << name << "=[ ";
    for (typename std::vector<T>::iterator it = variable.begin();
         it != variable.end(); ++it)
    {
      if (typeid(T) == typeid(bool))
        oss << std::boolalpha << *it << " ";
      else
        oss << *it << " ";
    }
    oss << "]";

    ROS_INFO("%s", oss.str().c_str());
    return true;
  }
  else {
    ROS_ERROR("Failed to receive parameter %s", name.c_str());
    return false;
  }
}

typedef struct timeEval_s
{
  ros::Time rosPrev, rosNew;
  ros::WallTime wallPrev, wallNew;
  double diff;

  timeEval_s()
  {
    rosPrev = rosNew = ros::Time::now();
    wallPrev = wallNew = ros::WallTime::now();
    diff = 0.0;
  }

  double updateTime(ros::Time t)
  {
    rosPrev = rosNew;
    rosNew = t;

    diff = (rosNew-rosPrev).toNSec() * 1e-9;

    return diff;
  }

  double updateTime(ros::WallTime t)
  {
    wallPrev = wallNew;
    wallNew = t;

    diff = (wallNew-wallPrev).toNSec() * 1e-9;

    return diff;
  }

}TimeEval;

// end of namespace pfuclt_omni_dataset
}

#endif // PFUCLT_AUX_H