fix core dumped err when loadplugin

plugins/static_layer.cpp

@@ -1,16 +1,50 @@
-
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, 2013, Willow Garage, Inc.
+ *  Copyright (c) 2015, Fetch Robotics, 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 <costmap_2d/static_layer.h>
 #include <costmap_2d/costmap_math.h>
-#include <data_convert/data_convert.h>
-#include <costmap_2d/utils.h>
+
 #include <tf3/convert.h>
-#include <tf3/utils.h>
-#include <boost/dll/alias.hpp>
-#include <filesystem>
-#include <string>
-#include <iostream>
 #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
 
+#include <boost/dll/alias.hpp>
+
+
 using costmap_2d::NO_INFORMATION;
 using costmap_2d::LETHAL_OBSTACLE;
 using costmap_2d::FREE_SPACE;
@@ -137,7 +171,6 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
   if(!map_shutdown_)
   {
     std::cout << "Received new map!" << std::endl;
-
     unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
 
     printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
@@ -188,18 +221,17 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
     height_ = size_y_;
     map_received_ = true;
     has_updated_data_ = true;
-    
   }
   else
   {
     printf("Stop receive new map!");
   }
+
   // shutdown the map subscrber if firt_map_only_ flag is on
   if (first_map_only_)
   {
-    printf("Shutting down the map subscriber. first_map_only flag is on");
+    printf("Shutting down the map subscriber. first_map_only flag is on\n");
     map_shutdown_ = true;
-    // map_sub_.shutdown();
   }
 }
 
@@ -302,12 +334,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
     tf3::TransformStampedMsg transformMsg;
     try
     {
-      // transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
-      bool status =tf_->canTransform(map_frame_, global_frame_, tf3::Time());
-      if(!status) throw tf3::TransformException("[static_layer] Cannot transform");
-      transformMsg = tf_->lookupTransform(map_frame_, 
-                                          global_frame_,
-                                          tf3::Time());
+      transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now());
     }
     catch (tf3::TransformException ex)
     {
@@ -317,7 +344,8 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
     // Copy map data given proper transformations
     tf3::Transform tf3_transform;
     tf3_transform = data_convert::convertTotf3Transform(transformMsg.transform);
-    // tf3::convert(transformMsg.transform, tf3_transform);
+    // tf2::Transform tf2_transform;
+    // tf2::convert(transform.transform, tf2_transform);
     for (unsigned int i = min_i; i < max_i; ++i)
     {
       for (unsigned int j = min_j; j < max_j; ++j)
@@ -341,11 +369,11 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
 }
 
 // Export factory function
-static PluginLayerPtr create_static_plugin() {
+static std::shared_ptr<Layer> create_static_plugin() {
     return std::make_shared<StaticLayer>();
 }
 
 // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
-BOOST_DLL_ALIAS(create_static_plugin, create_plugin)
+BOOST_DLL_ALIAS(create_static_plugin, create_static_layer)
 
 }  // namespace costmap_2d