add file costmap_2d_robot

CMakeLists.txt

@@ -12,12 +12,12 @@ set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d")
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 # --- Dependencies ---
-# find_package(tf2 REQUIRED)
+# find_package(tf3 REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system thread filesystem)
 find_package(GTest REQUIRED)
 find_package(PCL REQUIRED COMPONENTS common io)
-# set(TF2_LIBRARY /usr/lib/libtf2.so)
+# set(tf3_LIBRARY /usr/lib/libtf3.so)
 
 # --- Include other message packages if needed ---
 # if (NOT TARGET sensor_msgs)
@@ -39,6 +39,8 @@ find_package(PCL REQUIRED COMPONENTS common io)
 #     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../voxel_grid ${CMAKE_BINARY_DIR}/voxel_grid_build)
 # endif()
 
+add_definitions(-DCOSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
+
 include_directories(
     include  
     ${EIGEN3_INCLUDE_DIRS}
@@ -51,7 +53,8 @@ link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${EIGEN3_DEFINITIONS} ${PCL_DEFINITIONS})
 
 # --- Core library ---
-add_library(costmap_2d_new 
+add_library(costmap_2d
+    src/costmap_2d_robot.cpp
     src/array_parser.cpp
     src/costmap_2d.cpp
     src/observation_buffer.cpp
@@ -62,7 +65,7 @@ add_library(costmap_2d_new
     src/costmap_layer.cpp
 )
 
-target_link_libraries(costmap_2d_new
+target_link_libraries(costmap_2d
     ${Boost_LIBRARIES}
     std_msgs
     sensor_msgs
@@ -71,17 +74,22 @@ target_link_libraries(costmap_2d_new
     map_msgs
     laser_geometry
     voxel_grid
-    # ${TF2_LIBRARY}
+    # ${tf3_LIBRARY}
     tf3
 )
-target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS})
+target_include_directories(costmap_2d 
+        PUBLIC ${Boost_INCLUDE_DIRS}
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+        )
+
 
 # --- Plugin libraries ---
 add_library(static_layer SHARED
     plugins/static_layer.cpp
 )
 target_link_libraries(static_layer
-    costmap_2d_new
+    costmap_2d
     ${Boost_LIBRARIES}
     yaml-cpp
 )
@@ -90,7 +98,7 @@ add_library(obstacle_layer SHARED
     plugins/obstacle_layer.cpp
 )
 target_link_libraries(obstacle_layer
-    costmap_2d_new
+    costmap_2d
     ${Boost_LIBRARIES}
     yaml-cpp
 )
@@ -99,7 +107,7 @@ add_library(inflation_layer SHARED
     plugins/inflation_layer.cpp
 )
 target_link_libraries(inflation_layer
-    costmap_2d_new
+    costmap_2d
     ${Boost_LIBRARIES}
     yaml-cpp
 )
@@ -108,7 +116,7 @@ add_library(voxel_layer SHARED
     plugins/voxel_layer.cpp
 )
 target_link_libraries(voxel_layer
-    costmap_2d_new
+    costmap_2d
     ${Boost_LIBRARIES}
     yaml-cpp
 )
@@ -117,7 +125,7 @@ add_library(critical_layer SHARED
     plugins/critical_layer.cpp
 )
 target_link_libraries(critical_layer
-    costmap_2d_new
+    costmap_2d
     static_layer
     ${Boost_LIBRARIES}
     yaml-cpp
@@ -127,7 +135,7 @@ add_library(directional_layer SHARED
     plugins/directional_layer.cpp
 )
 target_link_libraries(directional_layer
-    costmap_2d_new
+    costmap_2d
     static_layer
     ${Boost_LIBRARIES}
     yaml-cpp
@@ -137,7 +145,7 @@ add_library(preferred_layer SHARED
     plugins/preferred_layer.cpp
 )
 target_link_libraries(preferred_layer
-    costmap_2d_new
+    costmap_2d
     static_layer
     ${Boost_LIBRARIES}
     yaml-cpp
@@ -147,20 +155,20 @@ add_library(unpreferred_layer SHARED
     plugins/unpreferred_layer.cpp
 )
 target_link_libraries(unpreferred_layer
-    costmap_2d_new
+    costmap_2d
     static_layer
     ${Boost_LIBRARIES}
     yaml-cpp
 )
 
-add_library(costmap_2d_robot SHARED
+# add_library(costmap_2d_robot SHARED
-    src/costmap_2d_robot.cpp
+#     src/costmap_2d_robot.cpp
-)
+# )
-target_link_libraries(costmap_2d_robot
+# target_link_libraries(costmap_2d_robot
-    costmap_2d_new
+#     costmap_2d
-    ${Boost_LIBRARIES}
+#     ${Boost_LIBRARIES}
-    yaml-cpp
+#     yaml-cpp
-)
+# )
 
 # --- Test executables ---
 add_executable(test_array_parser test/array_parser_test.cpp)
@@ -168,22 +176,22 @@ add_executable(test_costmap test/coordinates_test.cpp)
 add_executable(test_plugin test/static_layer_test.cpp)
 
 target_link_libraries(test_array_parser PRIVATE
-    costmap_2d_new
+    costmap_2d
     GTest::GTest
     GTest::Main
     pthread
 )
 
 target_link_libraries(test_costmap PRIVATE
-    costmap_2d_new
+    costmap_2d
     GTest::GTest
     GTest::Main
     pthread
 )
 
 target_link_libraries(test_plugin PRIVATE
-    ${TF2_LIBRARY}
+    ${tf3_LIBRARY}
-    costmap_2d_new
+    costmap_2d
     static_layer
     obstacle_layer
     inflation_layer

CODE_REVIEW.md

@@ -329,23 +329,23 @@ bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int&
 
 ## 🟠 **VẤN ĐỀ TRONG CMAKELISTS.TXT**
 
-### 9. **Hardcoded path đến tf2 library**
+### 9. **Hardcoded path đến tf3 library**
 
 **File:** `CMakeLists.txt:135`
 
 ```cmake
-set(TF2_LIBRARY /usr/lib/libtf2.so)  # ❌ Hardcoded path
+set(tf3_LIBRARY /usr/lib/libtf3.so)  # ❌ Hardcoded path
 ```
 
 **Vấn đề:**
 - Không portable, sẽ fail trên các hệ thống khác
-- Nên dùng `find_package(tf2)` hoặc `find_library()`
+- Nên dùng `find_package(tf3)` hoặc `find_library()`
 
 **Giải pháp:**
 ```cmake
-find_package(tf2 REQUIRED)
+find_package(tf3 REQUIRED)
 # hoặc
-find_library(TF2_LIBRARY tf2 PATHS /usr/lib /usr/local/lib)
+find_library(tf3_LIBRARY tf3 PATHS /usr/lib /usr/local/lib)
 ```
 
 ---

build/CMakeCache.txt

@@ -364,7 +364,7 @@ boost_thread_DIR:PATH=/usr/lib/x86_64-linux-gnu/cmake/boost_thread-1.71.0
 costmap_2d_BINARY_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d/build
 
 //Dependencies for the target
-costmap_2d_LIB_DEPENDS:STATIC=general;/usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0;general;/usr/lib/x86_64-linux-gnu/libboost_filesystem.so;general;/usr/lib/x86_64-linux-gnu/libboost_date_time.so;general;/usr/lib/x86_64-linux-gnu/libboost_iostreams.so;general;/usr/lib/x86_64-linux-gnu/libboost_regex.so;general;tf2;
+costmap_2d_LIB_DEPENDS:STATIC=general;/usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0;general;/usr/lib/x86_64-linux-gnu/libboost_filesystem.so;general;/usr/lib/x86_64-linux-gnu/libboost_date_time.so;general;/usr/lib/x86_64-linux-gnu/libboost_iostreams.so;general;/usr/lib/x86_64-linux-gnu/libboost_regex.so;general;tf3;
 
 //Value Computed by CMake
 costmap_2d_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/costmap_2d

build/CMakeFiles/costmap_2d.dir/CXX.includecache

@@ -99,7 +99,7 @@ costmap_2d/layered_costmap.h
 -
 string
 -
-tf2/buffer_core.h
+tf3/buffer_core.h
 -
 
 ../include/costmap_2d/layered_costmap.h
@@ -131,7 +131,7 @@ chrono
 -
 costmap_2d/observation.h
 -
-tf2/buffer_core.h
+tf3/buffer_core.h
 -
 sensor_msgs/PointCloud2.h
 -
@@ -249,7 +249,7 @@ vector
 /home/duongtd/robotics_core/costmap_2d/src/observation_buffer.cpp
 costmap_2d/observation_buffer.h
 -
-tf2/convert.h
+tf3/convert.h
 -
 sensor_msgs/point_cloud2_iterator.h
 -

build/CMakeFiles/layers.dir/CXX.includecache

@@ -87,9 +87,9 @@ costmap_2d/static_layer.h
 ../include/costmap_2d/data_convert.h
 geometry_msgs/TransformStamped.h
 -
-tf2/utils.h
+tf3/utils.h
 -
-tf2/compat.h
+tf3/compat.h
 -
 
 ../include/costmap_2d/directional_layer.h
@@ -127,7 +127,7 @@ costmap_2d/layered_costmap.h
 -
 string
 -
-tf2/buffer_core.h
+tf3/buffer_core.h
 -
 
 ../include/costmap_2d/layered_costmap.h
@@ -159,7 +159,7 @@ chrono
 -
 costmap_2d/observation.h
 -
-tf2/buffer_core.h
+tf3/buffer_core.h
 -
 sensor_msgs/PointCloud2.h
 -
@@ -441,7 +441,7 @@ costmap_2d/costmap_math.h
 -
 sensor_msgs/point_cloud2_iterator.h
 -
-tf2/utils.h
+tf3/utils.h
 -
 boost/dll/alias.hpp
 -
@@ -461,9 +461,9 @@ costmap_2d/data_convert.h
 -
 costmap_2d/utils.h
 -
-tf2/convert.h
+tf3/convert.h
 -
-tf2/utils.h
+tf3/utils.h
 -
 boost/dll/alias.hpp
 -

build/CMakeFiles/test_array_parser.dir/link.txt

@@ -1 +1 @@
-/usr/bin/c++       CMakeFiles/test_array_parser.dir/test/array_parser_test.cpp.o  -o test_array_parser  libcostmap_2d.a -lpthread /usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0 /usr/lib/x86_64-linux-gnu/libboost_filesystem.so /usr/lib/x86_64-linux-gnu/libboost_date_time.so /usr/lib/x86_64-linux-gnu/libboost_iostreams.so /usr/lib/x86_64-linux-gnu/libboost_regex.so -lyaml-cpp robot_time_build/librobot_time.a -ltf2 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread 
+/usr/bin/c++       CMakeFiles/test_array_parser.dir/test/array_parser_test.cpp.o  -o test_array_parser  libcostmap_2d.a -lpthread /usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0 /usr/lib/x86_64-linux-gnu/libboost_filesystem.so /usr/lib/x86_64-linux-gnu/libboost_date_time.so /usr/lib/x86_64-linux-gnu/libboost_iostreams.so /usr/lib/x86_64-linux-gnu/libboost_regex.so -lyaml-cpp robot_time_build/librobot_time.a -ltf3 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread 

build/CMakeFiles/test_costmap.dir/link.txt

@@ -1 +1 @@
-/usr/bin/c++       CMakeFiles/test_costmap.dir/test/coordinates_test.cpp.o  -o test_costmap  libcostmap_2d.a -lpthread /usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0 /usr/lib/x86_64-linux-gnu/libboost_filesystem.so /usr/lib/x86_64-linux-gnu/libboost_date_time.so /usr/lib/x86_64-linux-gnu/libboost_iostreams.so /usr/lib/x86_64-linux-gnu/libboost_regex.so -lyaml-cpp robot_time_build/librobot_time.a -ltf2 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread 
+/usr/bin/c++       CMakeFiles/test_costmap.dir/test/coordinates_test.cpp.o  -o test_costmap  libcostmap_2d.a -lpthread /usr/lib/x86_64-linux-gnu/libboost_system.so.1.71.0 /usr/lib/x86_64-linux-gnu/libboost_filesystem.so /usr/lib/x86_64-linux-gnu/libboost_date_time.so /usr/lib/x86_64-linux-gnu/libboost_iostreams.so /usr/lib/x86_64-linux-gnu/libboost_regex.so -lyaml-cpp robot_time_build/librobot_time.a -ltf3 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread 

include/costmap_2d/costmap_2d_robot.h

@@ -40,14 +40,10 @@
 
 #include <costmap_2d/layered_costmap.h>
 #include <costmap_2d/layer.h>
-// #include <costmap_2d/costmap_2d_publisher.h>
-// #include <costmap_2d/Costmap2DConfig.h>
 #include <costmap_2d/footprint.h>
 #include <geometry_msgs/Polygon.h>
 #include <geometry_msgs/PolygonStamped.h>
 #include <geometry_msgs/PoseStamped.h>
-// #include <dynamic_reconfigure/server.h>
-// #include <pluginlib/class_loader.hpp>
 #include <tf3/LinearMath/Transform.h>
 #include <robot/rate.h>
 #include <costmap_2d/data_convert.h>

include/costmap_2d/data_convert.h

@@ -83,7 +83,7 @@ namespace costmap_2d
         return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
     }
     
-    tf3::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
+    tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
     {
         tf3::Quaternion q(
             msg.rotation.x,
@@ -102,7 +102,7 @@ namespace costmap_2d
         return tf;
     }
 
-    tf3::Transform convertToTf2Transform(const tf3::TransformMsg& msg)
+    tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
     {
         tf3::Quaternion q(
             msg.rotation.x,

include/costmap_2d/observation_buffer.h

@@ -37,14 +37,14 @@ public:
    * @param  max_obstacle_height The minimum height of a hitpoint to be considered legal
    * @param  obstacle_range The range to which the sensor should be trusted for inserting obstacles
    * @param  raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
-   * @param  tf2_buffer A reference to a tf3 Buffer
+   * @param  tf3_buffer A reference to a tf3 Buffer
    * @param  global_frame The frame to transform PointClouds into
    * @param  sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
    * @param  tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
    */
   ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
-                    double raytrace_range, tf3::BufferCore& tf2_buffer, std::string global_frame,
+                    double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
                     std::string sensor_frame, double tf_tolerance);
 
   /**
@@ -107,7 +107,7 @@ private:
    */
   void purgeStaleObservations();
 
-  tf3::BufferCore& tf2_buffer_;
+  tf3::BufferCore& tf3_buffer_;
   // const ros::Duration observation_keep_time_;
   // const ros::Duration expected_update_rate_;
   // ros::Time last_updated_;

include/costmap_2d/obstacle_layer.h

@@ -52,7 +52,7 @@
 #include <sensor_msgs/point_cloud_conversion.h>
 
 #include <laser_geometry/laser_geometry.hpp>
-// #include <tf2_ros/message_filter.h>
+// #include <tf3_ros/message_filter.h>
 // #include <message_filters/subscriber.h>
 
 namespace costmap_2d

include/costmap_2d/testing_helper.h

@@ -49,14 +49,14 @@ unsigned int countValues(costmap_2d::Costmap2D& costmap, unsigned char value, bo
   return count;
 }
 
-void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
+void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
 {
   costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer();
   layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
   slayer->initialize(&layers, "static", &tf);
 }
 
-costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
+costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
 {
   costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer();
   olayer->initialize(&layers, "obstacles", &tf);
@@ -86,7 +86,7 @@ void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, doubl
   olayer->addStaticObservation(obs, true, true);
 }
 
-costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
+costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
 {
   costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
   ilayer->initialize(&layers, "inflation", &tf);

include/costmap_2d/utils.h

@@ -1,6 +1,9 @@
 #ifndef COSTMAP_2D_UTILS_H
 #define COSTMAP_2D_UTILS_H
 #include <yaml-cpp/yaml.h>
+#include <filesystem>
+#include <string>
+#include <iostream>
 
 namespace costmap_2d
 {
@@ -12,6 +15,29 @@ namespace costmap_2d
         return default_value;
     }
 
+    std::string getSourceFile(const std::string& root, const std::string& config_file_name)
+    {
+        std::string path_source = " ";
+        std::string sub_folder = "config";
+        namespace fs = std::filesystem;
+
+        fs::path cfg_dir = fs::path(root) / sub_folder;
+
+        if (fs::exists(cfg_dir) && fs::is_directory(cfg_dir))
+        {
+            for (const auto& entry : fs::recursive_directory_iterator(cfg_dir))
+            {
+                if (entry.is_regular_file() && entry.path().filename() == config_file_name)
+                {
+                    path_source = entry.path().string();
+                    break;  // tìm thấy thì dừng
+                }
+            }
+        }
+        
+        return path_source;
+    }
+
 }
 
 #endif  // COSTMAP_2D_UTILS_H

plugins/directional_layer.cpp

@@ -412,12 +412,12 @@ namespace costmap_2d
       return false;
     }
     // Copy map data given proper transformations
-    tf3::Transform tf2_transform;
+    tf3::Transform tf3_transform;
-    tf2_transform = convertToTf2Transform(transformMsg.transform);
+    tf3_transform = convertTotf3Transform(transformMsg.transform);
-    x = tf2_transform.getOrigin().x();
+    x = tf3_transform.getOrigin().x();
-    y = tf2_transform.getOrigin().y();
+    y = tf3_transform.getOrigin().y();
     // Extract the rotation as a quaternion from the transform
-    tf3::Quaternion rotation = tf2_transform.getRotation();
+    tf3::Quaternion rotation = tf3_transform.getRotation();
     // Convert the quaternion to a yaw angle (in radians)
     yaw = tf3::getYaw(rotation);
     return true;

plugins/static_layer.cpp

@@ -6,7 +6,9 @@
 #include <tf3/convert.h>
 #include <tf3/utils.h>
 #include <boost/dll/alias.hpp>
-
+#include <filesystem>
+#include <string>
+#include <iostream>
 
 using costmap_2d::NO_INFORMATION;
 using costmap_2d::LETHAL_OBSTACLE;
@@ -35,7 +37,22 @@ void StaticLayer::onInitialize()
 bool StaticLayer::getParams()
 {
     try {
-        YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml");
+        std::string config_file_name = "config.yaml";
+        std::string folder = COSTMAP_2D_DIR;
+
+
+        std::string path_source = getSourceFile(folder,config_file_name);
+        if(path_source != " ")
+        {
+          std::cout << "Path source: " << path_source << std::endl;
+        }
+        else
+        {
+          std::cout << "/cfg folder not found!" << std::endl;
+        }
+
+        
+        YAML::Node config = YAML::LoadFile(path_source);
         YAML::Node layer = config["static_layer"];
 
 
@@ -299,9 +316,9 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
       return;
     }
     // Copy map data given proper transformations
-    tf3::Transform tf2_transform;
+    tf3::Transform tf3_transform;
-    tf2_transform = convertToTf2Transform(transformMsg.transform);
+    tf3_transform = convertTotf3Transform(transformMsg.transform);
-    // tf3::convert(transformMsg.transform, tf2_transform);
+    // tf3::convert(transformMsg.transform, tf3_transform);
     for (unsigned int i = min_i; i < max_i; ++i)
     {
       for (unsigned int j = min_j; j < max_j; ++j)
@@ -310,7 +327,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
         layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
         // Transform from global_frame_ to map_frame_
         tf3::Vector3 p(wx, wy, 0);
-        p = tf2_transform*p;
+        p = tf3_transform*p;
         // Set master_grid with cell from map
         if (worldToMap(p.x(), p.y(), mx, my))
         {

plugins/voxel_layer.cpp

@@ -65,15 +65,15 @@ bool VoxelLayer::getParams()
         YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml");
         YAML::Node layer = config["voxel_layer"];
 
-        publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
+        // publish_voxel_ = loadParam(layer, "publish_voxel_map", false);
         enabled_ = loadParam(layer, "enabled", true);
         footprint_clearing_enabled_ = loadParam(layer, "footprint_clearing_enabled", true);
-        max_obstacle_height_ = loadParam(layer, "max_obstacle_height", true);
+        max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0);
-        size_z_ = loadParam(layer, "z_voxels", true);
+        size_z_ = loadParam(layer, "z_voxels", 10);
-        origin_z_ = loadParam(layer, "origin_z", true);
+        origin_z_ = loadParam(layer, "origin_z", 0);
-        z_resolution_ = loadParam(layer, "z_resolution", true);
+        z_resolution_ = loadParam(layer, "z_resolution", 0.2);
-        unknown_threshold_ = loadParam(layer, "max_obstacle_height", true);
+        unknown_threshold_ = loadParam(layer, "max_obstacle_height", 15);
-        mark_threshold_ = loadParam(layer, "mark_threshold", true);
+        mark_threshold_ = loadParam(layer, "mark_threshold", 0);
         combination_method_ = loadParam(layer, "combination_method", true);
         this->matchSize();
     } 

src/costmap_2d_robot.cpp

@@ -35,15 +35,20 @@
  * Author: Eitan Marder-Eppstein
  *         David V. Lu!!
  *********************************************************************/
-#include <costmap_2d/layered_costmap.h>
-#include <costmap_2d/costmap_2d_robot.h>
 #include <cstdio>
 #include <string>
 #include <algorithm>
 #include <vector>
+
+ #include <costmap_2d/layered_costmap.h>
+#include <costmap_2d/costmap_2d_robot.h>
+#include <costmap_2d/utils.h>
+
 #include <tf3/convert.h>
 #include <tf3/utils.h>
-// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <exception>
+
+// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 using namespace std;
 
@@ -85,7 +90,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
 //   ros::Time last_error = ros::Time::now();
 //   std::string tf_error;
   
-//   // we need to make sure that the transform between the robot base frame and the global frame is available
+// //   // we need to make sure that the transform between the robot base frame and the global frame is available
 //   while (ros::ok()
 //       && !tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &tf_error))
 //   { 
@@ -174,8 +179,58 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
 
 void Costmap2DROBOT::getParams()
 {
-  //   private_nh.param("global_frame", global_frame_, std::string("map"));
+  YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml");
-//   private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
+  YAML::Node layer = config["static_layer"];
+
+  global_frame_ = loadParam(layer, "global_frame", std::string("map"));
+  robot_base_frame_ = loadParam(layer, "robot_base_frame", std::string("base_link"));
+  robot::Time last_error = robot::Time::now();
+  std::string tf_error;
+
+  // check if we want a rolling window version of the costmap
+  bool rolling_window, track_unknown_space, always_send_full_costmap;
+  rolling_window = loadParam(layer, "rolling_window", false);
+  track_unknown_space = loadParam(layer, "track_unknown_space", false);
+  always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false);
+  
+  layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
+
+  struct PluginInfo { std::string path; std::string name; };
+  std::vector<PluginInfo> plugins_to_load = {
+      {"./costmap_2d/libstatic_layer.so", "static_layer"},
+      {"./costmap_2d/libinflation_layer.so", "inflation_layer"},
+      {"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
+      {"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
+  };
+
+  // if (private_nh.hasParam("plugins"))
+  // {
+    // my_list = loadParam(layer, "plugins", my_list);
+    // for (int32_t i = 0; i < my_list.size(); ++i)
+    // {
+      // std::string pname = static_cast<std::string>(my_list[i]["name"]);
+      // std::string type = static_cast<std::string>(my_list[i]["type"]);
+      // printf("%s: Using plugin \"%s\" with type %s\n", name_.c_str(), pname.c_str(), type.c_str());
+  for (auto& info : plugins_to_load)
+  {
+    try
+    {
+      // copyParentParameters(pname, type, private_nh);
+      auto creator = boost::dll::import_alias<PluginLayerPtr()>(
+          info.path, "create_plugin", boost::dll::load_mode::append_decorations
+      );
+      PluginLayerPtr plugin = creator();
+      std::cout << "Plugin created: " << info.name << std::endl;
+      plugin->initialize(layered_costmap_, info.name, &tf_);
+      layered_costmap_->addPlugin(plugin);
+    }
+    catch (std::exception &ex)
+    {
+      printf("Failed to create the s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", ex.what());
+    }
+  }
+    // }
+  // }
 }
 
 void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
@@ -575,17 +630,17 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
   // get the global pose of the robot
   try
   {
-    // // use current time if possible (makes sure it's not in the future)
+    // use current time if possible (makes sure it's not in the future)
-    // if (tf_.canTransform(global_frame_, robot_base_frame_, current_time))
+    if (tf_.canTransform(global_frame_, robot_base_frame_, convertTime(current_time)))
-    // {
+    {
-    //   geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time);
+      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, convertTime(current_time));
-    //   tf3::doTransform(robot_pose, global_pose, transform);
+      // tf3::doTransform(robot_pose, global_pose, transform);
-    // }
+    }
-    // // use the latest otherwise
+    // use the latest otherwise
-    // else
+    else
-    // {
+    {
-    //   tf_.transform(robot_pose, global_pose, global_frame_);
+      // tf_.transform(robot_pose, global_pose, global_frame_);
-    // }
+    }
   }
   catch (tf3::LookupException& ex)
   {

src/observation_buffer.cpp

@@ -1,7 +1,7 @@
 #include <costmap_2d/observation_buffer.h>
 #include <costmap_2d/data_convert.h>
-// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
-// #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+// #include <tf3_sensor_msgs/tf3_sensor_msgs.h>
 #include<tf3/convert.h>
 #include <sensor_msgs/point_cloud2_iterator.h>
 #include <cstdint>
@@ -16,9 +16,9 @@ namespace costmap_2d
 {
 ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
                                      double min_obstacle_height, double max_obstacle_height, double obstacle_range,
-                                     double raytrace_range, tf3::BufferCore& tf2_buffer, string global_frame,
+                                     double raytrace_range, tf3::BufferCore& tf3_buffer, string global_frame,
                                      string sensor_frame, double tf_tolerance) :
-    tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
+    tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
     last_updated_(robot::Time::now()), 
     global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
     min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
@@ -36,7 +36,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
 
   std::string tf_error;
 
-  if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &tf_error))
+  if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &tf_error))
   {
     printf("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());
@@ -57,14 +57,14 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
 
       // we need to transform the origin of the observation to the new global frame
       tf3::doTransform(origin, origin, 
-        tf2_buffer_.lookupTransform(new_global_frame, 
+        tf3_buffer_.lookupTransform(new_global_frame, 
                         origin.header.frame_id,
                         convertTime(origin.header.stamp)));
       obs.origin_ = origin.point;
 
       // we also need to transform the cloud of the observation to the new global frame
       tf3::doTransform(*(obs.cloud_), *(obs.cloud_), 
-        tf2_buffer_.lookupTransform(new_global_frame, 
+        tf3_buffer_.lookupTransform(new_global_frame, 
                         obs.cloud_->header.frame_id,
                         convertTime(obs.cloud_->header.stamp)));
     }
@@ -101,7 +101,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
     local_origin.point.y = 0;
     local_origin.point.z = 0;
     tf3::doTransform(local_origin, global_origin, 
-        tf2_buffer_.lookupTransform(global_frame_, 
+        tf3_buffer_.lookupTransform(global_frame_, 
                         local_origin.header.frame_id,
                         convertTime(local_origin.header.stamp)));
     tf3::convert(global_origin.point, observation_list_.front().origin_);
@@ -114,7 +114,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
 
     // transform the point cloud
     tf3::doTransform(cloud, global_frame_cloud, 
-        tf2_buffer_.lookupTransform(global_frame_, 
+        tf3_buffer_.lookupTransform(global_frame_, 
                         (cloud.header.frame_id),
                         convertTime(cloud.header.stamp)));
     global_frame_cloud.header.stamp = cloud.header.stamp;