update file config and loadParam

config/config.yaml

@@ -3,3 +3,24 @@ inflation_layer:
   inflate_unknown: false
   cost_scaling_factor: 15.0
   inflation_radius: 0.55
+
+costmap_2d:
+  plugins:
+    - name: static_layer
+      type: create_static_layer
+
+    - name: inflation_layer
+      type: create_inflation_layer
+
+    - name: obstacle_layer
+      type: create_obstacle_layer
+
+    - name: preferred_layer
+      type: create_preferred_layer
+
+  path: ./src/costmap_2d/libplugins.so
+
+  # robot_time_source: true
+  # update_frequency: 5.0
+  # publish_frequency: 2.0
+  # transform_tolerance: 0.5

src/costmap_2d_robot.cpp

@@ -120,31 +120,33 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
     always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false);
     
     layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
-
+    struct PluginConfig {
-    struct PluginInfo { std::string path; std::string name; };
+    std::string name;
-    std::vector<PluginInfo> plugins_to_load = {
+    std::string type;
-        {"./src/costmap_2d/libplugins.so", "create_static_layer"},
-        {"./src/costmap_2d/libplugins.so", "create_inflation_layer"},
-        {"./src/costmap_2d/libplugins.so", "create_obstacle_layer"},
-        {"./src/costmap_2d/libplugins.so", "create_preferred_layer"}
     };
+    std::vector<PluginConfig> my_list;
+    if (layer["plugins"] && layer["plugins"].IsSequence()) 
+    {
+      for (const auto& plugin_node : layer["plugins"]) {
+          PluginConfig p;
+          p.name = loadParam(plugin_node, "name", std::string(" "));
+          p.type = loadParam(plugin_node, "type", std::string(" "));
+          my_list.push_back(p);
+          std::cout << "Plugin to load: " << p.name << ", Type: " << p.type << std::endl;
+      }
+    }
 
-    // if (private_nh.hasParam("plugins"))
+    std::string path = loadParam(layer, "path", std::string(" "));
-    // {
+    std::cout << "Plugin to load: " << path  << std::endl;
-      // my_list = loadParam(layer, "plugins", my_list);
+
-      // for (int32_t i = 0; i < my_list.size(); ++i)
+    for (auto& info : my_list)
-      // {
-        // std::string pname = static_cast<std::string>(my_list[i]["name"]);
-        // std::string type = static_cast<std::string>(my_list[i]["type"]);
-        // printf("%s: Using plugin \"%s\" with type %s\n", name_.c_str(), pname.c_str(), type.c_str());
-    for (auto& info : plugins_to_load)
     {
       try
       {
         // copyParentParameters(pname, type, private_nh);
         creators_.push_back(
           boost::dll::import_alias<PluginLayerPtr()>(
-          info.path, info.name, boost::dll::load_mode::append_decorations)
+          path, info.type, boost::dll::load_mode::append_decorations)
         );
         PluginLayerPtr plugin = creators_.back()();
         std::cout << "Plugin created: " << info.name << std::endl;