add file costmap_2d_robot

plugins/static_layer.cpp

@@ -6,7 +6,9 @@
 #include <tf3/convert.h>
 #include <tf3/utils.h>
 #include <boost/dll/alias.hpp>
-
+#include <filesystem>
+#include <string>
+#include <iostream>
 
 using costmap_2d::NO_INFORMATION;
 using costmap_2d::LETHAL_OBSTACLE;
@@ -35,7 +37,22 @@ void StaticLayer::onInitialize()
 bool StaticLayer::getParams()
 {
     try {
-        YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml");
+        std::string config_file_name = "config.yaml";
+        std::string folder = COSTMAP_2D_DIR;
+
+
+        std::string path_source = getSourceFile(folder,config_file_name);
+        if(path_source != " ")
+        {
+          std::cout << "Path source: " << path_source << std::endl;
+        }
+        else
+        {
+          std::cout << "/cfg folder not found!" << std::endl;
+        }
+
+        
+        YAML::Node config = YAML::LoadFile(path_source);
         YAML::Node layer = config["static_layer"];
 
 
@@ -299,9 +316,9 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
       return;
     }
     // Copy map data given proper transformations
-    tf3::Transform tf2_transform;
-    tf2_transform = convertToTf2Transform(transformMsg.transform);
-    // tf3::convert(transformMsg.transform, tf2_transform);
+    tf3::Transform tf3_transform;
+    tf3_transform = convertTotf3Transform(transformMsg.transform);
+    // tf3::convert(transformMsg.transform, tf3_transform);
     for (unsigned int i = min_i; i < max_i; ++i)
     {
       for (unsigned int j = min_j; j < max_j; ++j)
@@ -310,7 +327,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
         layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
         // Transform from global_frame_ to map_frame_
         tf3::Vector3 p(wx, wy, 0);
-        p = tf2_transform*p;
+        p = tf3_transform*p;
         // Set master_grid with cell from map
         if (worldToMap(p.x(), p.y(), mx, my))
         {