fix core dumped err when loadplugin

plugins/inflation_layer.cpp

@@ -1,11 +1,48 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Eitan Marder-Eppstein
+ *         David V. Lu!!
+ *********************************************************************/
 #include <algorithm>
 #include <costmap_2d/inflation_layer.h>
 #include <costmap_2d/costmap_math.h>
 #include <costmap_2d/footprint.h>
-#include <costmap_2d/utils.h>
 #include <boost/thread.hpp>
 #include <boost/dll/alias.hpp>
 
+
 using costmap_2d::LETHAL_OBSTACLE;
 using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
 using costmap_2d::NO_INFORMATION;
@@ -21,7 +58,6 @@ InflationLayer::InflationLayer()
   , inflate_unknown_(false)
   , cell_inflation_radius_(0)
   , cached_cell_inflation_radius_(0)
-//   , dsrv_(NULL)
   , seen_(NULL)
   , cached_costs_(NULL)
   , cached_distances_(NULL)
@@ -44,16 +80,27 @@ void InflationLayer::onInitialize()
     seen_size_ = 0;
     need_reinflation_ = false;
 
-    getParams();
   }
 
-  InflationLayer::matchSize();
+  matchSize();
 }
 
 bool InflationLayer::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["inflation_layer"];
         double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0);
         double inflation_radius = loadParam(layer, "inflation_radius", 0.55);
@@ -146,21 +193,19 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
     return;
 
   // make sure the inflation list is empty at the beginning of the cycle (should always be true)
-  if (!inflation_cells_.empty()) {
-    throw std::runtime_error("The inflation list must be empty at the beginning of inflation");
-}
+  printf("The inflation list must be empty at the beginning of inflation\n");
 
   unsigned char* master_array = master_grid.getCharMap();
   unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
 
   if (seen_ == NULL) {
-    std::cerr <<"InflationLayer::updateCosts(): seen_ array is NULL" <<std::endl;
+    printf("InflationLayer::updateCosts(): seen_ array is NULL\n");
     seen_size_ = size_x * size_y;
     seen_ = new bool[seen_size_];
   }
   else if (seen_size_ != size_x * size_y)
   {
-    std::cerr <<"InflationLayer::updateCosts(): seen_ array size is wrong" <<std::endl;
+    printf("InflationLayer::updateCosts(): seen_ array size is wrong\n");
     delete[] seen_;
     seen_size_ = size_x * size_y;
     seen_ = new bool[seen_size_];
@@ -357,12 +402,13 @@ void InflationLayer::handleImpl(const void* data,
   printf("This function is not available!\n");
 }
 
+
 // Export factory function
-static PluginLayerPtr create_inflation_plugin() {
+static std::shared_ptr<Layer> create_inflation_plugin() {
     return std::make_shared<InflationLayer>();
 }
 
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
-BOOST_DLL_ALIAS(create_inflation_plugin, create_plugin)
+BOOST_DLL_ALIAS(create_inflation_plugin, create_inflation_layer)
 
 }  // namespace costmap_2d