update file costmap_2d_robot and file config params

include/costmap_2d/inflation_layer.h

@@ -187,7 +187,7 @@ private:
   double** cached_distances_;
   double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
 
-  bool getParams();
+  bool getParams(const std::string& config_file_name);
 
   bool need_reinflation_;  ///< Indicates that the entire costmap should be reinflated next time around.
 };

include/costmap_2d/obstacle_layer.h

@@ -164,7 +164,7 @@ protected:
   int combination_method_;
 
 private:
-  bool getParams();
+  bool getParams(const std::string& config_file_name);
 };
 
 }  // namespace costmap_2d

include/costmap_2d/static_layer.h

@@ -88,7 +88,7 @@ protected:
   unsigned char lethal_threshold_, unknown_cost_value_;
   
 private:
-  bool getParams();
+  bool getParams(const std::string& config_file_name);
 
 };
 

include/costmap_2d/utils.h

@@ -15,6 +15,25 @@ namespace costmap_2d
         return default_value;
     }
 
+    inline std::vector<geometry_msgs::Point> loadFootprint(const YAML::Node& node, const std::vector<geometry_msgs::Point>& default_value)
+    {
+        if( !node || !node.IsDefined())
+            return default_value;
+
+        std::vector<geometry_msgs::Point> fp;
+
+        for (const auto& p : node) {
+            if (p.size() != 2)
+                throw std::runtime_error("Footprint point must be [x, y]");
+
+            fp.push_back(geometry_msgs::Point{p[0].as<double>(), p[1].as<double>()});
+        }
+        std::cout << "Loaded footprint with " << fp.size() << " points." << std::endl;
+
+        return fp;
+    }
+
+
     inline std::string getSourceFile(const std::string& root, const std::string& config_file_name)
     {
         std::string path_source = " ";
@@ -30,11 +49,13 @@ namespace costmap_2d
                 if (entry.is_regular_file() && entry.path().filename() == config_file_name)
                 {
                     path_source = entry.path().string();
+                    std::cout << "Path source: " << path_source << std::endl;
                     break;  // tìm thấy thì dừng
                 }
             }
         }
-        
+        if(path_source == " ")
+            std::cout<< config_file_name << " file not found at path: "<< cfg_dir << std::endl;
         return path_source;
     }
 

include/costmap_2d/voxel_layer.h

@@ -83,7 +83,7 @@ protected:
   virtual void resetMaps();
 
 private:
-  bool getParams();
+  bool getParams(const std::string& config_file_name);
   void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
   virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y,
                                  double* max_x, double* max_y);