From 64db092d46969bfe5ef66f73c98171663a445c1e Mon Sep 17 00:00:00 2001
From: duongtd <tdd149cm@gmail.com>
Date: Tue, 2 Dec 2025 15:02:39 +0700
Subject: [PATCH] update file config and loadParam

---
 config/config.yaml       | 23 ++++++++++++++++++++++-
 src/costmap_2d_robot.cpp | 36 +++++++++++++++++++-----------------
 2 files changed, 41 insertions(+), 18 deletions(-)

diff --git a/config/config.yaml b/config/config.yaml
index d17193e..15b08f5 100644
--- a/config/config.yaml
+++ b/config/config.yaml
@@ -2,4 +2,25 @@ inflation_layer:
   enabled: true
   inflate_unknown: false
   cost_scaling_factor: 15.0
-  inflation_radius: 0.55
\ No newline at end of file
+  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
\ No newline at end of file
diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp
index dd10787..751b77d 100644
--- a/src/costmap_2d_robot.cpp
+++ b/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 PluginInfo { std::string path; std::string name; };
-    std::vector<PluginInfo> plugins_to_load = {
-        {"./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"}
+    struct PluginConfig {
+    std::string name;
+    std::string type;
     };
+    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"))
-    // {
-      // my_list = loadParam(layer, "plugins", my_list);
-      // for (int32_t i = 0; i < my_list.size(); ++i)
-      // {
-        // 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)
+    std::string path = loadParam(layer, "path", std::string(" "));
+    std::cout << "Plugin to load: " << path  << std::endl;
+
+    for (auto& info : my_list)
     {
       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;