From f052dac1421e7e2bedba4088780526a8775b0c29 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Wed, 7 Jan 2026 16:55:01 +0700 Subject: [PATCH] update for ROS --- CMakeLists.txt | 446 +++++++++++++++++++++------------------ src/costmap_2d_robot.cpp | 1 - 2 files changed, 237 insertions(+), 210 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fbd6423..41c45e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,43 +1,38 @@ -# --- CMake version và project name --- -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 3.0.2) +project(robot_costmap_2d VERSION 1.0.0 LANGUAGES CXX) -# ======================================================== -# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake -# ======================================================== - -# Detect if building with Catkin if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) set(BUILDING_WITH_CATKIN TRUE) message(STATUS "Building robot_costmap_2d with Catkin") + else() set(BUILDING_WITH_CATKIN FALSE) message(STATUS "Building robot_costmap_2d with Standalone CMake") endif() -project(robot_costmap_2d) +# C++ Standard - must be set before find_package +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) -# --- 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 +# 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) -# --- 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) +if (NOT BUILDING_WITH_CATKIN) -# --- 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 + # Enable Position Independent Code + set(CMAKE_POSITION_INDEPENDENT_CODE ON) -if(BUILDING_WITH_CATKIN) - find_package(catkin REQUIRED COMPONENTS + # 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}") + + set(PACKAGES_DIR robot_std_msgs robot_sensor_msgs geometry_msgs @@ -54,229 +49,262 @@ if(BUILDING_WITH_CATKIN) robot_cpp robot_time ) -endif() + +else() # ======================================================== # 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 + tf3 + robot_tf3_geometry_msgs + robot_tf3_sensor_msgs + data_convert + robot_xmlrpcpp + robot_cpp + robot_time + ) -if(BUILDING_WITH_CATKIN) catkin_package( INCLUDE_DIRS include LIBRARIES robot_costmap_2d plugins CATKIN_DEPENDS robot_std_msgs robot_sensor_msgs geometry_msgs robot_nav_msgs robot_map_msgs robot_laser_geometry robot_visualization_msgs robot_voxel_grid tf3 robot_tf3_geometry_msgs robot_tf3_sensor_msgs data_convert robot_xmlrpcpp robot_cpp robot_time - DEPENDS Eigen3 PCL + DEPENDS PCL Boost ) -endif() -# --- Define macro để dùng trong code --- -add_definitions(-DROBOT_COSTMAP_2D_DIR="${CMAKE_CURRENT_SOURCE_DIR}") - -# --- Include directories --- -# Thêm các folder chứa header files -include_directories( - include + include_directories( + include + ${catkin_INCLUDE_DIRS} ${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(BUILDING_WITH_CATKIN) - include_directories(${catkin_INCLUDE_DIRS}) + ) + link_directories(${PCL_LIBRARY_DIRS}) endif() -# --- Eigen và PCL definitions --- +# 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 --- -# 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 +# ======================================================== +# 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 ) -# --- Link các thư viện phụ thuộc --- -if(BUILDING_WITH_CATKIN) - target_link_libraries(robot_costmap_2d - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} # Boost - yaml-cpp - dl - ${PCL_LIBRARIES} - ) -else() - # Standalone mode: link dependencies with PUBLIC visibility to propagate include paths - target_link_libraries(robot_costmap_2d - PUBLIC - 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 # XMLRPC - robot_cpp - PRIVATE - ${Boost_LIBRARIES} # Boost - yaml-cpp - dl - ${PCL_LIBRARIES} - ) -endif() - -# --- Include directories cho target --- -target_include_directories(robot_costmap_2d - PUBLIC - ${Boost_INCLUDE_DIRS} # Boost headers - $ # Khi build từ source - $ # Khi install - ) - if(BUILDING_WITH_CATKIN) add_dependencies(robot_costmap_2d ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -endif() -# --- 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 -) - -# --- 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 -) - -# --- Cài đặt headers --- -if(NOT BUILDING_WITH_CATKIN) - install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} - ) -endif() - -# --- Plugin libraries --- -# Tạo các plugin shared library -add_library(plugins - SHARED - plugins/static_layer.cpp - plugins/obstacle_layer.cpp - plugins/inflation_layer.cpp - plugins/voxel_layer.cpp - plugins/critical_layer.cpp - plugins/directional_layer.cpp - plugins/preferred_layer.cpp - plugins/unpreferred_layer.cpp - ) - -if(BUILDING_WITH_CATKIN) - target_link_libraries(plugins - PRIVATE - robot_costmap_2d - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - yaml-cpp - ) -else() - # In standalone mode, robot_costmap_2d already has PUBLIC links to all dependencies - # so we just need to link it with PUBLIC visibility to get the include paths - target_link_libraries(plugins + target_include_directories(robot_costmap_2d PUBLIC - robot_costmap_2d - PRIVATE - ${Boost_LIBRARIES} - yaml-cpp - robot_time - robot_cpp + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) + + 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} + ) + +else() + + target_include_directories(robot_costmap_2d + PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ) + + target_link_libraries(robot_costmap_2d + PUBLIC + ${PACKAGES_DIR} + PRIVATE + Boost::boost Boost::system Boost::thread Boost::filesystem + yaml-cpp + dl + ${PCL_LIBRARIES} + ) + + 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() -set_target_properties(plugins PROPERTIES - LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +# ======================================================== +# Plugin library: plugins +# ======================================================== +add_library(plugins SHARED + plugins/static_layer.cpp + plugins/obstacle_layer.cpp + plugins/inflation_layer.cpp + plugins/voxel_layer.cpp + plugins/critical_layer.cpp + 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 + $ + $ + ) + + target_link_libraries(plugins + PUBLIC robot_costmap_2d + PRIVATE ${catkin_LIBRARIES} + PRIVATE Boost::boost Boost::system Boost::thread Boost::filesystem + PRIVATE yaml-cpp + ) + +else() + + target_include_directories(plugins + PUBLIC + $ + $ + ) + + 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 + ) + + set_target_properties(plugins PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} + BUILD_RPATH "${CMAKE_BINARY_DIR}" + INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" + ) + endif() -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 -) +# ======================================================== +# 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} + ) -install(EXPORT plugins-targets - FILE plugins-targets.cmake - NAMESPACE robot_costmap_2d:: - DESTINATION lib/cmake/plugins -) + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) +else() -# --- Option để bật/tắt test --- + 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 --- + 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) + target_link_libraries(test_array_parser PRIVATE + robot_costmap_2d + GTest::GTest + GTest::Main + Threads::Threads + ) + endif() - # --- 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 - robot_costmap_2d - GTest::GTest GTest::Main + 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 + Threads::Threads ) - - # --- 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" + 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 + Threads::Threads + tf3 + robot_time + GTest::GTest + GTest::Main ) - 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() \ No newline at end of file + endif() +endif() diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 46fdf81..4b8f876 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -128,7 +128,6 @@ 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);