update file costmap_2d_robot and file config params

config/config.yaml

@@ -1,26 +0,0 @@
-inflation_layer:
-  enabled: true
-  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

config/costmap_params.yaml

@@ -0,0 +1,38 @@
+costmap_2d:
+  global_frame: map
+  robot_base_frame: base_link
+  rolling_window: false
+  track_unknown_space: false
+
+  plugins:
+    - name: static_layer
+      type: create_static_layer
+
+    - name: inflation_layer
+      type: create_inflation_layer
+
+    - name: obstacle_layer
+      type: create_obstacle_layer
+
+    - name: voxel_layer
+      type: create_voxel_layer
+
+  path_plugins: ./src/costmap_2d/libplugins.so
+  foot_print:
+    - [0.3, 0.3]
+    - [0.3, -0.3]
+    - [-0.3, -0.3]
+    - [-0.3, 0.3]
+
+  layer_config_file_name: layer_params 
+
+  transform_tolerance: 0.0
+  update_frequency: 0.0
+  width: 0.0
+  height: 0.0
+  resolution: 0.0
+  origin_x: 0.0
+  origin_y: 0.0
+
+  footprint_padding: 0.0
+  robot_radius: 0.0

config/layer_params.yaml

@@ -0,0 +1,46 @@
+static_layer:
+  enabled: true
+  first_map_only: false
+  subscribe_to_updates: false
+  track_unknown_space: true
+  use_maximum: false
+  lethal_cost_threshold: 100
+  unknown_cost_value: -1
+  trinary_costmap: true
+  base_frame_id: "map"
+
+inflation_layer:
+  enabled: true
+  inflate_unknown: false
+  cost_scaling_factor: 15.0
+  inflation_radius: 0.55
+
+obstacle_layer:
+  track_unknown_space: true
+  transform_tolerance: 0.2
+  topic: "map"
+  sensor_frame: laser_frame
+  observation_persistence: 0.0
+  expected_update_rate: 0.0
+  data_type: PointCloud
+  min_obstacle_height: 0.0
+  max_obstacle_height: 2.0
+  inf_is_valid: false
+  clearing: false
+  marking: true
+  obstacle_range: 2.5
+  raytrace_range: 3.0
+  footprint_clearing_enabled: true
+  combination_method: 1
+
+voxel_layer:
+  enabled: true
+  footprint_clearing_enabled: true
+  max_obstacle_height: 3.0
+  origin_z: 0.0
+  z_resolution: 0.2
+  z_voxels: 16
+  unknown_threshold: 15.0
+  mark_threshold: 0
+  combination_method: 3
+

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

plugins/directional_layer.cpp

@@ -15,10 +15,8 @@ using costmap_2d::NO_INFORMATION;
 namespace costmap_2d
 {
   DirectionalLayer::DirectionalLayer()
-  {
+  {}
-    // ros::NodeHandle nh("~/" + name_);
+  
-    // lane_mask_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/direction_zones/lanes", 1);
-  }
   DirectionalLayer::~DirectionalLayer() {}
 
   bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> plan)

plugins/inflation_layer.cpp

@@ -79,26 +79,19 @@ void InflationLayer::onInitialize()
     seen_ = NULL;
     seen_size_ = 0;
     need_reinflation_ = false;
-
+    std::string config_file_name = name_ + ".yaml";
+    std::cout << "InflationLayer: " << config_file_name << std::endl;
+    getParams(config_file_name);
   }
 
   matchSize();
 }
 
-bool InflationLayer::getParams()
+bool InflationLayer::getParams(const std::string& config_file_name)
 {
     try {
-        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["inflation_layer"];

plugins/obstacle_layer.cpp

@@ -62,26 +62,18 @@ void ObstacleLayer::onInitialize()
   current_ = true;
   stop_receiving_data_ = false;
   global_frame_ = layered_costmap_->getGlobalFrameID();
-  getParams();
+  std::string config_file_name = name_ + ".yaml";
+  getParams(config_file_name);
 }
 
 ObstacleLayer::~ObstacleLayer()
 {}
 
-bool ObstacleLayer::getParams()
+bool ObstacleLayer::getParams(const std::string& config_file_name)
 {
   try {
-      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);
@@ -94,9 +86,9 @@ bool ObstacleLayer::getParams()
       default_value_ = FREE_SPACE;
 
     double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2);
-    // get the topics that we'll subscribe to from the parameter server
+    // // get the topics that we'll subscribe to from the parameter server
-    std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
+    // std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
-    printf("    Subscribed to Topics: %s\n", topics_string.c_str());
+    // printf("Subscribed to Topics: %s\n", topics_string.c_str());
 
     // get the parameters for the specific topic
     double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2;

plugins/static_layer.cpp

@@ -65,24 +65,16 @@ void StaticLayer::onInitialize()
   current_ = true;
 
   global_frame_ = layered_costmap_->getGlobalFrameID();
-  getParams();
+  std::string config_file_name = name_ + ".yaml";
+  getParams(config_file_name);
 
 }
 
-bool StaticLayer::getParams()
+bool StaticLayer::getParams(const std::string& config_file_name)
 {
     try {
-        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);

plugins/voxel_layer.cpp

@@ -54,26 +54,18 @@ namespace costmap_2d
 void VoxelLayer::onInitialize()
 {
   ObstacleLayer::onInitialize();
-  getParams();
+  std::string config_file_name = name_ + ".yaml";
+  getParams(config_file_name);
 }
 
 VoxelLayer::~VoxelLayer()
 {}
 
-bool VoxelLayer::getParams()
+bool VoxelLayer::getParams(const std::string& config_file_name)
 {
     try {
-        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["voxel_layer"];
@@ -82,12 +74,12 @@ bool VoxelLayer::getParams()
         enabled_ = loadParam(layer, "enabled", true);
         footprint_clearing_enabled_ = loadParam(layer, "footprint_clearing_enabled", true);
         max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0);
-        size_z_ = loadParam(layer, "z_voxels", 10);
+        size_z_ = loadParam(layer, "z_voxels", 10.0);
-        origin_z_ = loadParam(layer, "origin_z", 0);
+        origin_z_ = loadParam(layer, "origin_z", 0.0);
         z_resolution_ = loadParam(layer, "z_resolution", 0.2);
-        unknown_threshold_ = loadParam(layer, "max_obstacle_height", 15);
+        unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_);
         mark_threshold_ = loadParam(layer, "mark_threshold", 0);
-        combination_method_ = loadParam(layer, "combination_method", true);
+        combination_method_ = loadParam(layer, "combination_method", 0.0);
         this->matchSize();
     } 
     catch (const YAML::BadFile& e) {

src/costmap_2d_robot.cpp

@@ -66,7 +66,8 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
     map_update_thread_(NULL),
     footprint_padding_(0.0)
 {
-  getParams("config.yaml");
+  std::string config_file_name = name_ + ".yaml";
+  getParams(config_file_name);
 
   // create a thread to handle updating the map
   stop_updates_ = false;
@@ -80,14 +81,6 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
   {
     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<< config_file_name << " file not found!" << std::endl;
-    }
 
     
     YAML::Node config = YAML::LoadFile(path_source);
@@ -99,27 +92,28 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
     robot::Time last_error = robot::Time::now();
     std::string tf_error;
     
-    // we need to make sure that the transform between the robot base frame and the global frame is available
+    // // we need to make sure that the transform between the robot base frame and the global frame is available
-    while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error))
+    // while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error))
-    {
+    // {
-      if (last_error + robot::Duration(5.0) < robot::Time::now())
+    //   if (last_error + robot::Duration(5.0) < robot::Time::now())
-      {
+    //   {
-        printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
+    //     printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
-                robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
+    //             robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
-        last_error = robot::Time::now();
+    //     last_error = robot::Time::now();
-      }
+    //   }
-      // The error string will accumulate and errors will typically be the same, so the last
+    //   // The error string will accumulate and errors will typically be the same, so the last
-      // will do for the warning above. Reset the string here to avoid accumulation.
+    //   // will do for the warning above. Reset the string here to avoid accumulation.
-      tf_error.clear();
+    //   tf_error.clear();
-    }
+    // }
   
     // check if we want a rolling window version of the costmap
-    bool rolling_window, track_unknown_space, always_send_full_costmap;
+    bool rolling_window, track_unknown_space;
     rolling_window = loadParam(layer, "rolling_window", false);
     track_unknown_space = loadParam(layer, "track_unknown_space", false);
-    always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false);
+
     
     layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
+
     struct PluginConfig {
     std::string name;
     std::string type;
@@ -136,8 +130,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
       }
     }
 
-    std::string path = loadParam(layer, "path", std::string(" "));
+    std::string path_plugins = loadParam(layer, "path_plugins", std::string(" "));
-    std::cout << "Plugin to load: " << path  << std::endl;
+    std::string layer_config_file_name = loadParam(layer, "layer_config_file_name", std::string("layer_params"));
 
     for (auto& info : my_list)
     {
@@ -146,22 +140,24 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
         // copyParentParameters(pname, type, private_nh);
         creators_.push_back(
           boost::dll::import_alias<PluginLayerPtr()>(
-          path, info.type, boost::dll::load_mode::append_decorations)
+          path_plugins, info.type, boost::dll::load_mode::append_decorations)
         );
         PluginLayerPtr plugin = creators_.back()();
         std::cout << "Plugin created: " << info.name << std::endl;
-        plugin->initialize(layered_costmap_, info.name, &tf_);
+        plugin->initialize(layered_costmap_, layer_config_file_name, &tf_);
         layered_costmap_->addPlugin(plugin);
       }
       catch (std::exception &ex)
       {
-        printf("Failed to create the s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", ex.what());
+        printf("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", info.name.c_str(), ex.what());
       }
     }
 
-    //   setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
+    std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
+    new_footprint = loadFootprint(layer["foot_print"], new_footprint);
+    setUnpaddedRobotFootprint(new_footprint);
  
-    transform_tolerance_ = loadParam(layer, "transform_tolerance", false);
+    transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
     if (map_update_thread_ != NULL)
     {
       map_update_thread_shutdown_ = true;
@@ -170,14 +166,14 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
       map_update_thread_ = NULL;
     }
     map_update_thread_shutdown_ = false;
-    double map_update_frequency = loadParam(layer, "update_frequency", false);
+    double map_update_frequency = loadParam(layer, "update_frequency", 0.0);
 
     // find size parameters
-    double map_width_meters = loadParam(layer, "width", false);
+    double map_width_meters = loadParam(layer, "width", 0.0);
-    double map_height_meters = loadParam(layer, "height", false);
+    double map_height_meters = loadParam(layer, "height", 0.0);
-    double resolution = loadParam(layer, "resolution", false);
+    double resolution = loadParam(layer, "resolution", 0.0);
-    double origin_x = loadParam(layer, "origin_x", false);
+    double origin_x = loadParam(layer, "origin_x", 0.0);
-    double origin_y = loadParam(layer, "origin_y", false);
+    double origin_y = loadParam(layer, "origin_y", 0.0);
 
   if (!layered_costmap_->isSizeLocked())
   {
@@ -195,7 +191,6 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
   }
 
   double robot_radius = loadParam(layer, "robot_radius", 0.0);
-  std::vector<geometry_msgs::Point> new_footprint;// = loadParam(layer, "footprint", 0.0);
   readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius);
 
   // only construct the thread if the frequency is positive
@@ -279,7 +274,7 @@ void Costmap2DROBOT::mapUpdateLoop(double frequency)
   while (!map_update_thread_shutdown_)
   {
     updateMap();
-
+    std::cout << "Costmap2DROBOT::mapUpdateLoop updateMap\n";
     r.sleep();
     // make sure to sleep for the remainder of our cycle time
     if (r.cycleTime() > robot::Duration(1 / frequency))

src/footprint.cpp

@@ -27,13 +27,13 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include<costmap_2d/costmap_math.h>
+#include <costmap_2d/costmap_math.h>
 #include <boost/tokenizer.hpp>
 #include <boost/foreach.hpp>
 #include <boost/algorithm/string.hpp>
 #include <costmap_2d/footprint.h>
 #include <costmap_2d/array_parser.h>
-#include<geometry_msgs/Point32.h>
+#include <geometry_msgs/Point32.h>
 
 namespace costmap_2d
 {
@@ -207,8 +207,6 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
   return true;
 }
 
-
-
 // std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
 // {
 //   std::string full_param_name;

test/static_layer_test.cpp

@@ -18,7 +18,7 @@ class CostmapTester : public testing::Test {
     costmap_2d::Costmap2DROBOT costmap_ros_;
 };
 
-CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("test_costmap", tf){}
+CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_global_params", tf){}
 
 void CostmapTester::checkConsistentCosts(){
   costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();