fix core dumped err when loadplugin

plugins/voxel_layer.cpp

@@ -36,10 +36,8 @@
  *         David V. Lu!!
  *********************************************************************/
 #include <costmap_2d/voxel_layer.h>
-#include <costmap_2d/utils.h>
-
-#include <boost/dll/alias.hpp>
 #include <sensor_msgs/point_cloud2_iterator.h>
+#include <boost/dll/alias.hpp>
 
 #define VOXEL_BITS 16
 
@@ -59,10 +57,25 @@ void VoxelLayer::onInitialize()
   getParams();
 }
 
+VoxelLayer::~VoxelLayer()
+{}
+
 bool VoxelLayer::getParams()
 {
     try {
-        YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/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["voxel_layer"];
 
         // publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
@@ -83,12 +96,8 @@ bool VoxelLayer::getParams()
     }
 
     return true;
-
 }
 
-VoxelLayer::~VoxelLayer()
-{}
-
 void VoxelLayer::matchSize()
 {
   ObstacleLayer::matchSize();
@@ -211,14 +220,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
   //   grid_msg.resolutions.z = z_resolution_;
   //   grid_msg.header.frame_id = global_frame_;
   //   grid_msg.header.stamp = robot::Time::now();
-
-  //   ///////////////////////////////////////////
-  //   ////////////THAY THẾ PUBLISH NÀY///////////
-  //   ///////////////////////////////////////////
-  //   // voxel_pub_.publish(grid_msg);
-  //   ///////////////////////////////////////////
-  //   ///////////////////////////////////////////
-  //   ///////////////////////////////////////////
+  //   voxel_pub_.publish(grid_msg);
   // }
 
   updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
@@ -289,23 +291,18 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
 
   if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
   {
-    printf("The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
+    printf(
+        "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
         ox, oy, oz);
     return;
   }
 
-  ///////////////////////////////////////////
-  ////////////THAY THẾ PUBLISH NÀY///////////
-  ///////////////////////////////////////////
   // bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
   // if (publish_clearing_points)
   // {
     clearing_endpoints_.points.clear();
     clearing_endpoints_.points.reserve(clearing_observation_cloud_size);
   // }
-  ///////////////////////////////////////////
-  ///////////////////////////////////////////
-  ///////////////////////////////////////////
 
   // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
   double map_end_x = origin_x_ + getSizeInMetersX();
@@ -375,7 +372,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
     {
       unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
 
-      voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
+      // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
       voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
                                       unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
                                       cell_raytrace_range);
@@ -399,7 +396,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
     clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp;
     clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
 
-  //   clearing_endpoints_pub_.publish(clearing_endpoints_);
+    // clearing_endpoints_pub_.publish(clearing_endpoints_);
   // }
 }
 
@@ -461,11 +458,11 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
 }
 
 // Export factory function
-static PluginLayerPtr create_voxel_plugin() {
+static std::shared_ptr<Layer> create_voxel_plugin() {
     return std::make_shared<VoxelLayer>();
 }
 
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
-BOOST_DLL_ALIAS(create_voxel_plugin, create_plugin)
+BOOST_DLL_ALIAS(create_voxel_plugin, create_voxel_layer)
 
 }  // namespace costmap_2d