Add support for ROS publishers

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);
                                         }