update for ROS

2026-01-07 17:01:51 +07:00
parent ca9e100bd9
commit 768573b2bb
34 changed files with 2630 additions and 1963 deletions
--- a/src/Navigations/Cores/move_base_core/CMakeLists.txt
+++ b/src/Navigations/Cores/move_base_core/CMakeLists.txt
@@ -1,84 +1,150 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
 project(move_base_core VERSION 1.0.0 LANGUAGES CXX)

-# Chuẩn C++
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# ========================================================
-# Catkin specific configuration
-# ========================================================
 if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
  set(BUILDING_WITH_CATKIN TRUE)
  message(STATUS "Building move_base_core with Catkin")
+
 else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building move_base_core with Standalone CMake")
 endif()

-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS tf3 robot_time robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs)
+# C++ Standard - must be set before find_package
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+if (NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # 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  
+    tf3
+    robot_time
+    robot_geometry_msgs
+    robot_protocol_msgs
+    robot_nav_2d_msgs
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    tf3
+    robot_time
+    robot_geometry_msgs
+    robot_protocol_msgs
+    robot_nav_2d_msgs
+  )

  catkin_package(
    INCLUDE_DIRS include
-    LIBRARIES move_base_core
+    # LIBRARIES không cần vì đây là header-only library
    CATKIN_DEPENDS tf3 robot_time robot_geometry_msgs robot_protocol_msgs robot_nav_2d_msgs
-    # CATKIN_DEPENDS không cần vì dependencies không phải Catkin packages
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
  )
 endif()

 # Tìm tất cả header files
-file(GLOB HEADERS "include/move_base_core/*.h")
+file(GLOB_RECURSE HEADERS "include/move_base_core/*.h")
+
 # Tạo INTERFACE library (header-only)
-add_library(move_base_core INTERFACE)
+add_library(${PROJECT_NAME} INTERFACE)

-# Set include directories trước khi link libraries
-target_include_directories(move_base_core
-    INTERFACE 
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-
-# Link với dependencies - include directories sẽ được truyền tự động qua INTERFACE properties
 if(BUILDING_WITH_CATKIN)
-  target_link_libraries(move_base_core INTERFACE ${catkin_TARGETS})
-else()
-  target_link_libraries(move_base_core 
-  INTERFACE 
-      tf3 
-      robot_time 
-      robot_geometry_msgs 
-      robot_protocol_msgs
-      robot_nav_2d_msgs  # Thêm vì navigation.h include robot_nav_2d_msgs/Twist2DStamped.h
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${catkin_LIBRARIES}
+  )
+
+else()
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${PACKAGES_DIR}
+  )
+
 endif()

-# Cài đặt header files
-if(NOT BUILDING_WITH_CATKIN)
-  install(DIRECTORY include/move_base_core 
-          DESTINATION include
-          FILES_MATCHING PATTERN "*.h")
-endif()
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )

-# Cài đặt target
-install(TARGETS move_base_core 
-        EXPORT move_base_core-targets
-        LIBRARY DESTINATION lib
-        ARCHIVE DESTINATION lib
-        RUNTIME DESTINATION bin)
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )

-# Export targets
-install(EXPORT move_base_core-targets
-         FILE move_base_core-targets.cmake
-        DESTINATION lib/cmake/move_base_core)
+else()

-# In thông tin cấu hình
-message(STATUS "=================================")
-message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
-message(STATUS "Version: ${PROJECT_VERSION}")
-message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
-message(STATUS "Headers found:")
-foreach(hdr ${HEADERS})
+  install(TARGETS ${PROJECT_NAME}
+    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} (Header-Only)")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Headers found:")
+  foreach(hdr ${HEADERS})
    message(STATUS "  - ${hdr}")
-endforeach()
-message(STATUS "=================================")
+  endforeach()
+  message(STATUS "Dependencies: tf3, robot_time, robot_geometry_msgs, robot_protocol_msgs, robot_nav_2d_msgs")
+  message(STATUS "=================================")
+endif()
--- a/src/Navigations/Cores/robot_nav_core/CMakeLists.txt
+++ b/src/Navigations/Cores/robot_nav_core/CMakeLists.txt
@@ -1,120 +1,146 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_nav_core 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_nav_core with Catkin")
+
 else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building robot_nav_core with Standalone CMake")
 endif()

-# Tên dự án
-project(robot_nav_core VERSION 1.0.0 LANGUAGES CXX)
-
-# Chuẩn C++
+# C++ Standard - must be set before find_package
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)

-# ========================================================
-# Find dependencies
-# ========================================================
+if (NOT BUILDING_WITH_CATKIN)

-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # 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_costmap_2d
    tf3
    robot_protocol_msgs
  )
-endif()
+
+else()

 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_costmap_2d
+    tf3
+    robot_protocol_msgs
+  )

-if(BUILDING_WITH_CATKIN)
  catkin_package(
    INCLUDE_DIRS include
-    # Header-only library; keep LIBRARIES for visibility when exporting
-    LIBRARIES robot_nav_core
+    # LIBRARIES không cần vì đây là header-only library
    CATKIN_DEPENDS robot_costmap_2d tf3 robot_protocol_msgs
  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+  )
 endif()

-# Cho phép các project khác include được header của robot_nav_core
-set(${PROJECT_NAME}_INCLUDE_DIRS
-    ${CMAKE_CURRENT_SOURCE_DIR}/include
-    PARENT_SCOPE
-)
-
-include_directories(
-        ${PROJECT_SOURCE_DIR}/include
-)
-
-if(BUILDING_WITH_CATKIN)
-  include_directories(${catkin_INCLUDE_DIRS})
-endif()
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_nav_core/*.h")

 # Tạo INTERFACE library (header-only)
 add_library(${PROJECT_NAME} INTERFACE)

-# Link libraries
-if(BUILDING_WITH_CATKIN)
-  target_link_libraries(${PROJECT_NAME} INTERFACE 
-      ${catkin_LIBRARIES}
-  )
-else()
-  target_link_libraries(${PROJECT_NAME} INTERFACE 
-      robot_costmap_2d 
-      tf3 
-      robot_protocol_msgs
-  )
-endif()
-
 if(BUILDING_WITH_CATKIN)
  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-endif()

-# Set include directories
-target_include_directories(${PROJECT_NAME}
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
    INTERFACE 
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )

-# ========================================================
-# Installation
-# ========================================================
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${catkin_LIBRARIES}
+  )

-install(TARGETS ${PROJECT_NAME} 
-        EXPORT ${PROJECT_NAME}-targets
-        LIBRARY DESTINATION lib
-        ARCHIVE DESTINATION lib
-        RUNTIME DESTINATION bin)
+else()
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${PACKAGES_DIR}
+  )

-if(NOT BUILDING_WITH_CATKIN)
-  # Cài đặt header files (standalone)
-  install(DIRECTORY include/${PROJECT_NAME} 
-          DESTINATION include
-          FILES_MATCHING PATTERN "*.h")
 endif()

-# Export targets
-install(EXPORT ${PROJECT_NAME}-targets
-         FILE ${PROJECT_NAME}-targets.cmake
-        DESTINATION lib/cmake/${PROJECT_NAME})
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )

-# In thông tin cấu hình
-message(STATUS "=================================")
-message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
-message(STATUS "Version: ${PROJECT_VERSION}")
-message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
-message(STATUS "Headers found:")
-foreach(hdr ${HEADERS})
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+    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} (Header-Only)")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Headers found:")
+  foreach(hdr ${HEADERS})
    message(STATUS "  - ${hdr}")
-endforeach()
-message(STATUS "=================================")
+  endforeach()
+  message(STATUS "Dependencies: robot_costmap_2d, tf3, robot_protocol_msgs")
+  message(STATUS "=================================")
+endif()
--- a/src/Navigations/Cores/robot_nav_core2/CMakeLists.txt
+++ b/src/Navigations/Cores/robot_nav_core2/CMakeLists.txt
@@ -1,138 +1,152 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_nav_core2 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_nav_core2 with Catkin")
+
 else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building robot_nav_core2 with Standalone CMake")
 endif()

-# Tên dự án
-project(robot_nav_core2 VERSION 1.0.0 LANGUAGES CXX)
-
-# Chuẩn C++
+# C++ Standard - must be set before find_package
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)

-# ========================================================
-# Find dependencies
-# ========================================================
+if (NOT BUILDING_WITH_CATKIN)

-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # 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_costmap_2d
    tf3
    nav_grid
    robot_nav_2d_msgs
    robot_cpp
-    robot_sensor_msgs
-    robot_std_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_geometry_msgs
  )
-endif()
+
+else()

 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_costmap_2d
+    tf3
+    nav_grid
+    robot_nav_2d_msgs
+    robot_cpp
+    robot_geometry_msgs
+  )

-if(BUILDING_WITH_CATKIN)
  catkin_package(
    INCLUDE_DIRS include
-    # Header-only library; keep LIBRARIES for visibility when exporting
-    LIBRARIES robot_nav_core2
-    CATKIN_DEPENDS robot_costmap_2d tf3 nav_grid robot_nav_2d_msgs robot_cpp
+    # LIBRARIES không cần vì đây là header-only library
+    CATKIN_DEPENDS robot_costmap_2d tf3 nav_grid robot_nav_2d_msgs robot_cpp robot_geometry_msgs
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
  )
 endif()

-# Cho phép các project khác include được header của robot_nav_core2
-set(robot_nav_core2_INCLUDE_DIRS
-    ${CMAKE_CURRENT_SOURCE_DIR}/include
-    PARENT_SCOPE
-)
-
-include_directories(
-        ${PROJECT_SOURCE_DIR}/include
-)
-
-if(BUILDING_WITH_CATKIN)
-  include_directories(${catkin_INCLUDE_DIRS})
-endif()
-
 # Tìm tất cả header files
-file(GLOB HEADERS "include/robot_nav_core2/*.h")
+file(GLOB_RECURSE HEADERS "include/robot_nav_core2/*.h")

 # Tạo INTERFACE library (header-only)
 add_library(${PROJECT_NAME} INTERFACE)

-# Link libraries
-if(BUILDING_WITH_CATKIN)
-  target_link_libraries(${PROJECT_NAME} INTERFACE 
-      ${catkin_LIBRARIES}
-  )
-else()
-  target_link_libraries(${PROJECT_NAME} INTERFACE 
-      robot_costmap_2d 
-      tf3 
-      nav_grid 
-      robot_nav_2d_msgs
-      robot_cpp
-  )
-endif()
-
 if(BUILDING_WITH_CATKIN)
  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-endif()

-# Set include directories
-target_include_directories(${PROJECT_NAME}
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
    INTERFACE 
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )

-# ========================================================
-# Installation
-# ========================================================
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${catkin_LIBRARIES}
+  )

-# Cài đặt target
-install(TARGETS ${PROJECT_NAME}
-        EXPORT ${PROJECT_NAME}-targets)
+else()
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${PACKAGES_DIR}
+  )

-if(NOT BUILDING_WITH_CATKIN)
-  # Cài đặt header files (standalone)
-  install(DIRECTORY include/${PROJECT_NAME} 
-          DESTINATION include
-          FILES_MATCHING PATTERN "*.h")
 endif()

-# Export targets
-install(EXPORT ${PROJECT_NAME}-targets
-        FILE ${PROJECT_NAME}-targets.cmake
-        NAMESPACE ${PROJECT_NAME}::
-        DESTINATION lib/cmake/${PROJECT_NAME})
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )

-# In thông tin cấu hình
-message(STATUS "=================================")
-message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
-message(STATUS "Version: ${PROJECT_VERSION}")
-message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
-message(STATUS "Headers found:")
-foreach(hdr ${HEADERS})
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME}
+    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} (Header-Only)")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Headers found:")
+  foreach(hdr ${HEADERS})
    message(STATUS "  - ${hdr}")
-endforeach()
-message(STATUS "=================================")
+  endforeach()
+  message(STATUS "Dependencies: robot_costmap_2d, tf3, nav_grid, robot_nav_2d_msgs, robot_cpp, robot_geometry_msgs")
+  message(STATUS "=================================")
+endif()
--- a/src/Navigations/Cores/robot_nav_core2/package.xml
+++ b/src/Navigations/Cores/robot_nav_core2/package.xml
@@ -34,4 +34,7 @@
  <build_depend>robot_cpp</build_depend>
  <run_depend>robot_cpp</run_depend>

+  <build_depend>robot_geometry_msgs</build_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+
 </package>
--- a/src/Navigations/Cores/robot_nav_core_adapter/CMakeLists.txt
+++ b/src/Navigations/Cores/robot_nav_core_adapter/CMakeLists.txt
@@ -1,38 +1,35 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_nav_core_adapter 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_nav_core_adapter with Catkin")
+
 else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building robot_nav_core_adapter with Standalone CMake")
 endif()

-# Tên dự án
-project(robot_nav_core_adapter VERSION 1.0.0 LANGUAGES CXX)
-
-# Chuẩn C++
+# C++ Standard - must be set before find_package
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)

-if(NOT CMAKE_BUILD_TYPE)
-    set(CMAKE_BUILD_TYPE Release)
-endif()
-
-# ========================================================
 # Find dependencies
-# ========================================================
-
-# Find system dependencies
 find_package(Boost REQUIRED COMPONENTS filesystem system)
+find_package(Threads REQUIRED)

-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS
+if (NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # 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  
    geometry_msgs
    robot_std_msgs
    robot_nav_core
@@ -40,173 +37,208 @@ if(BUILDING_WITH_CATKIN)
    robot_nav_2d_utils
    robot_cpp
  )
-endif()

-# Dependencies packages (internal libraries)
-set(PACKAGES_DIR geometry_msgs robot_std_msgs robot_nav_core robot_nav_core2 robot_nav_2d_utils pthread)
+else()

 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    geometry_msgs
+    robot_std_msgs
+    robot_nav_core
+    robot_nav_core2
+    robot_nav_2d_utils
+    robot_cpp
+  )

-if(BUILDING_WITH_CATKIN)
  catkin_package(
    INCLUDE_DIRS include
    LIBRARIES costmap_adapter local_planner_adapter global_planner_adapter
    CATKIN_DEPENDS geometry_msgs robot_std_msgs robot_nav_core robot_nav_core2 robot_nav_2d_utils robot_cpp
+    DEPENDS Boost
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
  )
 endif()

-# Thư mục include
-include_directories(
-        ${PROJECT_SOURCE_DIR}/include
+# ========================================================
+# Libraries
+# ========================================================
+add_library(costmap_adapter SHARED
+  src/costmap_adapter.cpp
 )

-if(BUILDING_WITH_CATKIN)
-  include_directories(${catkin_INCLUDE_DIRS})
-endif()
+add_library(local_planner_adapter SHARED
+  src/local_planner_adapter.cpp
+)

-# ========================================================
-# Build
-# ========================================================
-
-# Tạo thư viện shared (.so)
-add_library(costmap_adapter src/costmap_adapter.cpp)
-
-# Link libraries
-if(BUILDING_WITH_CATKIN)
-  target_link_libraries(costmap_adapter 
-    PRIVATE ${catkin_LIBRARIES}
-  )
-else()
-  target_link_libraries(costmap_adapter 
-    PRIVATE ${PACKAGES_DIR}
-  )
-endif()
-target_include_directories(costmap_adapter PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-add_library(local_planner_adapter src/local_planner_adapter.cpp)
-
-if(BUILDING_WITH_CATKIN)
-  target_link_libraries(local_planner_adapter 
-    PRIVATE
-      Boost::filesystem
-      Boost::system
-      dl               
-      costmap_adapter 
-      ${catkin_LIBRARIES}
-  )
-else()
-  target_link_libraries(local_planner_adapter 
-    PRIVATE
-      Boost::filesystem
-      Boost::system
-      dl               
-      costmap_adapter 
-      robot_cpp
-      ${PACKAGES_DIR}
-  )
-endif()
-target_include_directories(local_planner_adapter PRIVATE
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-add_library(global_planner_adapter src/global_planner_adapter.cpp)
-
-if(BUILDING_WITH_CATKIN)
-  target_link_libraries(global_planner_adapter 
-    PRIVATE
-      Boost::filesystem
-      Boost::system
-      dl               
-      costmap_adapter 
-      ${catkin_LIBRARIES}
-  )
-else()
-  target_link_libraries(global_planner_adapter 
-    PRIVATE
-      Boost::filesystem
-      Boost::system
-      dl               
-      costmap_adapter 
-      ${PACKAGES_DIR}
-  )
-endif()
+add_library(global_planner_adapter SHARED
+  src/global_planner_adapter.cpp
+)

 if(BUILDING_WITH_CATKIN)
  add_dependencies(costmap_adapter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  add_dependencies(local_planner_adapter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
  add_dependencies(global_planner_adapter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  # costmap_adapter
+  target_include_directories(costmap_adapter
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(costmap_adapter
+    PUBLIC ${catkin_LIBRARIES}
+  )
+
+  # local_planner_adapter
+  target_include_directories(local_planner_adapter
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(local_planner_adapter
+    PUBLIC costmap_adapter
+    PRIVATE ${catkin_LIBRARIES}
+    PRIVATE Boost::filesystem Boost::system
+    PRIVATE dl
+    PRIVATE robot_cpp
+  )
+
+  # global_planner_adapter
+  target_include_directories(global_planner_adapter
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(global_planner_adapter
+    PUBLIC costmap_adapter
+    PRIVATE ${catkin_LIBRARIES}
+    PRIVATE Boost::filesystem Boost::system
+    PRIVATE dl
+  )
+
+else()
+
+  # costmap_adapter
+  target_include_directories(costmap_adapter
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(costmap_adapter
+    PUBLIC
+      ${PACKAGES_DIR}
+  )
+
+  set_target_properties(costmap_adapter PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+  # local_planner_adapter
+  target_include_directories(local_planner_adapter
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(local_planner_adapter
+    PUBLIC costmap_adapter
+    PRIVATE
+      ${PACKAGES_DIR}
+      Boost::filesystem Boost::system
+      dl
+      robot_cpp
+  )
+
+  set_target_properties(local_planner_adapter PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+  # global_planner_adapter
+  target_include_directories(global_planner_adapter
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(global_planner_adapter
+    PUBLIC costmap_adapter
+    PRIVATE
+      ${PACKAGES_DIR}
+      Boost::filesystem Boost::system
+      dl
+  )
+
+  set_target_properties(global_planner_adapter PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
 endif()

-target_include_directories(global_planner_adapter PRIVATE
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-
-# Ensure runtime loader prefers in-tree adapter libs over system/ROS libs.
-# Without this, dlopen() may resolve libcostmap_adapter.so from /opt/ros/noetic/lib,
-# which has a different ABI (Costmap2DROBOT) and causes missing symbols.
-set(_ROBOT_NAV_CORE_ADAPTER_RPATH
-    "${CMAKE_BINARY_DIR}/src/Navigations/Cores/robot_nav_core_adapter"
-    "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
-    "${CMAKE_BINARY_DIR}/src/Libraries/tf3"
-    "${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
-    "${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"
-)
-
-set_target_properties(costmap_adapter PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
-set_target_properties(local_planner_adapter PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
-set_target_properties(global_planner_adapter PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
-
 # ========================================================
-# Installation
+# 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 costmap_adapter local_planner_adapter global_planner_adapter
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )

-# Cài đặt library
-install(TARGETS global_planner_adapter local_planner_adapter costmap_adapter 
-        EXPORT robot_nav_core_adapter-targets
-        LIBRARY DESTINATION lib
-        ARCHIVE DESTINATION lib
-        RUNTIME DESTINATION bin)
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )

-if(NOT BUILDING_WITH_CATKIN)
-  # Cài đặt header files (standalone)
-  install(DIRECTORY include/robot_nav_core_adapter 
-          DESTINATION include
-          FILES_MATCHING PATTERN "*.h")
+else()
+
+  install(TARGETS costmap_adapter local_planner_adapter global_planner_adapter
+    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: costmap_adapter, local_planner_adapter, global_planner_adapter")
+  message(STATUS "Dependencies: geometry_msgs, robot_std_msgs, robot_nav_core, robot_nav_core2, robot_nav_2d_utils, robot_cpp, Boost")
+  message(STATUS "=================================")
 endif()
-
-# Export targets
-install(EXPORT robot_nav_core_adapter-targets
-        FILE robot_nav_core_adapter-targets.cmake
-        DESTINATION lib/cmake/robot_nav_core_adapter)
-
-# Tùy chọn build
-option(BUILD_SHARED_LIBS "Build shared libraries" ON)
-option(BUILD_TESTS "Build test programs" OFF)
-
-# Flags biên dịch
-# Warning flags - disabled to suppress warnings during build
-# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
-set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
-
-# In thông tin cấu hình
-message(STATUS "=================================")
-message(STATUS "Project: ${PROJECT_NAME}")
-message(STATUS "Version: ${PROJECT_VERSION}")
-message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
-message(STATUS "Headers found:")
-foreach(hdr ${HEADERS})
-    message(STATUS "  - ${hdr}")
-endforeach()
-message(STATUS "=================================")
--- a/src/Navigations/Libraries/nav_grid/CMakeLists.txt
+++ b/src/Navigations/Libraries/nav_grid/CMakeLists.txt
@@ -1,41 +1,48 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(nav_grid 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 nav_grid with Catkin")
+
 else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building nav_grid with Standalone CMake")
 endif()

-project(nav_grid VERSION 1.0.0 LANGUAGES CXX)
-
-# Chuẩn C++
+# C++ Standard - must be set before find_package
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)

-# ========================================================
 # Find dependencies
-# ========================================================
+find_package(GTest QUIET)

-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED)
-endif()
+if (NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # 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}")
+
+else()

 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED)

-if(BUILDING_WITH_CATKIN)
  catkin_package(
    INCLUDE_DIRS include
-    LIBRARIES nav_grid
+    # LIBRARIES không cần vì đây là header-only library
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
  )
 endif()

@@ -43,67 +50,104 @@ endif()
 file(GLOB_RECURSE HEADERS "include/nav_grid/*.h")

 # Tạo INTERFACE library (header-only)
-add_library(nav_grid INTERFACE)
-
-# Set include directories
-target_include_directories(nav_grid
-    INTERFACE 
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
+add_library(${PROJECT_NAME} INTERFACE)

 if(BUILDING_WITH_CATKIN)
-  add_dependencies(nav_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${catkin_LIBRARIES}
+  )
+
+else()
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
 endif()

-# Cài đặt header files
-if(NOT BUILDING_WITH_CATKIN)
-  install(DIRECTORY include/nav_grid 
-          DESTINATION include
-          FILES_MATCHING PATTERN "*.h")
-endif()
+if(BUILDING_WITH_CATKIN)
+  ## Mark libraries for installation
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+  install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )

-# Cài đặt target
-install(TARGETS nav_grid
-        EXPORT nav_grid-targets
-        ARCHIVE DESTINATION lib
-        LIBRARY DESTINATION lib
-        RUNTIME DESTINATION bin)
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )

-# Export targets
-install(EXPORT nav_grid-targets
-        FILE nav_grid-targets.cmake
-        NAMESPACE nav_grid::
-        DESTINATION lib/cmake/nav_grid)
+else()

-# Tùy chọn build tests
-option(BUILD_TESTING "Build test programs" OFF)
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )

-if(BUILD_TESTING)
-    find_package(GTest REQUIRED)
-    
-    # Tạo test executable
-    add_executable(nav_grid_test test/utest.cpp)
-    target_link_libraries(nav_grid_test 
-        PRIVATE 
-            nav_grid
-            GTest::GTest
-            GTest::Main
-    )
-    
-    # Thêm test vào CTest
-    enable_testing()
-    add_test(NAME nav_grid_test COMMAND nav_grid_test)
-endif()
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )

-# In thông tin cấu hình
-message(STATUS "=================================")
-message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
-message(STATUS "Version: ${PROJECT_VERSION}")
-message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
-message(STATUS "Headers found:")
-foreach(hdr ${HEADERS})
+  ## 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} (Header-Only)")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Headers found:")
+  foreach(hdr ${HEADERS})
    message(STATUS "  - ${hdr}")
-endforeach()
-message(STATUS "Build tests: ${BUILD_TESTING}")
-message(STATUS "=================================")
+  endforeach()
+  message(STATUS "Dependencies: None")
+  message(STATUS "=================================")
+endif()
+
+# ========================================================
+# Test executables
+# ========================================================
+if(GTest_FOUND OR GTEST_FOUND)
+  enable_testing()
+  find_package(Threads REQUIRED)
+
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/utest.cpp)
+    add_executable(${PROJECT_NAME}_test test/utest.cpp)
+    target_link_libraries(${PROJECT_NAME}_test PRIVATE
+      ${PROJECT_NAME}
+      GTest::GTest
+      GTest::Main
+      Threads::Threads
+    )
+    add_test(NAME ${PROJECT_NAME}_test COMMAND ${PROJECT_NAME}_test)
+  endif()
+else()
+  message(STATUS "Google Test not found. Tests will not be built.")
+endif()
--- a/src/Navigations/Packages/move_base/CMakeLists.txt
+++ b/src/Navigations/Packages/move_base/CMakeLists.txt
@@ -1,36 +1,55 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(move_base 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 move_base with Catkin")
+
 else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building move_base with Standalone CMake")
 endif()

-# Tên dự án
-project(move_base VERSION 1.0.0 LANGUAGES CXX)
-
-# Chuẩn C++
+# C++ Standard - must be set before find_package
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)

-if(NOT CMAKE_BUILD_TYPE)
-    set(CMAKE_BUILD_TYPE Release)
-endif()
-
-# ========================================================
-# Find Packages
-# ========================================================
+# Find dependencies
 find_package(Boost REQUIRED COMPONENTS filesystem system)
+find_package(Threads REQUIRED)
+find_package(yaml-cpp REQUIRED)

-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS
+if (NOT BUILDING_WITH_CATKIN)
+
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+  # 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  
+    geometry_msgs
+    robot_std_msgs
+    move_base_core
+    robot_nav_core
+    robot_costmap_2d
+    plugins
+    robot_tf3_sensor_msgs
+    robot_tf3_geometry_msgs
+    data_convert
+    robot_nav_2d_utils
+    robot_cpp
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
    geometry_msgs
    robot_std_msgs
    move_base_core
@@ -42,175 +61,145 @@ if(BUILDING_WITH_CATKIN)
    robot_nav_2d_utils
    robot_cpp
  )
-endif()

-
-# ========================================================
-# Source Files
-# ========================================================
-file(GLOB SOURCES "src/move_base.cpp")
-file(GLOB HEADERS "include/move_base/move_base.h")
-
-# ========================================================
-# Catkin specific configuration
-# ========================================================
-
-if(BUILDING_WITH_CATKIN)
  catkin_package(
    INCLUDE_DIRS include
-    LIBRARIES move_base
+    LIBRARIES ${PROJECT_NAME}
    CATKIN_DEPENDS geometry_msgs robot_std_msgs move_base_core robot_nav_core robot_costmap_2d robot_tf3_sensor_msgs robot_tf3_geometry_msgs data_convert robot_nav_2d_utils robot_cpp
+    DEPENDS Boost yaml-cpp
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
  )
 endif()

 # ========================================================
-# Include Directories
+# Libraries
 # ========================================================
-include_directories(
-  ${PROJECT_SOURCE_DIR}/include
+add_library(${PROJECT_NAME} SHARED
+  src/move_base.cpp
 )

 if(BUILDING_WITH_CATKIN)
-  include_directories(${catkin_INCLUDE_DIRS})
-endif()
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )

-# ========================================================
-# RPATH settings: ưu tiên thư viện build tại chỗ
-# ========================================================
-set(CMAKE_SKIP_BUILD_RPATH FALSE)
-set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
-set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
-set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
-set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-
-# ========================================================
-# Create Library
-# ========================================================
-add_library(move_base SHARED ${SOURCES} ${HEADERS})
-
-# ========================================================
-# Dependencies and Link Libraries
-# ========================================================
-
-if(BUILDING_WITH_CATKIN)
-  target_link_libraries(move_base 
+  target_link_libraries(${PROJECT_NAME}
    PUBLIC ${catkin_LIBRARIES}
    PRIVATE Boost::filesystem Boost::system
-  )
-else()
-  # Standalone CMake mode: link all dependencies manually
-  set(PACKAGES_DIR 
-    geometry_msgs 
-    robot_std_msgs 
-    move_base_core 
-    robot_nav_core 
-    robot_costmap_2d 
-    plugins  # Link với plugins library để có StaticLayer typeinfo
-    yaml-cpp
-    robot_tf3_sensor_msgs
-    robot_tf3_geometry_msgs
-    data_convert
    dl
-    pthread
-    robot_nav_2d_utils
  )

-  target_link_libraries(move_base 
-    PUBLIC ${PACKAGES_DIR} 
-    PUBLIC robot_cpp
-    PRIVATE Boost::filesystem Boost::system
+else()
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC
+      ${PACKAGES_DIR}
+    PRIVATE
+      Boost::filesystem Boost::system
+      yaml-cpp
+      dl
+  )
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
 endif()

-# Set RPATH để ưu tiên thư viện build cục bộ
-set_target_properties(move_base PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
-
-if(BUILDING_WITH_CATKIN)
-  add_dependencies(move_base ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-endif()
-
-
-# ========================================================
-# Include Directories for Target
-# ========================================================
-target_include_directories(move_base
-  PUBLIC 
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
-)
-
-
 # ========================================================
 # Executable
 # ========================================================
-add_executable(move_base_main src/move_base_main.cpp)
-target_link_libraries(move_base_main 
-  PRIVATE move_base robot_cpp
-  PRIVATE Boost::filesystem Boost::system
-)
+add_executable(${PROJECT_NAME}_main src/move_base_main.cpp)

-# Set RPATH for executable to find libraries in build directory first
-# Use RPATH instead of RUNPATH for higher priority
+if(BUILDING_WITH_CATKIN)
+  target_link_libraries(${PROJECT_NAME}_main PRIVATE
+    ${PROJECT_NAME}
+    robot_cpp
+    Boost::filesystem Boost::system
+    dl
+  )
+else()
+  target_link_libraries(${PROJECT_NAME}_main PRIVATE
+    ${PROJECT_NAME}
+    robot_cpp
+    Boost::filesystem Boost::system
+    dl
+  )

-# Force use of RPATH instead of RUNPATH
-set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--disable-new-dtags")
+  # Configure RPATH to find libraries in devel space
+  set_target_properties(${PROJECT_NAME}_main PROPERTIES
+    BUILD_WITH_INSTALL_RPATH FALSE
+    INSTALL_RPATH_USE_LINK_PATH TRUE
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+  )
+endif()

 # ========================================================
-# Installation
+# 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 ${PROJECT_NAME} ${PROJECT_NAME}_main
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )

-if(NOT BUILDING_WITH_CATKIN)
-  # Standalone CMake installation
-  install(DIRECTORY include/move_base 
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+
+else()
+
+  install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_main
+    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: ${PROJECT_NAME}")
+  message(STATUS "Executables: ${PROJECT_NAME}_main")
+  message(STATUS "Dependencies: geometry_msgs, robot_std_msgs, move_base_core, robot_nav_core, robot_costmap_2d, robot_tf3_sensor_msgs, robot_tf3_geometry_msgs, data_convert, robot_nav_2d_utils, robot_cpp, Boost, yaml-cpp")
+  message(STATUS "=================================")
 endif()
-
-install(TARGETS move_base move_base_main
-  EXPORT move_base-targets
-  LIBRARY DESTINATION lib
-  ARCHIVE DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-
-if(NOT BUILDING_WITH_CATKIN)
-  install(EXPORT move_base-targets
-    FILE move_base-targets.cmake
-    DESTINATION lib/cmake/move_base
-  )
-endif()
-
-
-# ========================================================
-# Build Options
-# ========================================================
-option(BUILD_SHARED_LIBS "Build shared libraries" ON)
-option(BUILD_TESTS "Build test programs" OFF)
-
-# ========================================================
-# Compiler Flags
-# ========================================================
-# Warning flags - disabled to suppress warnings during build
-# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")  # -w suppresses all warnings
-set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
-set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
-
-# ========================================================
-# Status Messages
-# ========================================================
-message(STATUS "=================================")
-message(STATUS "Project: ${PROJECT_NAME}")
-message(STATUS "Version: ${PROJECT_VERSION}")
-message(STATUS "Build mode: ${BUILDING_WITH_CATKIN}")
-message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
-message(STATUS "Build type: ${CMAKE_BUILD_TYPE}")
-message(STATUS "Headers found:")
-foreach(hdr ${HEADERS})
-    message(STATUS "  - ${hdr}")
-endforeach()
-message(STATUS "=================================")