utils.h

Go to the documentation of this file.

 
#ifndef GAZEBO_VIDEO_MONITOR_PLUGINS_INTERNAL_UTILS_H
#define GAZEBO_VIDEO_MONITOR_PLUGINS_INTERNAL_UTILS_H
 
#include <cxxabi.h>
#include <chrono>
#include <ctime>
#include <memory>
#include <sstream>
#include <string>
#include <vector>
 
#include <boost/filesystem/operations.hpp>
 
#include <ros/ros.h>
 
#include <sdf/Element.hh>
 
#include <gazebo_video_monitor_plugins/internal/types.h>
 
template <typename ClassName>
static std::string getClassName() {
  int status;
  std::string name = std::string(
      abi::__cxa_demangle(typeid(ClassName).name(), nullptr, nullptr, &status));
  auto pos = name.rfind("::");
  if (pos == std::string::npos) return name;
  return name.substr(name.rfind("::") + 2);
}
 
static bool createDirectory(const boost::filesystem::path &path) {
  if (not boost::filesystem::exists(path)) {
    if (not boost::filesystem::create_directory(path)) return false;
    ROS_INFO_STREAM(path << " directory has been created");
  }
  return true;
}
 
namespace internal {
 
static void parseRefModelConfig(const sdf::ElementPtr &sdf,
                                const RefModelConfigPtr &config) {
  if (sdf->HasElement("model") and config->model_name.empty())
    config->model_name = sdf->Get<std::string>("model");
  if (sdf->HasElement("link"))
    config->link_name = sdf->Get<std::string>("link");
}
 
}  // namespace internal
 
static RefModelConfigConstPtr parseRefModelConfig(const sdf::ElementPtr &sdf) {
  RefModelConfigPtr config = std::make_shared<RefModelConfig>();
  internal::parseRefModelConfig(sdf, config);
  return std::move(config);
}
 
static RefModelConfigConstPtr parseRefModelConfig(const sdf::ElementPtr &sdf,
                                                  ros::NodeHandle &nh) {
  RefModelConfigPtr config = std::make_shared<RefModelConfig>();
  if (sdf->HasElement("modelParam")) {
    auto model_param = sdf->Get<std::string>("modelParam");
    if (not nh.getParam(model_param, config->model_name))
      ROS_WARN_STREAM("Failed to retrieve " << model_param << " parameter");
  }
  internal::parseRefModelConfig(sdf, config);
  return std::move(config);
}
 
static std::string getStringTimestamp(std::time_t t) {
  std::tm tm = *std::localtime(&t);
  std::stringstream ss;
  ss << std::put_time(&tm, "%Y-%m-%d-%H-%M-%S");
  return ss.str();
}
 
static std::string toString(const std::vector<std::string> &names,
                            const std::string &delimiter = ", ") {
  std::stringstream ss;
  for (size_t i = 0; i < names.size(); ++i) {
    ss << names[i];
    if (i < names.size() - 1) ss << delimiter;
  }
  return ss.str();
}
 
#endif  // GAZEBO_VIDEO_MONITOR_PLUGINS_INTERNAL_UTILS_H