ros-parameter-manager.hpp

Go to the documentation of this file.

 
#ifndef RUSH_ROS_PARAMETER_MANAGER_HPP
#define RUSH_ROS_PARAMETER_MANAGER_HPP
 
#include <algorithm>
#include <chrono>
#include <iosfwd>
#include <iterator>
#include <map>
#include <ros/param.h>
#include <ros/this_node.h>
#include <set>
#include <stdexcept>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#include <xmlrpcpp/XmlRpcValue.h>
 
namespace rush::ros {
 
class ParamValue {
public:
  ParamValue() = default;
  explicit ParamValue(const XmlRpc::XmlRpcValue &v) : value_{v} {};
 
  template <typename T>
  T as() {
    return static_cast<T>(value_);
  }
 
  template <typename T>
  void as(T &x) {
    x = static_cast<T>(value_);
  }
 
  template <typename T>
  std::vector<T> as_vec() {
    std::vector<T> ret;
    as_vec(ret);
    return ret;
  }
 
  template <typename T>
  void as_vec(std::vector<T> &x) {
    if(!x.empty()) {
      x.clear();
    }
    int i = 0;
    while(i < value_.size()) {
      x.push_back(static_cast<T>(value_[i++]));
    }
  }
 
  friend std::ostream &operator<<(std::ostream &os, const ParamValue &obj) {
    os << obj.value_;
    return os;
  }
 
private:
  XmlRpc::XmlRpcValue value_;
};
 
class ParamMapper : public std::map<std::string, ParamValue> {
public:
  ParamMapper() = default;
 
  explicit ParamMapper(const std::string &ns) {
    load(ns);
  }
 
  ParamValue &operator[](const std::string &key) {
    if(std::map<std::string, ParamValue>::count(key) < 1) {
      throw std::runtime_error("Key " + key + " not found");
    }
    return std::map<std::string, ParamValue>::operator[](key);
  }
 
  void load(std::string ns = "") {
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
    ns_.insert(ns);
 
    std::vector<std::string> names;
    XmlRpc::XmlRpcValue v;
    if(ns.empty() || ns.back() != '/') {
      ns += '/';
    }
    if(ns.front() != '/') {
      ns = ::ros::this_node::getNamespace() + ns;
    }
 
    ::ros::param::getParamNames(names);
    names.erase(std::remove_if(names.begin(), names.end(), [&ns](const std::string &s) { return static_cast<bool>(s.compare(0, ns.size(), ns)); }), names.end());
 
    for(const std::string &n : names) {
      ::ros::param::get(n, v);
      const std::string key = n.substr(ns.size());
      if(std::map<std::string, ParamValue>::count(key) < 1) {
        std::map<std::string, ParamValue>::emplace(std::make_pair(key, v));
      } else {
        std::map<std::string, ParamValue>::operator[](key) = ParamValue(v);
      }
    }
  }
 
  void reload() {
    std::map<std::string, ParamValue>::clear();
    for(const std::string &ns : ns_) {
      load(ns);
    }
  }
 
  [[nodiscard]] std::vector<std::string> getKeys() const {
    if(this->empty()) {
      return {};
    }
    std::vector<std::string> keys;
    keys.reserve(this->size());
    std::transform(this->begin(), this->end(), std::back_inserter(keys), [](const auto &pair) { return pair.first; });
    return keys;
  }
 
private:
  std::set<std::string> ns_;
};
 
} // namespace rush::ros
 
#endif // RUSH_ROS_PARAMETER_MANAGER_HPP