From 2e6fd18c4e35a7aa4a7a6e89ae4058bd705d3c2f Mon Sep 17 00:00:00 2001 From: jcasals <jcasals@iri.upc.edu> Date: Mon, 9 Mar 2020 16:02:55 +0100 Subject: [PATCH] Add support for ROS publishers --- include/core/yaml/parser_yaml.hpp | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp index b4dd57825..2a145aa57 100644 --- a/include/core/yaml/parser_yaml.hpp +++ b/include/core/yaml/parser_yaml.hpp @@ -187,6 +187,12 @@ class ParserYAML { std::string _sensor_name; YAML::Node n; }; + struct PublisherManager{ + std::string _subscriber; + std::string _topic; + std::string _period; + YAML::Node n; + }; std::map<std::string, std::string> _params; std::string _active_name; std::vector<ParamsInitSensor> _paramsSens; @@ -195,6 +201,7 @@ class ParserYAML { std::string _file; std::string _path_root; std::vector<SubscriberManager> _subscriber_managers; + std::vector<PublisherManager> _publisher_managers; YAML::Node problem; std::string generatePath(std::string); YAML::Node loadYAML(std::string); @@ -207,6 +214,7 @@ public: _paramsSens = std::vector<ParamsInitSensor>(); _paramsProc = std::vector<ParamsInitProcessor>(); _subscriber_managers = std::vector<SubscriberManager>(); + _publisher_managers = std::vector<PublisherManager>(); _parsing_file = std::stack<std::string>(); _file = file; if (path_root != "") { @@ -425,6 +433,22 @@ void ParserYAML::parseFirstLevel(std::string file){ } catch (YAML::InvalidNode &e) { throw std::runtime_error("Error parsing subscriber managers @" + generatePath(file) + ". Please make sure that each manager has 'package', 'type', 'topic' and 'sensor_name' entries."); } + + try { + for (const auto &kv : n_config["ROS publisher managers"]) { + WOLF_DEBUG("WHAT"); + PublisherManager pPublisherManager = {kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv}; + _publisher_managers.push_back(pPublisherManager); + map_container.push_back(std::map<std::string, std::string>({ + {"type", kv["type"].Scalar()}, + {"topic", kv["topic"].Scalar()}, + {"period", kv["period"].Scalar()}})); + } + insert_register("ROS publisher managers", wolf::converter<std::string>::convert(map_container)); + map_container.clear(); + } catch (YAML::InvalidNode &e) { + throw std::runtime_error("Error parsing publisher managers @" + generatePath(file) + ". Please make sure that each manager has 'type', 'topic' and 'period' entries."); + } } std::map<std::string,std::string> ParserYAML::getParams(){ std::map<std::string,std::string> rtn = _params; @@ -479,9 +503,8 @@ void ParserYAML::parse(){ for (auto itt : node) { std::string node_key = itt.first.as<std::string>(); - // WOLF_INFO("node_key ", node_key); if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and - node_key != "ROS subscriber managers") + node_key != "ROS subscriber managers" and node_key != "ROS publisher managers") { this->walkTreeR(itt.second, tags, node_key); } -- GitLab