update

CMakeLists.txt

@@ -1,61 +1,38 @@
-# --- CMake version và project name ---
-cmake_minimum_required(VERSION 3.10)
-project(robot_costmap_2d)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_costmap_2d VERSION 1.0.0 LANGUAGES CXX)
 
-# --- C++ standard và position independent code ---
-set(CMAKE_CXX_STANDARD 17)            # Sử dụng C++17
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)  # Thư viện có thể build thành shared lib
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building robot_costmap_2d with Catkin")
 
-# --- RPATH settings: ưu tiên thư viện build tại chỗ ---
-# Dùng để runtime linker tìm thư viện đã build trước khi install
-set(CMAKE_SKIP_BUILD_RPATH FALSE)
-set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
-set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
-set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/robot_costmap_2d")
-set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_costmap_2d with Standalone CMake")
+endif()
 
-# --- Dependencies ---
-# Tìm các thư viện cần thiết
-# find_package(tf3 REQUIRED)           # Nếu dùng tf3
-find_package(Eigen3 REQUIRED)          # Thư viện Eigen cho toán học
-find_package(Boost REQUIRED COMPONENTS system thread filesystem)  # Boost: system, thread, filesystem
-find_package(GTest REQUIRED)           # Google Test cho unit test
-find_package(PCL REQUIRED COMPONENTS common io)   # Point Cloud Library
+# C++ Standard - must be set before find_package
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
 
-# --- Define macro để dùng trong code ---
-add_definitions(-DROBOT_COSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
+# Find dependencies
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system thread filesystem)
+find_package(GTest REQUIRED)
+find_package(PCL REQUIRED COMPONENTS common io)
+find_package(yaml-cpp REQUIRED)
 
-# --- Include directories ---
-# Thêm các folder chứa header files
-include_directories(
-    include  
-    ${EIGEN3_INCLUDE_DIRS}
-    ${Boost_INCLUDE_DIRS}
-    ${GTEST_INCLUDE_DIRS}
-    ${PCL_INCLUDE_DIRS}
-)
-link_directories(${PCL_LIBRARY_DIRS})  # Thêm thư viện PCL vào linker path
+if (NOT BUILDING_WITH_CATKIN)
 
-# --- Eigen và PCL definitions ---
-add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-# --- Core library: robot_costmap_2d ---
-# Tạo thư viện chính
-add_library(robot_costmap_2d
-    src/costmap_2d_robot.cpp
-    src/array_parser.cpp
-    src/costmap_2d.cpp
-    src/observation_buffer.cpp
-    src/layer.cpp
-    src/layered_costmap.cpp
-    src/costmap_math.cpp
-    src/footprint.cpp
-    src/costmap_layer.cpp
-)
+  # Cấu hình RPATH để tránh cycle trong runtime search path
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
 
-# --- Link các thư viện phụ thuộc ---
-target_link_libraries(robot_costmap_2d
-    ${Boost_LIBRARIES}        # Boost
+  set(PACKAGES_DIR  
     robot_std_msgs
     robot_sensor_msgs
     geometry_msgs
@@ -68,49 +45,132 @@ target_link_libraries(robot_costmap_2d
     robot_tf3_geometry_msgs
     robot_tf3_sensor_msgs
     data_convert
-    robot_xmlrpcpp                  # XMLRPC
-    yaml-cpp
-    dl
+    robot_xmlrpcpp
     robot_cpp
-)
+    robot_time
+  )
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
+else()
 
-# --- Include directories cho target ---
-target_include_directories(robot_costmap_2d 
-        PUBLIC 
-            ${Boost_INCLUDE_DIRS}  # Boost headers
-            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>   # Khi build từ source
-            $<INSTALL_INTERFACE:include/${PROJECT_NAME}>             # Khi install
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    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
   )
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-install(TARGETS robot_costmap_2d
-  EXPORT robot_costmap_2d-targets
-  ARCHIVE DESTINATION lib      # Thư viện tĩnh .a
-  LIBRARY DESTINATION lib      # Thư viện động .so
-  RUNTIME DESTINATION bin      # File thực thi (nếu có)
-  INCLUDES DESTINATION include # Cài đặt include
+  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 robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time
+    DEPENDS  PCL Boost 
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+    ${GTEST_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+    ${TF3_INCLUDE_DIR}
+  )
+  link_directories(${PCL_LIBRARY_DIRS})
+endif()
+
+# Define macro để dùng trong code
+add_definitions(-DROBOT_COSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
+
+# Eigen và PCL definitions
+add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})
+
+# ========================================================
+# Core library: robot_costmap_2d
+# ========================================================
+add_library(robot_costmap_2d SHARED
+  src/costmap_2d_robot.cpp
+  src/array_parser.cpp
+  src/costmap_2d.cpp
+  src/observation_buffer.cpp
+  src/layer.cpp
+  src/layered_costmap.cpp
+  src/costmap_math.cpp
+  src/footprint.cpp
+  src/costmap_layer.cpp
 )
 
-# --- Xuất export set robot_costmap_2dTargets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_costmap_2d/robot_costmap_2dTargets.cmake ---
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(robot_costmap_2d REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE robot_costmap_2d::robot_costmap_2d) ---
-install(EXPORT robot_costmap_2d-targets 
-  FILE robot_costmap_2d-targets.cmake
-  NAMESPACE robot_costmap_2d::
-  DESTINATION lib/cmake/robot_costmap_2d
-)
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(robot_costmap_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-# --- Cài đặt headers ---
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION include/${PROJECT_NAME}
-)
+  target_include_directories(robot_costmap_2d
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+      ${EIGEN3_INCLUDE_DIRS}
+      ${Boost_INCLUDE_DIRS}
+      ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
+  )
 
-# --- Plugin libraries ---
-# Tạo các plugin shared library
-add_library(plugins 
-    SHARED 
+  target_link_libraries(robot_costmap_2d
+    PUBLIC ${catkin_LIBRARIES}
+    PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
+    PRIVATE yaml-cpp
+    PRIVATE dl
+    PRIVATE ${PCL_LIBRARIES}
+    PRIVATE ${TF3_LIBRARY}
+  )
+
+else()
+
+  target_include_directories(robot_costmap_2d
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+      ${EIGEN3_INCLUDE_DIRS}
+      ${Boost_INCLUDE_DIRS}
+      ${PCL_INCLUDE_DIRS}
+      ${TF3_INCLUDE_DIR}
+  )
+
+  target_link_libraries(robot_costmap_2d
+    PUBLIC
+      ${PACKAGES_DIR}
+    PRIVATE
+      Boost::boost Boost::system Boost::thread Boost::filesystem
+      yaml-cpp
+      dl
+      ${PCL_LIBRARIES}
+      ${TF3_LIBRARY}
+  )
+
+  set_target_properties(robot_costmap_2d PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+endif()
+
+# ========================================================
+# Plugin library: plugins
+# ========================================================
+add_library(plugins SHARED
   plugins/static_layer.cpp
   plugins/obstacle_layer.cpp
   plugins/inflation_layer.cpp
@@ -119,76 +179,146 @@ add_library(plugins
   plugins/directional_layer.cpp
   plugins/preferred_layer.cpp
   plugins/unpreferred_layer.cpp
+)
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(plugins ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  target_include_directories(plugins
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
   )
 
-target_link_libraries(plugins 
-    PRIVATE
-        robot_costmap_2d 
-        ${Boost_LIBRARIES} 
-        yaml-cpp
-        robot_time
-        robot_cpp 
+  target_link_libraries(plugins
+    PUBLIC robot_costmap_2d
+    PRIVATE ${catkin_LIBRARIES}
+    PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
+    PRIVATE yaml-cpp
+    PRIVATE ${TF3_LIBRARY}
   )
 
-set_target_properties(plugins PROPERTIES
+else()
+
+  target_include_directories(plugins
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(plugins
+    PUBLIC robot_costmap_2d
+    PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem
+    PRIVATE yaml-cpp
+    PRIVATE robot_time
+    PRIVATE robot_cpp
+    PRIVATE ${TF3_LIBRARY}
+  )
+
+  set_target_properties(plugins PROPERTIES
     LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
 
-install(TARGETS plugins
-  EXPORT plugins-targets
-  ARCHIVE DESTINATION lib      # Thư viện tĩnh .a
-  LIBRARY DESTINATION lib      # Thư viện động .so
-  RUNTIME DESTINATION bin      # File thực thi (nếu có)
-  INCLUDES DESTINATION include # Cài đặt include
-)
+endif()
 
-install(EXPORT plugins-targets 
-  FILE plugins-targets.cmake
-  NAMESPACE robot_costmap_2d::
-  DESTINATION lib/cmake/plugins
-)
+# ========================================================
+# Install
+# ========================================================
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS robot_costmap_2d plugins
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
 
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
 
-# --- Option để bật/tắt test ---
+else()
+
+  install(TARGETS robot_costmap_2d plugins
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # Print configuration info
+  message(STATUS "=================================")
+  message(STATUS "Project: ${PROJECT_NAME}")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Libraries: robot_costmap_2d, plugins")
+  message(STATUS "Dependencies: 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, Eigen3, PCL, Boost, yaml-cpp")
+  message(STATUS "=================================")
+endif()
+
+# ========================================================
+# Test executables
+# ========================================================
 option(BUILD_COSTMAP_TESTS "Build robot_costmap_2d test executables" ON)
 
 if(BUILD_COSTMAP_TESTS)
-    # --- Test executables ---
+  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)
-    add_executable(test_costmap test/coordinates_test.cpp)
-    add_executable(test_plugin test/static_layer_test.cpp)
-
-    # --- Link thư viện cho test ---
-    target_link_libraries(test_array_parser PRIVATE robot_costmap_2d GTest::GTest GTest::Main pthread)
-    target_link_libraries(test_costmap PRIVATE robot_costmap_2d GTest::GTest GTest::Main pthread)
-    target_link_libraries(test_plugin PRIVATE 
-    
-        ${Boost_LIBRARIES}
-        Boost::filesystem
-        Boost::system
-        dl
-        pthread
-        yaml-cpp
-        tf3
-        robot_time
+    target_link_libraries(test_array_parser PRIVATE 
       robot_costmap_2d
-        GTest::GTest GTest::Main
+      GTest::GTest
+      GTest::Main
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
     )
+  endif()
 
-    # --- Set RPATH để tìm thư viện của project này thay vì system ROS libraries ---
-    set_target_properties(test_array_parser PROPERTIES
-        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
-        LINK_FLAGS "-Wl,--disable-new-dtags"
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/coordinates_test.cpp)
+    add_executable(test_costmap test/coordinates_test.cpp)
+    target_link_libraries(test_costmap PRIVATE
+      robot_costmap_2d
+      GTest::GTest
+      GTest::Main
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
     )
-    set_target_properties(test_costmap PROPERTIES
-        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
-        LINK_FLAGS "-Wl,--disable-new-dtags"
-    )
-    set_target_properties(test_plugin PROPERTIES
-        BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-        INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
-        LINK_FLAGS "-Wl,--disable-new-dtags"
+  endif()
+
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/static_layer_test.cpp)
+    add_executable(test_plugin test/static_layer_test.cpp)
+    target_link_libraries(test_plugin PRIVATE
+      robot_costmap_2d
+      Boost::boost Boost::filesystem Boost::system
+      yaml-cpp
+      dl
+      Boost::system Boost::thread
+      robot_time
+      GTest::GTest
+      GTest::Main
+      ${TF3_LIBRARY}
     )
+  endif()
 endif()

config/static_layer_params.yaml

@@ -1,5 +1,6 @@
 static_layer:
   enabled: true
+  map_topic: "map"
   first_map_only: false
   subscribe_to_updates: false
   track_unknown_space: true

config/voxel_layer_params.yaml

@@ -7,5 +7,5 @@ voxel_layer:
   z_voxels: 16
   unknown_threshold: 15.0
   mark_threshold: 0
-  combination_method: 3
+  combination_method: 1
 

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
 {
@@ -74,12 +73,6 @@ public:
 namespace robot_costmap_2d
 {
 
-  void signalHandler(int)
-  {
-    std::signal(SIGINT, SIG_DFL);
-    std::raise(SIGINT);
-  }
-
 /** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
  * topics that provide observations about obstacles in either the form
  * of PointCloud or LaserScan messages. */
@@ -205,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_;
@@ -256,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
@@ -276,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/critical_layer.h

@@ -10,6 +10,12 @@ class CriticalLayer : public StaticLayer
 public:
   CriticalLayer();
   virtual ~CriticalLayer();
+
+  LayerType getType() const override
+  {
+      return LayerType::CRITICAL_LAYER;
+  }
+  
 private:
   unsigned char interpretValue(unsigned char value) override;
   void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;

include/robot_costmap_2d/directional_layer.h

@@ -14,6 +14,11 @@ namespace robot_costmap_2d
     bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
     void resetMap();
 
+    LayerType getType() const override
+    {
+        return LayerType::DIRECTIONAL_LAYER;
+    }
+    
 	private:
     void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
 		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path);

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/inflation_layer.h

@@ -120,6 +120,11 @@ public:
    */
   void setInflationParameters(double inflation_radius, double cost_scaling_factor);
 
+  LayerType getType() const override
+  {
+      return LayerType::INFLATION_LAYER;
+  }
+  
 protected:
   virtual void onFootprintChanged();
   boost::recursive_mutex* inflation_access_;

include/robot_costmap_2d/layer.h

@@ -42,9 +42,23 @@
 #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
 {
+
+enum class LayerType
+{
+    UNKNOWN,
+    STATIC_LAYER,
+    OBSTACLE_LAYER,
+    INFLATION_LAYER,
+    CRITICAL_LAYER,
+    DIRECTIONAL_LAYER,
+    PREFERRED_LAYER,
+    UNPREFERRED_LAYER,
+    VOXEL_LAYER
+};
+
 class LayeredCostmap;
 
 class Layer
@@ -134,6 +148,9 @@ public:
   void dataCallBack(const T& value, const std::string& topic) {
       handle(value, topic);
   }
+
+  virtual LayerType getType() const { return LayerType::UNKNOWN; }
+  
 protected:
 
   // Hàm template public, dùng để gửi dữ liệu

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,12 +52,13 @@
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_sensor_msgs/point_cloud_conversion.h>
 
-#include <robot/console.h>
+
 
 namespace robot_costmap_2d
 {
 struct CallBackInfo
 {
+  std::string observation_source;
   std::string data_type;
   std::string topic;
   bool inf_is_valid;
@@ -84,6 +86,11 @@ public:
   void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
   void clearStaticObservations(bool marking, bool clearing);
 
+  LayerType getType() const override
+  {
+      return LayerType::OBSTACLE_LAYER;
+  }
+  
 protected:
   void handleImpl(const void* data, 
                   const std::type_info&, 

include/robot_costmap_2d/preferred_layer.h

@@ -10,6 +10,12 @@ class PreferredLayer : public StaticLayer
 public:
   PreferredLayer();
   virtual ~PreferredLayer();
+
+  LayerType getType() const override
+  {
+      return LayerType::PREFERRED_LAYER;
+  }
+  
 private:
   unsigned char interpretValue(unsigned char value);
 };

include/robot_costmap_2d/static_layer.h

@@ -63,6 +63,12 @@ public:
   virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   virtual void matchSize();
+
+  LayerType getType() const override
+  {
+      return LayerType::STATIC_LAYER;
+  }
+  
 protected:
   void handleImpl(const void* data,
                   const std::type_info& type,

include/robot_costmap_2d/testing_helper.h

@@ -25,12 +25,12 @@ char printableCost(unsigned char cost)
 
 void printMap(robot_costmap_2d::Costmap2D& costmap)
 {
-  printf("map:\n");
+  robot::log_info("map:\n");
   for (int i = 0; i < costmap.getSizeInCellsY(); i++){
     for (int j = 0; j < costmap.getSizeInCellsX(); j++){
-      printf("%4d", int(costmap.getCost(j, i)));
+      robot::log_info("%4d", int(costmap.getCost(j, i)));
     }
-    printf("\n\n");
+    robot::log_info("\n\n");
   }
 }
 

include/robot_costmap_2d/unpreferred_layer.h

@@ -11,6 +11,11 @@ public:
   UnPreferredLayer();
   virtual ~UnPreferredLayer();
 
+  LayerType getType() const override
+  {
+      return LayerType::UNPREFERRED_LAYER;
+  }
+  
 private:
   unsigned char interpretValue(unsigned char value);
 

include/robot_costmap_2d/voxel_layer.h

@@ -77,6 +77,10 @@ public:
   virtual void matchSize();
   virtual void reset();
 
+  LayerType getType() const override
+  {
+      return LayerType::VOXEL_LAYER;
+  }
 
 protected:
 

package.xml

@@ -20,7 +20,48 @@
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
   <build_depend>libconsole-bridge-dev</build_depend>
-
   <run_depend>libconsole-bridge-dev</run_depend>
 
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+
+  <build_depend>robot_sensor_msgs</build_depend>
+  <run_depend>robot_sensor_msgs</run_depend>
+
+  <build_depend>geometry_msgs</build_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+  <build_depend>robot_nav_msgs</build_depend>
+  <run_depend>robot_nav_msgs</run_depend>
+
+  <build_depend>robot_map_msgs</build_depend>
+  <run_depend>robot_map_msgs</run_depend>
+
+  <build_depend>robot_laser_geometry</build_depend>
+  <run_depend>robot_laser_geometry</run_depend>
+
+  <build_depend>robot_visualization_msgs</build_depend>
+  <run_depend>robot_visualization_msgs</run_depend>
+
+  <build_depend>robot_voxel_grid</build_depend>
+  <run_depend>robot_voxel_grid</run_depend>
+
+  <build_depend>robot_tf3_geometry_msgs</build_depend>
+  <run_depend>robot_tf3_geometry_msgs</run_depend>
+
+  <build_depend>robot_tf3_sensor_msgs</build_depend>
+  <run_depend>robot_tf3_sensor_msgs</run_depend>
+
+  <build_depend>data_convert</build_depend>
+  <run_depend>data_convert</run_depend>
+
+  <build_depend>robot_xmlrpcpp</build_depend>
+  <run_depend>robot_xmlrpcpp</run_depend>
+
+  <build_depend>robot_cpp</build_depend>
+  <run_depend>robot_cpp</run_depend>
+
+  <build_depend>robot_time</build_depend>
+  <run_depend>robot_time</run_depend>
+
 </package>

plugins/critical_layer.cpp

@@ -13,7 +13,6 @@ CriticalLayer::~CriticalLayer(){}
 
 unsigned char CriticalLayer::interpretValue(unsigned char value) 
 {
-  // printf("TEST PLUGIN CRITICAL\n");
   // check if the static value is above the unknown or lethal thresholds
   if(value >= *this->threshold_)
     return CRITICAL_SPACE;
@@ -23,7 +22,6 @@ unsigned char CriticalLayer::interpretValue(unsigned char value)
 
 void CriticalLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
 {
-  // printf("TEST PLUGIN CRITICAL\n");
   if (!map_received_)
     return;
     

plugins/directional_layer.cpp

@@ -36,7 +36,7 @@ namespace robot_costmap_2d
     {
       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 / 2, size_y / 2, new_map.info.resolution);
+      robot::log_info("Received a %d X %d map at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
 
       // resize costmap if size, resolution or origin do not match
       Costmap2D *master = layered_costmap_->getCostmap();
@@ -48,7 +48,7 @@ namespace robot_costmap_2d
           master->getOriginY() != new_map.info.origin.position.y))
       {
         // Update the size of the layered costmap (and all layers, including this one)
-        printf("Resizing costmap to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
+        robot::log_info("Resizing costmap to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
         layered_costmap_->resizeMap(size_x / 2, size_y / 2, new_map.info.resolution, new_map.info.origin.position.x,
                                     new_map.info.origin.position.y,
                                     true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
@@ -59,7 +59,7 @@ namespace robot_costmap_2d
               origin_y_ != new_map.info.origin.position.y)
       {
         // only update the size of the costmap stored locally in this layer
-        printf("Resizing static layer to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
+        robot::log_info("Resizing static layer to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
         resizeMap(size_x / 2, size_y / 2, new_map.info.resolution,
                   new_map.info.origin.position.x, new_map.info.origin.position.y);
       }
@@ -99,12 +99,12 @@ namespace robot_costmap_2d
     }
     else
     {
-      printf("Stop receive new map!\n");
+      robot::log_info("Stop receive new map!\n");
     }
     // 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\n");
+      robot::log_info("Shutting down the map subscriber. first_map_only flag is on\n");
       map_shutdown_ = true;
       // map_sub_.shutdown();
     }
@@ -129,7 +129,7 @@ namespace robot_costmap_2d
       unsigned int mx, my;
       if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
       {
-        printf("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
+        robot::log_error("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
         return false;
       }
       // Convert to yaw
@@ -263,8 +263,7 @@ namespace robot_costmap_2d
       y_max_w = std::max(y_max_w, y[i].second);
 
     }
-    // printf("%d %d %d %d", x_min, y_min, x_max, y_max);
-    // printf("%f %f %f %f", x_min_w, y_min_w, x_max_w, y_max_w);
+
     for (int i = 0; i < yaw_robot.size(); i++)
     {
       if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
@@ -407,7 +406,7 @@ namespace robot_costmap_2d
     }
     catch (tf3::TransformException &ex)
     {
-      printf("%s\n", ex.what());
+      robot::log_error("%s\n", ex.what());
       return false;
     }
     // Copy map data given proper transformations

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);
@@ -186,7 +192,7 @@ void InflationLayer::onFootprintChanged()
   computeCaches();
   need_reinflation_ = true;
 
-  printf("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
+  robot::log_info("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
             " inscribed_radius_ = %.3f, inflation_radius_ = %.3f\n",
             layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
 }
@@ -198,19 +204,20 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
     return;
 
   // make sure the inflation list is empty at the beginning of the cycle (should always be true)
-  printf("The inflation list must be empty at the beginning of inflation\n");
+  if(!inflation_cells_.empty())
+    robot::log_error("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) {
-    printf("InflationLayer::updateCosts(): seen_ array is NULL\n");
+    robot::log_error("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)
   {
-    printf("InflationLayer::updateCosts(): seen_ array size is wrong\n");
+    robot::log_error("InflationLayer::updateCosts(): seen_ array size is wrong\n");
     delete[] seen_;
     seen_size_ = size_x * size_y;
     seen_ = new bool[seen_size_];
@@ -404,7 +411,7 @@ void InflationLayer::handleImpl(const void* data,
                               const std::type_info& info,
                               const std::string& source)
 {
-  printf("This function is not available!\n");
+  robot::log_error("This function is not available!\n");
 }
 
 

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);
@@ -165,6 +172,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
         }
 
         CallBackInfo info_tmp;
+        info_tmp.observation_source = source;
         info_tmp.data_type = data_type;
         info_tmp.topic = topic;
         info_tmp.inf_is_valid = inf_is_valid;
@@ -186,8 +194,8 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
 
         // enabled_ = enabled;
 
-        printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
-                  sensor_frame.c_str());
+        robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
+                  priv_nh.getNamespace().c_str());
 
         // create an observation buffer
         observation_buffers_.push_back(
@@ -202,7 +210,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
         if (clearing)
           clearing_buffers_.push_back(observation_buffers_.back());
 
-        printf(
+        robot::log_info(
             "Created an observation buffer for topic %s, global frame: %s, "
             "expected update rate: %.2f, observation persistence: %.2f\n",
             topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
@@ -210,7 +218,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
       } 
     }
     catch (const YAML::BadFile& e) {
-        std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
+        robot::log_error("Cannot open YAML file: %s\n", e.what());
         return false;
     }
     
@@ -237,6 +245,8 @@ void ObstacleLayer::handleImpl(const void* data,
           topic == callback_infos_[i].topic &&
           !callback_infos_[i].inf_is_valid)
       {
+        // if(topic == "/f_scan") robot::log_error("DATA front laser! %d",i);
+        // if(topic == "/b_scan") robot::log_error("DATA back laser! %d",i);
         laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
       }
       else if (type == typeid(robot_sensor_msgs::LaserScan) &&
@@ -244,6 +254,7 @@ void ObstacleLayer::handleImpl(const void* data,
               topic == callback_infos_[i].topic &&
               callback_infos_[i].inf_is_valid)
       {
+        
         laserScanValidInfCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
       }
       else if (type == typeid(robot_sensor_msgs::PointCloud) &&
@@ -252,7 +263,7 @@ void ObstacleLayer::handleImpl(const void* data,
       {
         if (callback_infos_[i].inf_is_valid)
         {
-          printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
+          robot::log_error("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.\n");
         }
         pointCloudCallback(*static_cast<const robot_sensor_msgs::PointCloud*>(data), buffer);
       }
@@ -262,19 +273,23 @@ void ObstacleLayer::handleImpl(const void* data,
       {
         if (callback_infos_[i].inf_is_valid)
         {
-          printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
+          robot::log_error("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.\n");
         }
         pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
       }
-      else 
-      {
-          std::cout << "obstacle_layer: Unknown type: " << type.name() << std::endl;
-      }
+      // else 
+      // {
+      //     std::cout << "obstacle_layer: check type: " << (type == typeid(robot_sensor_msgs::LaserScan)) << std::endl 
+      //     << "obstacle_layer: inf_is_valid: " << callback_infos_[i].inf_is_valid << std::endl
+      //     << "data type: " << callback_infos_[i].data_type << std::endl
+      //     << "topic: " << topic << std::endl
+      //     << "topic check: " << callback_infos_[i].topic << std::endl << std::endl; 
+      // }
     }
   }
   else 
   {
-    std::cout << "Stop receiving data!" << std::endl;
+    robot::log_info("Stop receiving data!\n");
     return;
   }
 }
@@ -293,13 +308,13 @@ void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& messag
   }
   catch (tf3::TransformException &ex)
   {
-    printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
+    robot::log_error("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
              ex.what());
     projector_.projectLaser(message, cloud);
   }
   catch (std::runtime_error &ex)
   {
-    printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
+    robot::log_error("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
     return; //ignore this message
   }
 
@@ -335,13 +350,13 @@ void ObstacleLayer::laserScanValidInfCallback(const robot_sensor_msgs::LaserScan
   }
   catch (tf3::TransformException &ex)
   {
-    printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
+    robot::log_error("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
              global_frame_.c_str(), ex.what());
     projector_.projectLaser(message, cloud);
   }
   catch (std::runtime_error &ex)
   {
-    printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
+    robot::log_error("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
     return; //ignore this message
   }
 
@@ -358,7 +373,7 @@ void ObstacleLayer::pointCloudCallback(const robot_sensor_msgs::PointCloud& mess
 
   if (!robot_sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
   {
-    printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
+    robot::log_error("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
     return;
   }
 
@@ -422,7 +437,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       // if the obstacle is too high or too far away from the robot we won't add it
       if (pz > max_obstacle_height_)
       {
-        printf("The point is too high\n");
+        robot::log_error("The point is too high\n");
         continue;
       }
 
@@ -433,7 +448,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       // if the point is far enough away... we won't consider it
       if (sq_dist >= sq_obstacle_range)
       {
-        printf("The point is too far away\n");
+        robot::log_error("The point is too far away\n");
         continue;
       }
 
@@ -441,7 +456,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
       unsigned int mx, my;
       if (!worldToMap(px, py, mx, my))
       {
-        printf("Computing map coords failed\n");
+        robot::log_error("Computing map coords failed\n");
         continue;
       }
 
@@ -546,7 +561,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
   unsigned int x0, y0;
   if (!worldToMap(ox, oy, x0, y0))
   {
-    printf(
+    robot::log_error(
         "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
         ox, oy);
     return;

plugins/preferred_layer.cpp

@@ -14,7 +14,6 @@ PreferredLayer::~PreferredLayer(){}
 
 unsigned char PreferredLayer::interpretValue(unsigned char value) 
 {
-  printf("TEST PLUGIN !!!\n");
   // check if the static value is above the unknown or lethal thresholds
   if(value == 0) return NO_INFORMATION;
   else if (value >= *this->threshold_)

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);
@@ -201,7 +207,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
     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);
+    robot::log_info("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
 
     // resize costmap if size, resolution or origin do not match
     Costmap2D* master = layered_costmap_->getCostmap();
@@ -213,7 +219,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
         master->getOriginY() != new_map.info.origin.position.y))
     {
       // Update the size of the layered costmap (and all layers, including this one)
-      printf("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
+      robot::log_info("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
       layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
                                   new_map.info.origin.position.y,
                                   true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
@@ -224,7 +230,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
             origin_y_ != new_map.info.origin.position.y)
     {
       // only update the size of the costmap stored locally in this layer
-      printf("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
+      robot::log_info("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
       resizeMap(size_x, size_y, new_map.info.resolution,
                 new_map.info.origin.position.x, new_map.info.origin.position.y);
     }
@@ -247,11 +253,9 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
       {
         unsigned char value = new_map.data[index];
         costmap_[index] = interpretValue(value);
-        // printf("%d , ",costmap_[index]);
         // 3. Ghi giá trị biến
         // file << static_cast<int>(costmap_[index]) << " , ";
         ++index;
-        // printf("%d , ",costmap_[index]);
       }
       // file << std::endl;
     }
@@ -268,13 +272,13 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
   }
   else
   {
-    printf("Stop receive new map!");
+    robot::log_info("Stop receive new map!\n");
   }
 
   // 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\n");
+    robot::log_info("Shutting down the map subscriber. first_map_only flag is on\n");
     map_shutdown_ = true;
   }
 }
@@ -283,7 +287,7 @@ void StaticLayer::incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& upda
 {
   if(!map_update_shutdown_)
   {
-    std::cout << "Update new map!" << std::endl;
+    robot::log_info("Update new map!\n");
     unsigned int di = 0;
     for (unsigned int y = 0; y < update.height ; y++)
     {
@@ -328,7 +332,6 @@ void StaticLayer::reset()
   {
     onInitialize();
   }
-  printf("RESET MAP");
 }
 
 void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
@@ -382,7 +385,7 @@ void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_
     }
     catch (tf3::TransformException ex)
     {
-      printf("%s", ex.what());
+      robot::log_error("StaticLayer::updateCosts(): %s \n", ex.what());
       return;
     }
     // Copy map data given proper transformations

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);
@@ -112,7 +119,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();
   } 
   catch (const YAML::BadFile& e) {
@@ -316,7 +322,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
 
   if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
   {
-    printf(
+    robot::log_error(
         "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;

src/costmap_2d.cpp

@@ -471,15 +471,12 @@ bool Costmap2D::saveMap(std::string file_name)
     return false;
   }
 
-  fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
   for (unsigned int iy = 0; iy < size_y_; iy++)
   {
     for (unsigned int ix = 0; ix < size_x_; ix++)
     {
       unsigned char cost = getCost(ix, iy);
-      fprintf(fp, "%d ", cost);
     }
-    fprintf(fp, "\n");
   }
   fclose(fp);
   return true;

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),
@@ -67,12 +77,11 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
     map_update_thread_(NULL),
     footprint_padding_(0.0)
 {
-  std::signal(SIGINT, signalHandler);
   robot::NodeHandle nh("~");
   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;
@@ -80,23 +89,31 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
   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"))
-      nh.getParam("global_frame", global_frame);
+    if (priv_nh.hasParam("global_frame"))
+      priv_nh.getParam("global_frame", global_frame);
 
     if (nh.hasParam("robot_base_frame"))
       nh.getParam("robot_base_frame", robot_base_frame);
@@ -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,17 +149,41 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
     robot::PluginLoaderHelper loader;
     
 
-    if (nh.hasParam("rolling_window"))
-      nh.getParam("rolling_window", rolling_window);
+    if (priv_nh.hasParam("rolling_window"))
+      priv_nh.getParam("rolling_window", rolling_window);
 
-    if (nh.hasParam("track_unknown_space"))
-      nh.getParam("track_unknown_space", track_unknown_space);
+    if (priv_nh.hasParam("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_);
-
     layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
 
+    // find size parameters
+    double map_width_meters = loadParam(layer, "width", 0.0);
+    double map_height_meters = loadParam(layer, "height", 0.0);
+    double resolution = loadParam(layer, "resolution", 0.0);
+    double origin_x = loadParam(layer, "origin_x", 0.0);
+    double origin_y = loadParam(layer, "origin_y", 0.0);
+
+    if (priv_nh.hasParam("width"))
+      priv_nh.getParam("width", map_width_meters);
+    if (priv_nh.hasParam("height"))
+      priv_nh.getParam("height", map_height_meters);
+    if (priv_nh.hasParam("resolution"))
+      priv_nh.getParam("resolution", resolution);
+    if (priv_nh.hasParam("origin_x"))
+      priv_nh.getParam("origin_x", origin_x);
+    if (priv_nh.hasParam("origin_y"))
+      priv_nh.getParam("origin_y", origin_y);
+
+    if (!layered_costmap_->isSizeLocked())
+    {
+      // robot::log_warning("ROBOT origin_x: %f | origin_y: %f", origin_x, origin_y);
+      layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
+                                  (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
+    }
+    
     struct PluginConfig 
     {
       std::string name;
@@ -148,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)
@@ -205,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)
@@ -232,13 +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);
 
@@ -254,31 +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);
 
-    // find size parameters
-    double map_width_meters = loadParam(layer, "width", 0.0);
-    double map_height_meters = loadParam(layer, "height", 0.0);
-    double resolution = loadParam(layer, "resolution", 0.0);
-    double origin_x = loadParam(layer, "origin_x", 0.0);
-    double origin_y = loadParam(layer, "origin_y", 0.0);
-
-    if (nh.hasParam("update_frequency"))
-      nh.getParam("update_frequency", map_update_frequency);
-    if (nh.hasParam("width"))
-      nh.getParam("width", map_width_meters);
-    if (nh.hasParam("height"))
-      nh.getParam("height", map_height_meters);
-    if (nh.hasParam("resolution"))
-      nh.getParam("resolution", resolution);
-    if (nh.hasParam("origin_x"))
-      nh.getParam("origin_x", origin_x);
-    if (nh.hasParam("origin_y"))
-      nh.getParam("origin_y", origin_y);
-
-    if (!layered_costmap_->isSizeLocked())
-    {
-      layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
-                                  (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
-    }
+    if (priv_nh.hasParam("update_frequency"))
+      priv_nh.getParam("update_frequency", map_update_frequency);
 
     // If the padding has changed, call setUnpaddedRobotFootprint() to
     // re-apply the padding.
@@ -292,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"))
-      nh.getParam("robot_radius", robot_radius);
+    if (priv_nh.hasParam("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
@@ -307,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));
@@ -381,7 +514,7 @@ void Costmap2DROBOT::mapUpdateLoop(double frequency)
     r.sleep();
     // make sure to sleep for the remainder of our cycle time
     if (r.cycleTime() > robot::Duration(1 / frequency))
-      printf("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", frequency,
+      robot::log_warning("Map update %s loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", name_.c_str(), frequency,
                r.cycleTime().toSec());
   }
 }
@@ -397,17 +530,19 @@ void Costmap2DROBOT::updateMap()
       double x = pose.pose.position.x,
              y = pose.pose.position.y,
              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.stamp = robot::Time::now();
-      transformFootprint(x, y, yaw, padded_footprint_, footprint);
+      
+      footprint_.header.frame_id = global_frame_;
+      footprint_.header.stamp = robot::Time::now();
+      transformFootprint(x, y, yaw, padded_footprint_, footprint_);
 
       initialized_ = true;
     }
   }
 }
 
+
 void Costmap2DROBOT::start()
 {
   // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::start");
@@ -431,11 +566,11 @@ void Costmap2DROBOT::start()
   robot::Rate r(100.0);
   while (!initialized_ && map_update_thread_)
   {
-    // if (robot::Time::now() - start_time > robot::Duration(5.0))
-    // {
-    //   printf("Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
-    //   break;
-    // }
+    if (robot::Time::now() - start_time > robot::Duration(5.0))
+    {
+      robot::log_warning_throttle(3.0, "Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
+      break;
+    }
     r.sleep();
   }
 }
@@ -474,7 +609,7 @@ void Costmap2DROBOT::resume()
   {
     if (robot::Time::now() - start_time > robot::Duration(5.0))
     {
-      printf("Costmap2DROBOT::resume() timed out waiting for costmap to initialize\n");
+      robot::log_warning("Costmap2DROBOT::resume() timed out waiting for costmap to initialize\n");
       break;
     }
     r.sleep();
@@ -500,6 +635,13 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
 {
   robot_geometry_msgs::PoseStamped robot_pose;
   robot_geometry_msgs::Pose pose_default;
+  pose_default.orientation.x = 0;
+  pose_default.orientation.y = 0;
+  pose_default.orientation.z = 0;
+  pose_default.orientation.w = 1;
+  pose_default.position.x = 0;
+  pose_default.position.y = 0;
+  pose_default.position.z = 0;
   global_pose.pose = pose_default;
   robot_pose.pose = pose_default;
 
@@ -511,10 +653,22 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
   try
   {
     // use current time if possible (makes sure it's not in the future)
-    if (tf_.canTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time)))
+    if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time()))
     {
-      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time));
+      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_,tf3::Time());
       tf3::doTransform(robot_pose, global_pose, transform);
+      // robot::log_error("%s ||| %f | %f | %f ||| %f | %f | %f | %f", transform.child_frame_id.c_str(),
+      // global_pose.pose.position.x,
+      // global_pose.pose.position.y,
+      // global_pose.pose.position.z,
+      // global_pose.pose.orientation.x,
+      // global_pose.pose.orientation.y,
+      // global_pose.pose.orientation.z,
+      // global_pose.pose.orientation.w);
+      // transform.transform.rotation.x,
+      // transform.transform.rotation.y,
+      // transform.transform.rotation.z,
+      // transform.transform.rotation.w);
     }
     // use the latest otherwise
     else
@@ -523,31 +677,31 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
       tf3::TransformStampedMsg transform = tf_.lookupTransform(
                     global_frame_,                      // frame đích
                     robot_base_frame_,                      // frame nguồn
-                    data_convert::convertTime(robot_pose.header.stamp)
+                    tf3::Time()
                 );
       tf3::doTransform(robot_pose, global_pose, transform);
     }
   }
   catch (tf3::LookupException& ex)
   {
-    printf("Cost Map No Transform available Error looking up robot pose: %s\n", ex.what());
+    robot::log_error("Costmap2DROBOT %s No Transform available Error looking up robot pose: %s\n", name_.c_str(), ex.what());
     return false;
   }
   catch (tf3::ConnectivityException& ex)
   {
-    printf("Connectivity Error looking up robot pose: %s\n", ex.what());
+    robot::log_error("Costmap2DROBOT %s Connectivity Error looking up robot pose: %s\n", name_.c_str(), ex.what());
     return false;
   }
   catch (tf3::ExtrapolationException& ex)
   {
-    printf("Extrapolation Error looking up robot pose: %s\n", 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);
   // check global_pose timeout
   if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_)
   {
-    printf("Costmap2DROBOT transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n",
+    robot::log_warning("Costmap2DROBOT %s transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n", name_.c_str(),
             current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
     return false;
   }

src/footprint.cpp

@@ -174,15 +174,15 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ro
 
   if (error != "")
   {
-    printf("Error parsing footprint parameter: '%s'\n", error.c_str());
-    printf("  Footprint string was '%s'.\n", footprint_string.c_str());
+    robot::log_error("Error parsing footprint parameter: '%s'\n", error.c_str());
+    robot::log_error("  Footprint string was '%s'.\n", footprint_string.c_str());
     return false;
   }
 
   // convert vvf into points.
   if (vvf.size() < 3)
   {
-    printf("You must specify at least three points for the robot footprint, reverting to previous footprint.\n");
+    robot::log_error("You must specify at least three points for the robot footprint, reverting to previous footprint.\n");
     return false;
   }
   footprint.reserve(vvf.size());
@@ -198,7 +198,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ro
     }
     else
     {
-      printf("Points in the footprint specification must be pairs of numbers.  Found a point with %d numbers.\n",
+      robot::log_error("Points in the footprint specification must be pairs of numbers.  Found a point with %d numbers.\n",
                  int(vvf[ i ].size()));
       return false;
     }
@@ -256,7 +256,7 @@ double getNumberFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& value, const std::string
       value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeDouble)
   {
     std::string& value_string = value;
-    printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
+    robot::log_error("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
                full_param_name.c_str(), value_string.c_str());
     throw std::runtime_error("Values in the footprint specification must be numbers");
   }
@@ -270,7 +270,7 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot_xmlrpcpp::
   if (footprint_xmlrpc.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
       footprint_xmlrpc.size() < 3)
   {
-    printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
+    robot::log_error("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
                full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
     throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
                              "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
@@ -286,7 +286,7 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot_xmlrpcpp::
     if (point.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
         point.size() != 2)
     {
-      printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
+      robot::log_error("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
                 "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.\n",
                  full_param_name.c_str());
       throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "

src/layered_costmap.cpp

@@ -111,7 +111,6 @@ namespace robot_costmap_2d
 
     minx_ = miny_ = 1e30;
     maxx_ = maxy_ = -1e30;
-
     for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
          ++plugin)
     {
@@ -124,7 +123,7 @@ namespace robot_costmap_2d
       (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
       if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
       {
-        printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
+        robot::log_error("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
                                "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
                           prev_minx, prev_miny, prev_maxx, prev_maxy,
                           minx_, miny_, maxx_, maxy_,
@@ -141,8 +140,6 @@ namespace robot_costmap_2d
     y0 = std::max(0, y0);
     yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
 
-    printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
-    
     if (xn < x0 || yn < y0)
       return;
 

src/observation_buffer.cpp

@@ -68,7 +68,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
   robot_geometry_msgs::TransformStamped transformStamped;
   if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
   {
-    printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
+    robot::log_error("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
               global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
     return false;
   }
@@ -106,7 +106,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
     }
     catch (TransformException& ex)
     {
-      printf("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
+      robot::log_error("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
                 new_global_frame.c_str(), ex.what());
       return false;
     }
@@ -140,7 +140,8 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
     tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
                         global_frame_,                      // frame đích
                         local_origin.header.frame_id,                      // frame nguồn
-                        data_convert::convertTime(local_origin.header.stamp)
+                        tf3::Time()
+                        // data_convert::convertTime(local_origin.header.stamp)
                         );
     tf3::doTransform(local_origin, global_origin, tfm_1);
 
@@ -161,7 +162,8 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
     tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
                         global_frame_,                      // frame đích
                         cloud.header.frame_id,                      // frame nguồn
-                        data_convert::convertTime(cloud.header.stamp)
+                        tf3::Time()
+                        // data_convert::convertTime(cloud.header.stamp)
                         );
     tf3::doTransform(cloud, global_frame_cloud, tfm_2);
     global_frame_cloud.header.stamp = cloud.header.stamp;
@@ -205,7 +207,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
   {
     // if an exception occurs, we need to remove the empty observation from the list
     observation_list_.pop_front();
-    printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
+    robot::log_error("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
               cloud.header.frame_id.c_str(), ex.what());
     return;
   }
@@ -265,8 +267,7 @@ bool ObservationBuffer::isCurrent() const
   bool current = (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
   if (!current)
   {
-    printf(
-        "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
+    robot::log_error("The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
         topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
   }
   return current;