update

CMakeLists.txt

@@ -49,7 +49,7 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_cpp
     robot_time
   )
-
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()
 
 # ========================================================
@@ -64,7 +64,6 @@ else()
     robot_laser_geometry
     robot_visualization_msgs
     robot_voxel_grid
-    tf3
     robot_tf3_geometry_msgs
     robot_tf3_sensor_msgs
     data_convert
@@ -73,10 +72,12 @@ else()
     robot_time
   )
 
+  find_library(TF3_LIBRARY NAMES tf3)
+  
   catkin_package(
     INCLUDE_DIRS include
     LIBRARIES robot_costmap_2d plugins
-    CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid tf3 robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
+    CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
     DEPENDS  PCL Boost 
   )
 
@@ -87,6 +88,7 @@ else()
     ${Boost_INCLUDE_DIRS}
     ${GTEST_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
+    ${TF3_INCLUDE_DIR}
   )
   link_directories(${PCL_LIBRARY_DIRS})
 endif()
@@ -122,6 +124,7 @@ if(BUILDING_WITH_CATKIN)
       ${EIGEN3_INCLUDE_DIRS}
       ${Boost_INCLUDE_DIRS}
       ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
   )
 
   target_link_libraries(robot_costmap_2d
@@ -130,6 +133,7 @@ if(BUILDING_WITH_CATKIN)
     PRIVATE yaml-cpp
     PRIVATE dl
     PRIVATE ${PCL_LIBRARIES}
+    PRIVATE ${TF3_LIBRARY}
   )
 
 else()
@@ -141,6 +145,7 @@ else()
       ${EIGEN3_INCLUDE_DIRS}
       ${Boost_INCLUDE_DIRS}
       ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
   )
 
   target_link_libraries(robot_costmap_2d
@@ -151,6 +156,7 @@ else()
       yaml-cpp
       dl
       ${PCL_LIBRARIES}
+      ${TF3_LIBRARY}
   )
 
   set_target_properties(robot_costmap_2d PROPERTIES
@@ -189,6 +195,7 @@ if(BUILDING_WITH_CATKIN)
     PRIVATE ${catkin_LIBRARIES}
     PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
     PRIVATE yaml-cpp
+    PRIVATE ${TF3_LIBRARY}
   )
 
 else()
@@ -205,6 +212,7 @@ else()
     PRIVATE yaml-cpp
     PRIVATE robot_time
     PRIVATE robot_cpp
+    PRIVATE ${TF3_LIBRARY}
   )
 
   set_target_properties(plugins PROPERTIES
@@ -273,13 +281,18 @@ endif()
 option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" ON)
 
 if(BUILD_COSTMAP_TESTS)
+  find_package(GTest REQUIRED)
+  find_package(Boost REQUIRED COMPONENTS system thread filesystem)
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
+
   if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/array_parser_test.cpp)
     add_executable(test_array_parser test/array_parser_test.cpp)
-    target_link_libraries(test_array_parser PRIVATE
+    target_link_libraries(test_array_parser PRIVATE 
       robot_costmap_2d
       GTest::GTest
       GTest::Main
-      Threads::Threads
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
     )
   endif()
 
@@ -289,7 +302,8 @@ if(BUILD_COSTMAP_TESTS)
       robot_costmap_2d
       GTest::GTest
       GTest::Main
-      Threads::Threads
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
     )
   endif()
 
@@ -300,11 +314,11 @@ if(BUILD_COSTMAP_TESTS)
       Boost::boost Boost::filesystem Boost::system
       yaml-cpp
       dl
-      Threads::Threads
+      Boost::system Boost::thread
-      tf3
       robot_time
       GTest::GTest
       GTest::Main
+      ${TF3_LIBRARY}
     )
   endif()
 endif()

include/robot_costmap_2d/costmap_2d_robot.h

@@ -47,14 +47,13 @@
 #include <robot_geometry_msgs/PoseStamped.h>
 
 #include <tf3/LinearMath/Transform.h>
-#include <robot/rate.h>
+#include <robot/robot.h>
 #include <data_convert/data_convert.h>
 #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <robot_xmlrpcpp/XmlRpcValue.h>
 
-#include <robot/node_handle.h>
+
-#include <robot/plugin_loader_helper.h>
 
 class RobotSuperValue : public robot_xmlrpcpp::XmlRpcValue
 {
@@ -199,13 +198,19 @@ public:
     return padded_footprint_;
   }
 
+  inline const robot_geometry_msgs::PolygonStamped& getRobotFootprintPolygonStamped() const noexcept
+  {
+    return footprint_;
+  }
+
   /** @brief Return the current unpadded footprint of the robot as a vector of points.
    *
    * This is the raw version of the footprint without padding.
    *
    * The footprint initially comes from the rosparam "footprint" but
    * can be overwritten by dynamic reconfigure or by messages received
-   * on the "footprint" topic. */
+   * on the "footprint" topic. 
+   */
   inline const std::vector<robot_geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
   {
     return unpadded_footprint_;
@@ -250,6 +255,7 @@ protected:
   double transform_tolerance_;  ///< timeout before transform errors
 
 private:
+  void copyParentParameters(const std::string& costmap_name, const std::string& plugin_name, const std::string& plugin_type, robot::NodeHandle& nh);
   /** @brief Set the footprint from the new_config object.
    *
    * If the values of footprint and robot_radius are the same in
@@ -270,10 +276,11 @@ private:
 
   std::vector<robot_geometry_msgs::Point> unpadded_footprint_;
   std::vector<robot_geometry_msgs::Point> padded_footprint_;
+  robot_geometry_msgs::PolygonStamped footprint_;
   float footprint_padding_;
 
 private:
-  void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
+  void getParams(const std::string& config_file_name,const std::string& name, robot::NodeHandle& nh);
 };
 // class Costmap2DROBOT
 }  // namespace robot_costmap_2d

include/robot_costmap_2d/footprint.h

@@ -43,7 +43,7 @@
 #include <robot_geometry_msgs/Point.h>
 #include <robot_geometry_msgs/Point32.h>
 
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 #include <robot_xmlrpcpp/XmlRpcValue.h> 
 
 namespace robot_costmap_2d

include/robot_costmap_2d/layer.h

@@ -42,7 +42,7 @@
 #include <robot_costmap_2d/utils.h>
 #include <string>
 #include <tf3/buffer_core.h>
-#include <robot/node_handle.h>
+#include <robot/robot.h>
 namespace robot_costmap_2d
 {
 

include/robot_costmap_2d/observation_buffer.h

@@ -40,10 +40,9 @@
 #include <vector>
 #include <list>
 #include <string>
-#include <robot/time.h>
+#include <robot/robot.h>
 #include <robot_costmap_2d/observation.h>
 #include <tf3/buffer_core.h>
-
 #include <robot_sensor_msgs/PointCloud2.h>
 
 // Thread support

include/robot_costmap_2d/obstacle_layer.h

@@ -38,6 +38,7 @@
 #ifndef ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
 #define ROBOT_COSTMAP_2D_OBSTACLE_LAYER_H_
 
+#include <robot/robot.h>
 #include <robot_costmap_2d/costmap_layer.h>
 #include <robot_costmap_2d/layered_costmap.h>
 #include <robot_costmap_2d/observation_buffer.h>
@@ -51,7 +52,7 @@
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_sensor_msgs/point_cloud_conversion.h>
 
-#include <robot/console.h>
+
 
 namespace robot_costmap_2d
 {

package.xml

@@ -46,9 +46,6 @@
   <build_depend>robot_voxel_grid</build_depend>
   <run_depend>robot_voxel_grid</run_depend>
 
-  <build_depend>tf3</build_depend>
-  <run_depend>tf3</run_depend>
-
   <build_depend>robot_tf3_geometry_msgs</build_depend>
   <run_depend>robot_tf3_geometry_msgs</run_depend>
 

plugins/inflation_layer.cpp

@@ -91,7 +91,13 @@ void InflationLayer::onInitialize()
 bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
     try {
-        std::string folder = ROBOT_COSTMAP_2D_DIR;
+        const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+        std::string folder;
+        if (env_config && std::filesystem::exists(env_config))
+        {
+          folder = std::string(env_config);
+          // robot::log_error("config_directory: %s", folder.c_str());
+        }
         std::string path_source = getSourceFile(folder,config_file_name);
 
         YAML::Node config = YAML::LoadFile(path_source);

plugins/obstacle_layer.cpp

@@ -75,7 +75,14 @@ ObstacleLayer::~ObstacleLayer()
 bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
   try {
-      std::string folder = ROBOT_COSTMAP_2D_DIR;
+      const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+      std::string folder;
+      if (env_config && std::filesystem::exists(env_config))
+      {
+        folder = std::string(env_config);
+        // robot::log_error("config_directory: %s", folder.c_str());
+      }
+      // robot::log_error("folder: %s", folder.c_str());
       std::string path_source = getSourceFile(folder,config_file_name);
       
       YAML::Node config = YAML::LoadFile(path_source);
@@ -111,7 +118,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
       std::string topics_string = loadParam(layer,"observation_sources", std::string(""));
       if (nh.hasParam("observation_sources"))
         nh.getParam("observation_sources", topics_string);
-      robot::log_error("Subscribed to Topics: %s\n", topics_string.c_str());
+      robot::log_info("Subscribed to Topics: %s\n", topics_string.c_str());
 
       // now we need to split the topics based on whitespace which we can use a stringstream for
       std::stringstream ss(topics_string);
@@ -188,7 +195,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
         // enabled_ = enabled;
 
         robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
-                  sensor_frame.c_str());
+                  priv_nh.getNamespace().c_str());
 
         // create an observation buffer
         observation_buffers_.push_back(

plugins/static_layer.cpp

@@ -76,7 +76,13 @@ void StaticLayer::onInitialize()
 bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
 {
     try {
-        std::string folder = ROBOT_COSTMAP_2D_DIR;
+        const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+        std::string folder;
+        if (env_config && std::filesystem::exists(env_config))
+        {
+          folder = std::string(env_config);
+          // robot::log_error("config_directory: %s", folder.c_str());
+        }
         std::string path_source = getSourceFile(folder,config_file_name);
         
         YAML::Node config = YAML::LoadFile(path_source);

plugins/voxel_layer.cpp

@@ -68,7 +68,14 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
 {
   try 
   {
-    std::string folder = ROBOT_COSTMAP_2D_DIR;
+    const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+    std::string folder;
+    if (env_config && std::filesystem::exists(env_config))
+    {
+      folder = std::string(env_config);
+      // robot::log_error("config_directory: %s", folder.c_str());
+    }
+
     std::string path_source = getSourceFile(folder,config_file_name);
     
     YAML::Node config = YAML::LoadFile(path_source);
@@ -111,7 +118,6 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
     }
     if (nh.hasParam("combination_method"))
       nh.getParam("combination_method", combination_method_);
-    
 
     this->matchSize();
   } 

src/costmap_2d_robot.cpp

@@ -55,6 +55,16 @@ using namespace std;
 
 namespace robot_costmap_2d
 {
+template <class T>
+void move_parameter(robot::NodeHandle& old_h, robot::NodeHandle& new_h, std::string name,T& value)
+{
+  if (!old_h.hasParam(name))
+    return;
+
+  old_h.getParam(name, value);
+  new_h.setParam(name, value);
+}
+
 Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
     layered_costmap_(NULL),
     name_(name),
@@ -71,38 +81,45 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
   robot::NodeHandle priv_nh(nh, name);
   name_ = name;
   std::string config_file_name = "costmap_params.yaml";
-  getParams(config_file_name, priv_nh);
+  getParams(config_file_name, name_, nh);
-
+ 
   // create a thread to handle updating the map
   stop_updates_ = false;
   initialized_ = true;
   stopped_ = false;
 }
 
-void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeHandle& nh)
+void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::string& name, robot::NodeHandle& nh)
 {
   try 
   {
-    std::string folder = ROBOT_COSTMAP_2D_DIR;
+    const char *env_config = std::getenv("PNKX_NAV_CORE_CONFIG_DIR");
+    std::string folder;
+    if (env_config && std::filesystem::exists(env_config))
+    {
+      folder = std::string(env_config);
+      // robot::log_error("config_directory: %s", folder.c_str());
+    }
     std::string path_source = getSourceFile(folder,config_file_name);
 
     YAML::Node config = YAML::LoadFile(path_source);
     YAML::Node layer = config["robot_costmap_2d"];
 
+    robot::NodeHandle priv_nh(priv_nh, name);
+
     std::string global_frame =
     loadParam(layer, "global_frame", std::string("map"));
     std::string robot_base_frame =
         loadParam(layer, "robot_base_frame", std::string("base_link"));
 
-    if (nh.hasParam("global_frame"))
+    if (priv_nh.hasParam("global_frame"))
-      nh.getParam("global_frame", global_frame);
+      priv_nh.getParam("global_frame", global_frame);
 
     if (nh.hasParam("robot_base_frame"))
       nh.getParam("robot_base_frame", robot_base_frame);
 
     global_frame_ = global_frame;
     robot_base_frame_ = robot_base_frame;
-    robot::log_error("robot_base_frame: %s", robot_base_frame.c_str());
 
     robot::Time last_error = robot::Time::now();
     std::string tf_error;
@@ -112,6 +129,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
     {
       if (last_error + robot::Duration(5.0) < robot::Time::now())
       {
+        std::string all_frames_string = tf_.allFramesAsString();
+        robot::log_info("[%s:%d]\n INFO: tf allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str());
         // std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl;
         robot::log_warning("[%s:%d] %0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
                 __FILE__, __LINE__, robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
@@ -130,15 +149,14 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
     robot::PluginLoaderHelper loader;
     
 
-    if (nh.hasParam("rolling_window"))
+    if (priv_nh.hasParam("rolling_window"))
-      nh.getParam("rolling_window", rolling_window);
+      priv_nh.getParam("rolling_window", rolling_window);
 
-    if (nh.hasParam("track_unknown_space"))
+    if (priv_nh.hasParam("track_unknown_space"))
-      nh.getParam("track_unknown_space", track_unknown_space);
+      priv_nh.getParam("track_unknown_space", track_unknown_space);
 
-    if (nh.hasParam("library_path"))
+    if (priv_nh.hasParam("library_path"))
       path_plugins = loader.findLibraryPath(name_);
-    robot::log_error("name: %s | rolling_window: %x", name_.c_str(), rolling_window);
     layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
 
     // find size parameters
@@ -148,16 +166,16 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
     double origin_x = loadParam(layer, "origin_x", 0.0);
     double origin_y = loadParam(layer, "origin_y", 0.0);
 
-    if (nh.hasParam("width"))
+    if (priv_nh.hasParam("width"))
-      nh.getParam("width", map_width_meters);
+      priv_nh.getParam("width", map_width_meters);
-    if (nh.hasParam("height"))
+    if (priv_nh.hasParam("height"))
-      nh.getParam("height", map_height_meters);
+      priv_nh.getParam("height", map_height_meters);
-    if (nh.hasParam("resolution"))
+    if (priv_nh.hasParam("resolution"))
-      nh.getParam("resolution", resolution);
+      priv_nh.getParam("resolution", resolution);
-    if (nh.hasParam("origin_x"))
+    if (priv_nh.hasParam("origin_x"))
-      nh.getParam("origin_x", origin_x);
+      priv_nh.getParam("origin_x", origin_x);
-    if (nh.hasParam("origin_y"))
+    if (priv_nh.hasParam("origin_y"))
-      nh.getParam("origin_y", origin_y);
+      priv_nh.getParam("origin_y", origin_y);
 
     if (!layered_costmap_->isSizeLocked())
     {
@@ -173,10 +191,10 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
     };
     std::vector<PluginConfig> my_list;
       
-    if(nh.hasParam("plugins"))
+    if(priv_nh.hasParam("plugins"))
     {
       my_list.clear();
-      YAML::Node my_plugins = nh.getParamValue("plugins");
+      YAML::Node my_plugins = priv_nh.getParamValue("plugins");
       if (my_plugins && my_plugins.IsSequence())
       {
         for (size_t i = 0; i < my_plugins.size(); ++i)
@@ -230,12 +248,12 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
         }
       }
     }
-
+    robot::NodeHandle private_nh("~");
     for (auto& info : my_list)
     {
       try
       {
-        // copyParentParameters(pname, type, private_nh);
+        copyParentParameters(name_, info.name, info.type, private_nh);
         creators_.push_back(
           boost::dll::import_alias<PluginLayerPtr()>(
           path_plugins, info.type, boost::dll::load_mode::append_decorations)
@@ -257,14 +275,15 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
     new_footprint = loadFootprint(layer["footprint"], new_footprint);
     transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0);
 
-    if (nh.hasParam("footprint"))
+    if (priv_nh.hasParam("footprint"))
     {
       std::cout <<"FOOTPRINT ROBOT:"<<std::endl;
-      new_footprint = makeFootprintFromParams(nh);
+      new_footprint = makeFootprintFromParams(priv_nh);
     }
 
     if (nh.hasParam("transform_tolerance"))
       nh.getParam("transform_tolerance", transform_tolerance_);
+    // robot::log_info("transform_tolerance: %d", transform_tolerance_);
     
     setUnpaddedRobotFootprint(new_footprint);
 
@@ -280,8 +299,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
     map_update_thread_shutdown_ = false;
     double map_update_frequency = loadParam(layer, "update_frequency", 0.0);
 
-    if (nh.hasParam("update_frequency"))
+    if (priv_nh.hasParam("update_frequency"))
-      nh.getParam("update_frequency", map_update_frequency);
+      priv_nh.getParam("update_frequency", map_update_frequency);
 
     // If the padding has changed, call setUnpaddedRobotFootprint() to
     // re-apply the padding.
@@ -295,8 +314,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
     }
 
     double robot_radius = loadParam(layer, "robot_radius", 0.0);
-    if (nh.hasParam("robot_radius"))
+    if (priv_nh.hasParam("robot_radius"))
-      nh.getParam("robot_radius", robot_radius);
+      priv_nh.getParam("robot_radius", robot_radius);
     readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius);
 
     // only construct the thread if the frequency is positive
@@ -310,6 +329,117 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
   }
 }
 
+void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, 
+                                          const std::string& plugin_name, 
+                                          const std::string& plugin_type, 
+                                          robot::NodeHandle& nh)
+{
+  robot::NodeHandle costmap_nh(nh, costmap_name);
+  robot::NodeHandle costmap_plugin_nh(costmap_nh, plugin_name);
+  robot::NodeHandle plugin_nh(nh, plugin_name);
+  if(plugin_type == "StaticLayer")
+  {
+    std::string map_topic;
+    int unknown_cost_value;
+    int lethal_cost_threshold;
+    bool track_unknown_space;
+    move_parameter(plugin_nh, costmap_plugin_nh, "map_topic", map_topic);
+    move_parameter(plugin_nh, costmap_plugin_nh, "unknown_cost_value", unknown_cost_value);
+    move_parameter(plugin_nh, costmap_plugin_nh, "lethal_cost_threshold", lethal_cost_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
+  }
+  else if(plugin_type == "VoxelLayer")
+  {
+    double origin_z;
+    double z_resolution;
+    int z_voxels;
+    int mark_threshold;
+    int unknown_threshold;
+    bool publish_voxel_map;
+    move_parameter(plugin_nh, costmap_plugin_nh, "origin_z", origin_z);
+    move_parameter(plugin_nh, costmap_plugin_nh, "z_resolution", z_resolution);
+    move_parameter(plugin_nh, costmap_plugin_nh, "z_voxels", z_voxels);
+    move_parameter(plugin_nh, costmap_plugin_nh, "mark_threshold", mark_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "unknown_threshold", unknown_threshold);
+    move_parameter(plugin_nh, costmap_plugin_nh, "publish_voxel_map", publish_voxel_map);
+    if(plugin_nh.hasParam("observation_sources"))
+    {
+      std::string topics_string;
+      move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
+      robot::log_error("topics_string: %s", topics_string.c_str());
+      std::stringstream ss(topics_string);
+      std::string source;
+      while (ss >> source)
+      {
+        robot::NodeHandle plugin_nh_element(plugin_nh, source);
+        robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
+        std::string topic;
+        std::string data_type;
+        bool clearing;
+        bool marking;
+        bool inf_is_valid;
+        double min_obstacle_height;
+        double max_obstacle_height;
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
+        robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
+      }
+    }
+  }
+  else if(plugin_type == "ObstacleLayer")
+  {
+    double max_obstacle_height;
+    double raytrace_range;
+    double obstacle_range;
+    bool track_unknown_space;
+    move_parameter(plugin_nh, costmap_plugin_nh, "max_obstacle_height", max_obstacle_height);
+    move_parameter(plugin_nh, costmap_plugin_nh, "raytrace_range", raytrace_range);
+    move_parameter(plugin_nh, costmap_plugin_nh, "obstacle_range", obstacle_range);
+    move_parameter(plugin_nh, costmap_plugin_nh, "track_unknown_space", track_unknown_space);
+    if(plugin_nh.hasParam("observation_sources"))
+    {
+      std::string topics_string;
+      move_parameter(plugin_nh, costmap_plugin_nh, "observation_sources", topics_string);
+      robot::log_error("topics_string: %s", topics_string.c_str());
+      std::stringstream ss(topics_string);
+      std::string source;
+      while (ss >> source)
+      {
+        robot::NodeHandle plugin_nh_element(plugin_nh, source);
+        robot::NodeHandle costmap_plugin_nh_element(costmap_plugin_nh, source);
+        std::string topic;
+        std::string data_type;
+        bool clearing;
+        bool marking;
+        bool inf_is_valid;
+        double min_obstacle_height;
+        double max_obstacle_height;
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "topic", topic);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "data_type", data_type);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "clearing", clearing);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "marking", marking);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "inf_is_valid", inf_is_valid);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "min_obstacle_height", min_obstacle_height);
+        move_parameter(plugin_nh_element, costmap_plugin_nh_element, "max_obstacle_height", max_obstacle_height);
+        robot::log_info("topic: %s data_type: %s clearing: %d marking: %d inf_is_valid: %d min_obstacle_height: %f max_obstacle_height: %f", topic.c_str(), data_type.c_str(), clearing, marking, inf_is_valid, min_obstacle_height, max_obstacle_height);
+      }
+    }
+  }
+  else if(plugin_type == "InflationLayer")
+  {
+    double cost_scaling_factor;
+    double inflation_radius;
+    move_parameter(plugin_nh, costmap_plugin_nh, "cost_scaling_factor", cost_scaling_factor);
+    move_parameter(plugin_nh, costmap_plugin_nh, "inflation_radius", inflation_radius);
+  }
+
+}
+
 void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const robot_geometry_msgs::Polygon& footprint)
 { 
   setUnpaddedRobotFootprint(toPointVector(footprint));
@@ -402,16 +532,17 @@ void Costmap2DROBOT::updateMap()
              yaw = data_convert::getYaw(pose.pose.orientation);
       // robot::log_error("ROBOT POSE: %f | %f | %f",x,y,yaw);
       layered_costmap_->updateMap(x, y, yaw);
-      robot_geometry_msgs::PolygonStamped footprint;
+      
-      footprint.header.frame_id = global_frame_;
+      footprint_.header.frame_id = global_frame_;
-      footprint.header.stamp = robot::Time::now();
+      footprint_.header.stamp = robot::Time::now();
-      transformFootprint(x, y, yaw, padded_footprint_, footprint);
+      transformFootprint(x, y, yaw, padded_footprint_, footprint_);
 
       initialized_ = true;
     }
   }
 }
 
+
 void Costmap2DROBOT::start()
 {
   // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::start");
@@ -563,7 +694,7 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
   }
   catch (tf3::ExtrapolationException& ex)
   {
-    robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
+    // robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
     return false;
   }
   // ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);

test/static_layer_test.cpp

@@ -67,9 +67,9 @@ void CostmapTester::compareCells(robot_costmap_2d::Costmap2D& costmap, unsigned
     double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
     unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
     if(neighbor_cost < expected_lowest_cost){
-      robot::log_error("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
+      printf("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
           x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
-      robot::log_error("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
+      printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
       costmap.saveMap("failing_costmap.pgm");
     }
     EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == robot_costmap_2d::FREE_SPACE));