From 59ec58a018c1d0a5dedd298356813fdb7bbdab6d Mon Sep 17 00:00:00 2001 From: duongtd Date: Tue, 18 Nov 2025 11:44:01 +0700 Subject: [PATCH] add file costmap_2d_robot --- CMakeLists.txt | 60 +++++++------ CODE_REVIEW.md | 10 +-- build/CMakeCache.txt | 2 +- .../costmap_2d.dir/CXX.includecache | 6 +- build/CMakeFiles/layers.dir/CXX.includecache | 14 +-- .../CMakeFiles/test_array_parser.dir/link.txt | 2 +- build/CMakeFiles/test_costmap.dir/link.txt | 2 +- include/costmap_2d/costmap_2d_robot.h | 4 - include/costmap_2d/data_convert.h | 4 +- include/costmap_2d/observation_buffer.h | 6 +- include/costmap_2d/obstacle_layer.h | 2 +- include/costmap_2d/testing_helper.h | 6 +- include/costmap_2d/utils.h | 26 ++++++ plugins/directional_layer.cpp | 10 +-- plugins/static_layer.cpp | 29 ++++-- plugins/voxel_layer.cpp | 14 +-- src/costmap_2d_robot.cpp | 89 +++++++++++++++---- src/observation_buffer.cpp | 18 ++-- 18 files changed, 203 insertions(+), 101 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cb7f620..0975d9f 100644 --- a/CMakeLists.txt +++ b/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} + $ + $ + ) + # --- 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 - src/costmap_2d_robot.cpp -) -target_link_libraries(costmap_2d_robot - costmap_2d_new - ${Boost_LIBRARIES} - yaml-cpp -) +# add_library(costmap_2d_robot SHARED +# src/costmap_2d_robot.cpp +# ) +# target_link_libraries(costmap_2d_robot +# costmap_2d +# ${Boost_LIBRARIES} +# 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} - costmap_2d_new + ${tf3_LIBRARY} + costmap_2d static_layer obstacle_layer inflation_layer diff --git a/CODE_REVIEW.md b/CODE_REVIEW.md index ff15cca..7ae596a 100644 --- a/CODE_REVIEW.md +++ b/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) ``` --- diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt index 84dd3a3..064bc2c 100644 --- a/build/CMakeCache.txt +++ b/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 diff --git a/build/CMakeFiles/costmap_2d.dir/CXX.includecache b/build/CMakeFiles/costmap_2d.dir/CXX.includecache index 2df4a64..2369d5f 100644 --- a/build/CMakeFiles/costmap_2d.dir/CXX.includecache +++ b/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 - diff --git a/build/CMakeFiles/layers.dir/CXX.includecache b/build/CMakeFiles/layers.dir/CXX.includecache index ffaf3b5..4d034aa 100644 --- a/build/CMakeFiles/layers.dir/CXX.includecache +++ b/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 - diff --git a/build/CMakeFiles/test_array_parser.dir/link.txt b/build/CMakeFiles/test_array_parser.dir/link.txt index 6ce2f6f..368cd1c 100644 --- a/build/CMakeFiles/test_array_parser.dir/link.txt +++ b/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 diff --git a/build/CMakeFiles/test_costmap.dir/link.txt b/build/CMakeFiles/test_costmap.dir/link.txt index 17989f0..325ef3a 100644 --- a/build/CMakeFiles/test_costmap.dir/link.txt +++ b/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 diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/costmap_2d/costmap_2d_robot.h index 7100c74..53c3250 100644 --- a/include/costmap_2d/costmap_2d_robot.h +++ b/include/costmap_2d/costmap_2d_robot.h @@ -40,14 +40,10 @@ #include #include -// #include -// #include #include #include #include #include -// #include -// #include #include #include #include diff --git a/include/costmap_2d/data_convert.h b/include/costmap_2d/data_convert.h index 617844a..c6ed5d7 100644 --- a/include/costmap_2d/data_convert.h +++ b/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, diff --git a/include/costmap_2d/observation_buffer.h b/include/costmap_2d/observation_buffer.h index 5036b0a..7238409 100644 --- a/include/costmap_2d/observation_buffer.h +++ b/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_; diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h index bf813d9..666621a 100644 --- a/include/costmap_2d/obstacle_layer.h +++ b/include/costmap_2d/obstacle_layer.h @@ -52,7 +52,7 @@ #include #include -// #include +// #include // #include namespace costmap_2d diff --git a/include/costmap_2d/testing_helper.h b/include/costmap_2d/testing_helper.h index 0a2bdc0..81f9c3e 100644 --- a/include/costmap_2d/testing_helper.h +++ b/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(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); diff --git a/include/costmap_2d/utils.h b/include/costmap_2d/utils.h index aee2b9f..51fa0bd 100644 --- a/include/costmap_2d/utils.h +++ b/include/costmap_2d/utils.h @@ -1,6 +1,9 @@ #ifndef COSTMAP_2D_UTILS_H #define COSTMAP_2D_UTILS_H #include +#include +#include +#include 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 \ No newline at end of file diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp index c3852a4..757bd9c 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -412,12 +412,12 @@ namespace costmap_2d return false; } // Copy map data given proper transformations - tf3::Transform tf2_transform; - tf2_transform = convertToTf2Transform(transformMsg.transform); - x = tf2_transform.getOrigin().x(); - y = tf2_transform.getOrigin().y(); + tf3::Transform tf3_transform; + tf3_transform = convertTotf3Transform(transformMsg.transform); + x = tf3_transform.getOrigin().x(); + 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; diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index 5842e83..2e73d60 100644 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -6,7 +6,9 @@ #include #include #include - +#include +#include +#include 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; - tf2_transform = convertToTf2Transform(transformMsg.transform); - // tf3::convert(transformMsg.transform, tf2_transform); + tf3::Transform tf3_transform; + tf3_transform = convertTotf3Transform(transformMsg.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)) { diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index 3d82878..932ab7f 100644 --- a/plugins/voxel_layer.cpp +++ b/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); - size_z_ = loadParam(layer, "z_voxels", true); - origin_z_ = loadParam(layer, "origin_z", true); - z_resolution_ = loadParam(layer, "z_resolution", true); - unknown_threshold_ = loadParam(layer, "max_obstacle_height", true); - mark_threshold_ = loadParam(layer, "mark_threshold", true); + max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0); + size_z_ = loadParam(layer, "z_voxels", 10); + origin_z_ = loadParam(layer, "origin_z", 0); + z_resolution_ = loadParam(layer, "z_resolution", 0.2); + unknown_threshold_ = loadParam(layer, "max_obstacle_height", 15); + mark_threshold_ = loadParam(layer, "mark_threshold", 0); combination_method_ = loadParam(layer, "combination_method", true); this->matchSize(); } diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index a50d36a..4001b32 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -35,15 +35,20 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#include -#include #include #include #include #include + + #include +#include +#include + #include #include -// #include +#include + +// #include 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")); -// private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link")); + YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml"); + 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 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(my_list[i]["name"]); + // std::string type = static_cast(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( + 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) - // if (tf_.canTransform(global_frame_, robot_base_frame_, current_time)) - // { - // geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time); - // tf3::doTransform(robot_pose, global_pose, transform); - // } - // // use the latest otherwise - // else - // { - // tf_.transform(robot_pose, global_pose, global_frame_); - // } + // use current time if possible (makes sure it's not in the future) + if (tf_.canTransform(global_frame_, robot_base_frame_, convertTime(current_time))) + { + tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, convertTime(current_time)); + // tf3::doTransform(robot_pose, global_pose, transform); + } + // use the latest otherwise + else + { + // tf_.transform(robot_pose, global_pose, global_frame_); + } } catch (tf3::LookupException& ex) { diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index 8794e98..061cc39 100644 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -1,7 +1,7 @@ #include #include -// #include -// #include +// #include +// #include #include #include #include @@ -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;