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/Libraries/common_msgs
+++ b/src/Libraries/common_msgs
--- a/src/Libraries/costmap_2d
+++ b/src/Libraries/costmap_2d
--- a/src/Libraries/geometry2
+++ b/src/Libraries/geometry2
--- a/src/Libraries/laser_geometry
+++ b/src/Libraries/laser_geometry
--- a/src/Libraries/robot_cpp/CMakeLists.txt
+++ b/src/Libraries/robot_cpp/CMakeLists.txt
@@ -1,40 +1,64 @@
 cmake_minimum_required(VERSION 3.0.2)
+project(robot_cpp 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_cpp with Catkin")
+
 else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building robot_cpp with Standalone CMake")
 endif()

-project(robot_cpp)
-
+# C++ Standard - must be set before find_package
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+set(CMAKE_CXX_EXTENSIONS OFF)

-# ========================================================
-# Find Packages
-# ========================================================
+# Find dependencies
+find_package(yaml-cpp REQUIRED)
+find_package(console_bridge REQUIRED)

-if(BUILDING_WITH_CATKIN)
-  ## Find catkin macros and libraries
-  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  
    robot_xmlrpcpp
+    robot_time
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_xmlrpcpp
+    robot_time
+  )
+
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS robot_xmlrpcpp robot_time
+    # Note: yaml-cpp and console_bridge are system dependencies, 
+    # linked via target_link_libraries, not via catkin_package DEPENDS
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
  )
 endif()

-## System dependencies
-# Find yaml-cpp library
-find_package(yaml-cpp REQUIRED)
-
-# Use target if available (modern CMake)
+# Use yaml-cpp target if available (modern CMake)
 if(TARGET yaml-cpp::yaml-cpp)
  set(YAML_CPP_TARGET yaml-cpp::yaml-cpp)
  message(STATUS "Using yaml-cpp target: yaml-cpp::yaml-cpp")
@@ -51,80 +75,83 @@ else()
  message(STATUS "Using yaml-cpp library name: yaml-cpp")
 endif()

-find_package(console_bridge REQUIRED)
-
-# ========================================================
-# Catkin specific configuration
-# ========================================================
-
-if(BUILDING_WITH_CATKIN)
-  ## The catkin_package macro generates cmake config files for your package
-  ## Note: yaml-cpp and console_bridge are system dependencies, not declared in DEPENDS
-  ## as they don't provide the required INCLUDE_DIRS/LIBRARIES variables for catkin
-  catkin_package(
-    INCLUDE_DIRS include
-    LIBRARIES robot_cpp
-    CATKIN_DEPENDS robot_xmlrpcpp
-  )
-endif()
-
-###########
-## Build ##
-###########
-
-## Compiler flags
+# Compiler flags
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  # Warning flags - disabled to suppress warnings during build
  # add_compile_options(-Wall -Wextra -Wpedantic -fPIC)
  add_compile_options(-w -fPIC)  # -w suppresses all warnings
 endif()

-## Include directories
-include_directories(
-  include
-)
-
-# yaml-cpp and console_bridge are linked via targets, so we don't need to add their include dirs
-# if they provide targets (modern CMake), or we handle them via target_link_libraries
-
-if(BUILDING_WITH_CATKIN)
-  include_directories(${catkin_INCLUDE_DIRS})
-endif()
-
-# Create unified robot_cpp library combining console and node_handle
+# Libraries
 add_library(${PROJECT_NAME} SHARED
  src/console.cpp
  src/node_handle.cpp
  src/plugin_loader_helper.cpp
 )

-target_include_directories(${PROJECT_NAME}
-  PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
-)
+# Enable C++17 filesystem feature
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17)

-## Link libraries
-# Use the determined yaml-cpp target/library
-target_link_libraries(${PROJECT_NAME}
-  PUBLIC
-    ${YAML_CPP_TARGET}
-    robot_xmlrpcpp
-    robot_time
-  PRIVATE
-    dl  # Required for dladdr() function used in plugin_loader_helper.cpp
-)
-
-
-
-## Add cmake target dependencies
 if(BUILDING_WITH_CATKIN)
  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-endif()

-set_target_properties(${PROJECT_NAME} PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link filesystem library if needed (for GCC < 9 or Clang)
+  set(FILESYSTEM_LIB "")
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0")
+    set(FILESYSTEM_LIB "stdc++fs")
+  elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    find_library(FILESYSTEM_LIB c++fs)
+  endif()
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${catkin_LIBRARIES}
+    PRIVATE ${YAML_CPP_TARGET}
+    PRIVATE dl  # Required for dladdr() function used in plugin_loader_helper.cpp
+  )
+  
+  if(FILESYSTEM_LIB)
+    target_link_libraries(${PROJECT_NAME} PRIVATE ${FILESYSTEM_LIB})
+  endif()
+
+else()
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  # Link filesystem library if needed (for GCC < 9 or Clang)
+  set(FILESYSTEM_LIB "")
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS "9.0")
+    set(FILESYSTEM_LIB "stdc++fs")
+  elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    find_library(FILESYSTEM_LIB c++fs)
+  endif()
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC
+      ${PACKAGES_DIR}
+      ${YAML_CPP_TARGET}
+    PRIVATE
+      dl  # Required for dladdr() function used in plugin_loader_helper.cpp
+  )
+  
+  if(FILESYSTEM_LIB)
+    target_link_libraries(${PROJECT_NAME} PRIVATE ${FILESYSTEM_LIB})
+  endif()
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  )
+
+endif()

 # Define CMAKE_BINARY_DIR as compile definition so it's available at runtime
 target_compile_definitions(${PROJECT_NAME} 
@@ -132,25 +159,44 @@ target_compile_definitions(${PROJECT_NAME}
    PNKX_NAV_CORE_BUILD_DIR="${CMAKE_BINARY_DIR}"
 )

-# ========================================================
-# Installation (Standalone CMake only)
-# ========================================================
+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}
+  )

-# Export target trong mọi trường hợp để các target khác có thể export và phụ thuộc vào nó
-install(TARGETS ${PROJECT_NAME}
-  EXPORT ${PROJECT_NAME}-targets
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin)
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )
+  
+else()

-if(NOT BUILDING_WITH_CATKIN)
+  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/
    DESTINATION include
-    FILES_MATCHING PATTERN "*.h")
-
-  install(EXPORT ${PROJECT_NAME}-targets
-    # NAMESPACE robot::
-    DESTINATION lib/cmake/${PROJECT_NAME})
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN ".svn" EXCLUDE
+  )

  # ========================================================
  # Install CMake Config File for find_package()
@@ -167,29 +213,16 @@ if(NOT BUILDING_WITH_CATKIN)
    ${CMAKE_CURRENT_BINARY_DIR}/robot_cppConfig.cmake
    DESTINATION lib/cmake/${PROJECT_NAME}
  )
-else()
-  # Khi build với Catkin, vẫn cần export để các target khác có thể export
-  install(EXPORT ${PROJECT_NAME}-targets
-    # NAMESPACE robot::
-    DESTINATION lib/cmake/${PROJECT_NAME})
+
+  # Print configuration info
+  message(STATUS "=================================")
+  message(STATUS "Project: ${PROJECT_NAME}")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Dependencies: robot_xmlrpcpp, robot_time, yaml-cpp, console_bridge")
+  message(STATUS "=================================")
 endif()

-# ========================================================
-# Install CMake Config File for find_package()
-# ========================================================
-# Generate config file from template
-configure_file(
-  ${CMAKE_CURRENT_SOURCE_DIR}/robot_cppConfig.cmake.in
-  ${CMAKE_CURRENT_BINARY_DIR}/robot_cppConfig.cmake
-  @ONLY
-)
-
-# Install config file
-install(FILES
-  ${CMAKE_CURRENT_BINARY_DIR}/robot_cppConfig.cmake
-  DESTINATION lib/cmake/${PROJECT_NAME}
-)
-
 # ========================================================
 # Test executable
 # ========================================================
@@ -207,8 +240,8 @@ if(BUILD_TESTS)
  
  target_link_libraries(test_node_handle
    PRIVATE
-      robot_cpp
-      yaml-cpp
+      ${PROJECT_NAME}
+      ${YAML_CPP_TARGET}
  )
  
  set_target_properties(test_node_handle PROPERTIES
--- a/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h
+++ b/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h
@@ -38,10 +38,15 @@ class PluginLoaderHelper
 public:
  /**
   * @brief Constructor
-   * @param nh NodeHandle to read parameters from (default: root NodeHandle)
+   */
+  PluginLoaderHelper();
+
+  /**
+   * @brief Constructor
+   * @param nh NodeHandle to read parameters from
   * @param config_namespace Namespace in NodeHandle where plugins are stored (default: root)
   */
-  PluginLoaderHelper(robot::NodeHandle nh = robot::NodeHandle(), const std::string& config_namespace = "");
+  PluginLoaderHelper(robot::NodeHandle nh, const std::string& config_namespace = "");

  /**
   * @brief Find library path from symbol name (export name)
@@ -76,6 +81,12 @@ public:
   */
  static std::string getBuildDirectory();

+  /**
+   * @brief Get workspace path at runtime
+   * @return Workspace path (e.g., /home/robotics/AGV/Diff_Wheel_Prj/t800_v2_ws), or empty if not found
+   */
+  static std::string getWorkspacePath();
+
 private:
  /**
   * @brief Resolve library path (handle relative paths, search in search_paths)
--- a/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp
+++ b/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp
@@ -27,17 +27,25 @@
 namespace robot
 {

+PluginLoaderHelper::PluginLoaderHelper()
+  : PluginLoaderHelper(robot::NodeHandle(), "")
+{
+  // Delegate to the other constructor with default NodeHandle
+}
+
 PluginLoaderHelper::PluginLoaderHelper(robot::NodeHandle nh, const std::string& config_namespace)
  : nh_(nh), config_namespace_(config_namespace)
 {
  std::string build_dir = getBuildDirectory();
  if (!build_dir.empty()) {
    search_paths_.push_back(build_dir);
-    // Thêm các subdirectories thường dùng
-    search_paths_.push_back(build_dir + "/src/Algorithms/Packages/global_planners");
-    search_paths_.push_back(build_dir + "/src/Algorithms/Packages/local_planners");
-    search_paths_.push_back(build_dir + "/src/Navigations/Cores/robot_nav_core2_adapter");
-    search_paths_.push_back(build_dir + "/src/Libraries/robot_costmap_2d");
+  }
+  
+  // Thêm các subdirectories thường dùng từ workspace path
+  std::string workspace_path = getWorkspacePath();
+  if (!workspace_path.empty()) {
+    search_paths_.push_back(workspace_path + "/install/lib");
+    search_paths_.push_back(workspace_path + "/devel/lib");
  }
 }

@@ -56,7 +64,6 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
  } else {
    param_path = symbol_name + "/library_path";
  }
-  
  // Try to read from NodeHandle
  std::string library_path;
  if (nh_.hasParam(param_path)) {
@@ -97,12 +104,13 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
    std::string ld_path_str(ld_path);
    std::stringstream ss(ld_path_str);
    std::string path;
+
    while (std::getline(ss, path, ':')) {
      if (!path.empty()) {
        // Try different naming conventions
        std::vector<std::string> possible_names = {
-          "lib" + symbol_name + ".so",
-          symbol_name + ".so"
+          "lib" + library_path + ".so",
+          library_path + ".so"
        };
        
        // Also try without namespace
@@ -111,7 +119,7 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name)
          possible_names.push_back("lib" + short_name + ".so");
          possible_names.push_back(short_name + ".so");
        }
-        
+      
        for (const auto& name : possible_names) {
          std::filesystem::path full_path = std::filesystem::path(path) / name;
          if (std::filesystem::exists(full_path)) {
@@ -327,5 +335,103 @@ std::string PluginLoaderHelper::getBuildDirectory()
  return "";
 }

+std::string PluginLoaderHelper::getWorkspacePath()
+{
+  // Method 1: Từ environment variable PNKX_NAV_CORE_DIR
+  const char* workspace_path = std::getenv("PNKX_NAV_CORE_DIR");
+  if (workspace_path && std::filesystem::exists(workspace_path)) {
+    return std::string(workspace_path);
+  }
+
+  // Method 2: Từ environment variable ROS_WORKSPACE hoặc CATKIN_WS
+  const char* ros_workspace = std::getenv("ROS_WORKSPACE");
+  if (ros_workspace && std::filesystem::exists(ros_workspace)) {
+    return std::string(ros_workspace);
+  }
+
+  const char* catkin_ws = std::getenv("CATKIN_WS");
+  if (catkin_ws && std::filesystem::exists(catkin_ws)) {
+    return std::string(catkin_ws);
+  }
+
+  // Method 3: Từ build directory (lấy parent directory)
+  std::string build_dir = getBuildDirectory();
+  if (!build_dir.empty()) {
+    std::filesystem::path build_path(build_dir);
+    std::filesystem::path parent = build_path.parent_path();
+    // Kiểm tra xem có phải workspace không (có thư mục src/)
+    if (std::filesystem::exists(parent / "src") && std::filesystem::is_directory(parent / "src")) {
+      return parent.string();
+    }
+  }
+
+  // Method 4: Từ executable path và tìm workspace
+  try {
+    char exe_path[PATH_MAX];
+    ssize_t count = readlink("/proc/self/exe", exe_path, PATH_MAX);
+    if (count != -1) {
+      exe_path[count] = '\0';
+      std::filesystem::path exe_dir = std::filesystem::path(exe_path).parent_path();
+      
+      // Thử các relative paths: ../.., ../../.., etc. để tìm workspace
+      std::vector<std::filesystem::path> relative_paths = {
+        exe_dir.parent_path().parent_path(),  // ../..
+        exe_dir.parent_path().parent_path().parent_path(),  // ../../..
+        exe_dir.parent_path().parent_path().parent_path().parent_path()  // ../../../..
+      };
+      
+      for (const auto& rel_path : relative_paths) {
+        // Kiểm tra xem có phải workspace không (có thư mục src/ hoặc build/)
+        if (std::filesystem::exists(rel_path) && 
+            std::filesystem::is_directory(rel_path) &&
+            (std::filesystem::exists(rel_path / "src") || 
+             std::filesystem::exists(rel_path / "build"))) {
+          try {
+            return std::filesystem::canonical(rel_path).string();
+          } catch (...) {
+            return rel_path.string();
+          }
+        }
+      }
+    }
+  } catch (const std::exception&) {
+    // Ignore errors
+  }
+
+  // Method 5: Từ library path (librobot_cpp.so) và tìm workspace
+  Dl_info dl_info;
+  if (dladdr(reinterpret_cast<void*>(&PluginLoaderHelper::getWorkspacePath), &dl_info) != 0) {
+    if (dl_info.dli_fname) {
+      std::filesystem::path lib_path(dl_info.dli_fname);
+      std::filesystem::path lib_dir = lib_path.parent_path();
+      
+      // Tìm workspace từ library path
+      std::filesystem::path current = lib_dir;
+      for (int i = 0; i < 5; ++i) {  // Tối đa 5 levels up
+        if (std::filesystem::exists(current / "src") || 
+            std::filesystem::exists(current / "build")) {
+          try {
+            return std::filesystem::canonical(current).string();
+          } catch (...) {
+            return current.string();
+          }
+        }
+        current = current.parent_path();
+        if (current == current.parent_path()) {  // Reached root
+          break;
+        }
+      }
+    }
+  }
+
+  // Method 6: Hardcoded fallback (nếu các phương pháp trên không hoạt động)
+  std::string fallback = "/home/robotics/AGV/Diff_Wheel_Prj/t800_v2_ws";
+  if (std::filesystem::exists(fallback)) {
+    return fallback;
+  }
+
+  return "";
+}
+
 } // namespace robot

--- a/src/Libraries/robot_nav_2d_msgs/CMakeLists.txt
+++ b/src/Libraries/robot_nav_2d_msgs/CMakeLists.txt
@@ -1,106 +1,149 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_nav_2d_msgs 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_2d_msgs with Catkin")
-  find_package(catkin REQUIRED)
+
 else()
  set(BUILDING_WITH_CATKIN FALSE)
  message(STATUS "Building robot_nav_2d_msgs with Standalone CMake")
 endif()

-project(robot_nav_2d_msgs VERSION 1.0.0 LANGUAGES CXX)
+if (NOT BUILDING_WITH_CATKIN)

-# Chuẩn C++
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+
+  # 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_std_msgs
+    robot_geometry_msgs
+  )
+
+else()

 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_std_msgs
+    robot_geometry_msgs
+  )

-if(BUILDING_WITH_CATKIN)
-  ## The catkin_package macro generates cmake config files for your package
-  ## Note: Dependencies (robot_std_msgs, geometry_msgs) are not Catkin packages,
-  ## they are built as CMake targets in the same workspace and will be available
-  ## when this package is built. They are linked via target_link_libraries instead of CATKIN_DEPENDS
  catkin_package(
    INCLUDE_DIRS include
    # LIBRARIES không cần vì đây là header-only library
    # 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_RECURSE HEADERS "include/robot_nav_2d_msgs/*.h")

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

-# Set include directories
-target_include_directories(robot_nav_2d_msgs
-    INTERFACE 
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-
-# Link dependencies (header-only, chỉ cần include paths)
-# Các dependencies này cũng là header-only từ common_msgs
-target_link_libraries(robot_nav_2d_msgs
-    INTERFACE
-        robot_std_msgs
-        geometry_msgs
-)
-
-# Add include directories from dependencies explicitly for Catkin build
 if(BUILDING_WITH_CATKIN)
+  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}
+  )
+
+  # Add include directories from dependencies explicitly for Catkin build
  # Use relative paths from current source directory
-  # From robot_nav_2d_msgs (pnkx_robot_nav_core2/src/Libraries/robot_nav_2d_msgs) 
-  # to robot_std_msgs (pnkx_robot_nav_core2/src/Libraries/common_msgs/robot_std_msgs) is ../common_msgs/robot_std_msgs
-  get_filename_component(ROBOT_STD_MSGS_INCLUDE "${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/robot_std_msgs/include" ABSOLUTE)
-  get_filename_component(GEOMETRY_MSGS_INCLUDE "${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/robot_geometry_msgs/include" ABSOLUTE)
-  target_include_directories(robot_nav_2d_msgs INTERFACE
+  target_include_directories(${PROJECT_NAME} INTERFACE
    $<BUILD_INTERFACE:${ROBOT_STD_MSGS_INCLUDE}>
    $<BUILD_INTERFACE:${GEOMETRY_MSGS_INCLUDE}>
  )
+
+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/robot_nav_2d_msgs 
-          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 robot_nav_2d_msgs
-        EXPORT robot_nav_2d_msgs-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
+  )
+else()

-# Export targets
-# Bây giờ có thể export dependencies vì robot_std_msgs và geometry_msgs đã được export
-install(EXPORT robot_nav_2d_msgs-targets
-        FILE robot_nav_2d_msgs-targets.cmake
-        NAMESPACE robot_nav_2d_msgs::
-        DESTINATION lib/cmake/robot_nav_2d_msgs)
+  install(TARGETS ${PROJECT_NAME}
+    EXPORT ${PROJECT_NAME}-targets
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )

-# 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})
+  # 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 "Dependencies: robot_std_msgs, geometry_msgs")
-message(STATUS "=================================")
-
+  endforeach()
+  message(STATUS "Dependencies: robot_std_msgs, geometry_msgs")
+  message(STATUS "=================================")
+endif()
--- a/src/Libraries/robot_nav_2d_msgs/package.xml
+++ b/src/Libraries/robot_nav_2d_msgs/package.xml
@@ -19,4 +19,8 @@
    
  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>

+  <build_depend>robot_std_msgs</build_depend>
+  <build_depend>robot_geometry_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
 </package>
--- a/src/Libraries/robot_nav_2d_utils/CMakeLists.txt
+++ b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt
@@ -1,22 +1,6 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
 project(robot_nav_2d_utils VERSION 1.0.0 LANGUAGES CXX)

-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-
-# 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}")
-
-# ========================================================
-# Catkin specific configuration
-# ========================================================
-
 if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
  set(BUILDING_WITH_CATKIN TRUE)
  message(STATUS "Building robot_nav_2d_utils with Catkin")
@@ -26,7 +10,41 @@ else()
  message(STATUS "Building robot_nav_2d_utils with Standalone CMake")
 endif()

-if(BUILDING_WITH_CATKIN)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# Find dependencies
+find_package(console_bridge REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system thread)
+
+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  
+    robot_nav_2d_msgs
+    robot_geometry_msgs
+    robot_nav_msgs
+    nav_grid
+    robot_nav_core2
+    tf3
+    robot_tf3_geometry_msgs
+    robot_cpp 
+    console_bridge
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
  find_package(catkin REQUIRED COMPONENTS         
    robot_nav_2d_msgs
    robot_geometry_msgs
@@ -38,183 +56,178 @@ if(BUILDING_WITH_CATKIN)
    robot_cpp 
  )

+# Find dependencies
+  find_package(console_bridge REQUIRED)
+  find_package(Boost REQUIRED COMPONENTS system thread)
+
  catkin_package(
    INCLUDE_DIRS include
    LIBRARIES ${PROJECT_NAME}_conversions ${PROJECT_NAME}_path_ops ${PROJECT_NAME}_polygons ${PROJECT_NAME}_bounds ${PROJECT_NAME}_tf_help ${PROJECT_NAME}
-    # CATKIN_DEPENDS không cần vì dependencies không phải Catkin packages
+    CATKIN_DEPENDS robot_cpp robot_geometry_msgs robot_nav_2d_msgs robot_nav_msgs nav_grid robot_nav_core2
    DEPENDS console_bridge Boost
  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+  )
 endif()

-# Find dependencies
-find_package(console_bridge REQUIRED)
-find_package(Boost REQUIRED COMPONENTS system thread)
-
 # Libraries
 add_library(${PROJECT_NAME}_conversions src/conversions.cpp)
-target_include_directories(${PROJECT_NAME}_conversions
-    PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-target_link_libraries(${PROJECT_NAME}_conversions
-    PUBLIC
-        robot_nav_2d_msgs
-        robot_geometry_msgs
-        robot_nav_msgs
-        nav_grid
-        robot_nav_core2
-        robot_cpp
-    PRIVATE
-        console_bridge::console_bridge
-        Boost::system
-        Boost::thread
-)
-
-set_target_properties(${PROJECT_NAME}_conversions PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  BUILD_RPATH "${CMAKE_BINARY_DIR}"
-  INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
-)
-
 add_library(${PROJECT_NAME}_path_ops src/path_ops.cpp)
-target_include_directories(${PROJECT_NAME}_path_ops
-    PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-target_link_libraries(${PROJECT_NAME}_path_ops
-    PUBLIC
-        robot_nav_2d_msgs
-        robot_geometry_msgs
-        robot_cpp
-)
-
-set_target_properties(${PROJECT_NAME}_path_ops PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  BUILD_RPATH "${CMAKE_BINARY_DIR}"
-  INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
-)
-
 add_library(${PROJECT_NAME}_polygons src/polygons.cpp src/footprint.cpp)
-target_include_directories(${PROJECT_NAME}_polygons
-    PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-
-if(robot_xmlrpcpp_FOUND)
-    target_include_directories(${PROJECT_NAME}_polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
-    target_link_libraries(${PROJECT_NAME}_polygons
-        PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
-        PRIVATE ${robot_xmlrpcpp_LIBRARIES})
-elseif(XmlRpcCpp_FOUND)
-    target_include_directories(${PROJECT_NAME}_polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS})
-    target_link_libraries(${PROJECT_NAME}_polygons 
-        PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp 
-        PRIVATE ${XmlRpcCpp_LIBRARIES})
-else()
-    target_include_directories(${PROJECT_NAME}_polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
-    target_link_libraries(${PROJECT_NAME}_polygons
-        PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
-        PRIVATE ${robot_xmlrpcpp_LIBRARIES})
-endif()
-
-set_target_properties(${PROJECT_NAME}_polygons PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  BUILD_RPATH "${CMAKE_BINARY_DIR}"
-  INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
-)
-
 add_library(${PROJECT_NAME}_bounds src/bounds.cpp)
-target_include_directories(${PROJECT_NAME}_bounds
-    PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-target_link_libraries(${PROJECT_NAME}_bounds
-    PUBLIC
-        nav_grid
-        robot_nav_core2
-        robot_cpp
-)
-
-set_target_properties(${PROJECT_NAME}_bounds PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  BUILD_RPATH "${CMAKE_BINARY_DIR}"
-  INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
-)
-
 add_library(${PROJECT_NAME}_tf_help src/tf_help.cpp)
-target_include_directories(${PROJECT_NAME}_tf_help
+add_library(${PROJECT_NAME} INTERFACE)
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME}_conversions ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(${PROJECT_NAME}_path_ops ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(${PROJECT_NAME}_polygons ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(${PROJECT_NAME}_bounds ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(${PROJECT_NAME}_tf_help ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  target_link_libraries(${PROJECT_NAME}_conversions ${PACKAGES_DIR} ${Boost_LIBRARIES})
+  target_link_libraries(${PROJECT_NAME}_path_ops ${PACKAGES_DIR} ${Boost_LIBRARIES})
+  target_link_libraries(${PROJECT_NAME}_polygons ${PACKAGES_DIR} ${Boost_LIBRARIES})
+  target_link_libraries(${PROJECT_NAME}_bounds ${PACKAGES_DIR} ${Boost_LIBRARIES})
+  target_link_libraries(${PROJECT_NAME}_tf_help ${PACKAGES_DIR} ${Boost_LIBRARIES})
+  target_link_libraries(${PROJECT_NAME} INTERFACE ${PACKAGES_DIR} ${Boost_LIBRARIES})
+
+else()
+
+  target_link_libraries(${PROJECT_NAME}_conversions
+      PUBLIC
+        ${PACKAGES_DIR}
+        ${Boost_LIBRARIES}
+  )
+  target_link_libraries(${PROJECT_NAME}_path_ops
+      PUBLIC
+        ${PACKAGES_DIR}
+        ${Boost_LIBRARIES}
+  )
+  target_link_libraries(${PROJECT_NAME}_polygons
+      PUBLIC
+        ${PACKAGES_DIR}
+        ${Boost_LIBRARIES}
+  )
+  target_link_libraries(${PROJECT_NAME}_bounds
+      PUBLIC
+        ${PACKAGES_DIR}
+        ${Boost_LIBRARIES}
+  )
+  target_link_libraries(${PROJECT_NAME}_tf_help
+      PUBLIC
+        ${PACKAGES_DIR}
+        ${Boost_LIBRARIES}
+  )
+  target_link_libraries(${PROJECT_NAME}
+      INTERFACE
+        ${PACKAGES_DIR}
+        ${Boost_LIBRARIES}
+  )
+
+  set_target_properties(${PROJECT_NAME}_conversions PROPERTIES
+  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  )
+  set_target_properties(${PROJECT_NAME}_path_ops PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  )
+  set_target_properties(${PROJECT_NAME}_polygons PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  )
+  set_target_properties(${PROJECT_NAME}_bounds PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  )
+  set_target_properties(${PROJECT_NAME}_tf_help PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+  )
+  # Note: ${PROJECT_NAME} is an INTERFACE library, so it cannot have LIBRARY_OUTPUT_DIRECTORY
+
+  target_include_directories(${PROJECT_NAME}_conversions
    PUBLIC
        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
        $<INSTALL_INTERFACE:include>
-)
-target_link_libraries(${PROJECT_NAME}_tf_help
-  PUBLIC
-    robot_nav_2d_msgs
-    robot_geometry_msgs
-    robot_nav_msgs
-    robot_nav_core2
-    tf3
-    robot_tf3_geometry_msgs
-    robot_cpp
-)
+  )
+  target_include_directories(${PROJECT_NAME}_path_ops
+      PUBLIC
+          $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+          $<INSTALL_INTERFACE:include>
+  )
+  target_include_directories(${PROJECT_NAME}_polygons
+      PUBLIC
+          $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+          $<INSTALL_INTERFACE:include>
+  )
+  target_include_directories(${PROJECT_NAME}_bounds
+      PUBLIC
+          $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+          $<INSTALL_INTERFACE:include>
+  )
+  target_include_directories(${PROJECT_NAME}_tf_help
+      PUBLIC
+          $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+          $<INSTALL_INTERFACE:include>
+  )
+  target_include_directories(${PROJECT_NAME}
+      INTERFACE
+          $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+          $<INSTALL_INTERFACE:include>
+  )

-set_target_properties(${PROJECT_NAME}_tf_help PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  BUILD_RPATH "${CMAKE_BINARY_DIR}"
-  INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
-)
-
-# Create an INTERFACE library that represents all robot_nav_2d_utils libraries
-add_library(${PROJECT_NAME} INTERFACE)
-target_include_directories(${PROJECT_NAME}
-    INTERFACE
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-target_link_libraries(${PROJECT_NAME}
-    INTERFACE
-        ${PROJECT_NAME}_conversions
-        ${PROJECT_NAME}_path_ops
-        ${PROJECT_NAME}_polygons
-        ${PROJECT_NAME}_bounds
-        ${PROJECT_NAME}_tf_help
-        robot_cpp
-)
-
-# Install header files
-if(NOT BUILDING_WITH_CATKIN)
-  install(DIRECTORY include/${PROJECT_NAME} 
-          DESTINATION include
-          FILES_MATCHING PATTERN "*.h")
-
-  # Install mapbox headers
-  install(DIRECTORY include/mapbox
-          DESTINATION include
-          FILES_MATCHING PATTERN "*.hpp")
 endif()

-# Install targets
-install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_conversions ${PROJECT_NAME}_path_ops ${PROJECT_NAME}_polygons ${PROJECT_NAME}_bounds ${PROJECT_NAME}_tf_help
-        EXPORT ${PROJECT_NAME}-targets
-        ARCHIVE DESTINATION lib
-        LIBRARY DESTINATION lib
-        RUNTIME DESTINATION bin)
+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}_conversions ${PROJECT_NAME}_path_ops ${PROJECT_NAME}_polygons ${PROJECT_NAME}_bounds ${PROJECT_NAME}_tf_help
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/ include/mapbox
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+    PATTERN ".svn" EXCLUDE
+  )
+else()
+
+  install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_conversions ${PROJECT_NAME}_path_ops ${PROJECT_NAME}_polygons ${PROJECT_NAME}_bounds ${PROJECT_NAME}_tf_help
+      EXPORT ${PROJECT_NAME}-targets
+      LIBRARY DESTINATION lib
+      ARCHIVE DESTINATION lib
+      RUNTIME DESTINATION bin
+  )
+
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+      FILE ${PROJECT_NAME}-targets.cmake
+      DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/ include/mapbox
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+    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}_conversions, ${PROJECT_NAME}_path_ops, ${PROJECT_NAME}_polygons, ${PROJECT_NAME}_bounds, ${PROJECT_NAME}_tf_help")
+  message(STATUS "Dependencies: robot_nav_2d_msgs, robot_geometry_msgs, robot_nav_msgs, nav_grid, robot_nav_core2, tf3, console_bridge, Boost")
+  message(STATUS "=================================")
+endif()
+
+
+

-# Export targets
-install(EXPORT ${PROJECT_NAME}-targets
-        FILE ${PROJECT_NAME}-targets.cmake
-        NAMESPACE ${PROJECT_NAME}::
-        DESTINATION lib/cmake/${PROJECT_NAME})

-# 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}_conversions, ${PROJECT_NAME}_path_ops, ${PROJECT_NAME}_polygons, ${PROJECT_NAME}_bounds, ${PROJECT_NAME}_tf_help")
-message(STATUS "Dependencies: robot_nav_2d_msgs, robot_geometry_msgs, robot_nav_msgs, nav_grid, robot_nav_core2, tf3, console_bridge, Boost")
-message(STATUS "=================================")
--- a/src/Libraries/robot_time
+++ b/src/Libraries/robot_time
--- a/src/Libraries/tf3/CMakeLists.txt
+++ b/src/Libraries/tf3/CMakeLists.txt
@@ -1,113 +1,208 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(tf3)
+project(tf3 VERSION 1.0.0 LANGUAGES CXX)

-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}")
-
-# ========================================================
-# Find Packages
-# ========================================================
-# ========================================================
-# 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 tf3 with Catkin")

-  find_package(catkin REQUIRED COMPONENTS console_bridge)
 else()
  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building tf3 with Standalone CMake")
 endif()

+# 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(console_bridge REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system thread)

+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)
-  ## The catkin_package macro generates cmake config files for your package
  catkin_package(
    INCLUDE_DIRS include
-    LIBRARIES tf3
-    DEPENDS console_bridge
+    LIBRARIES ${PROJECT_NAME}
+    DEPENDS console_bridge Boost
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${console_bridge_INCLUDE_DIRS}
  )
 endif()
-include_directories(include ${console_bridge_INCLUDE_DIRS})

-# export user definitions
-
-#CPP Libraries
-add_library(tf3 src/cache.cpp src/buffer_core.cpp src/static_cache.cpp)
-target_link_libraries(tf3 ${Boost_LIBRARIES} ${console_bridge_LIBRARIES})
-set_target_properties(tf3 PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-  BUILD_RPATH "${CMAKE_BINARY_DIR}"
-  INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+# Libraries
+add_library(${PROJECT_NAME} SHARED
+  src/cache.cpp
+  src/buffer_core.cpp
+  src/static_cache.cpp
 )

-## Add cmake target dependencies
 if(BUILDING_WITH_CATKIN)
-  add_dependencies(tf3 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+  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>
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${catkin_LIBRARIES}
+    PRIVATE Boost::boost Boost::system Boost::thread
+    PRIVATE console_bridge::console_bridge
+  )
+
+else()
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC
+      Boost::boost Boost::system Boost::thread
+      console_bridge::console_bridge
+  )
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
 endif()
-target_include_directories(tf3 PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)

-add_executable(simple_tf3_example examples/simple_tf3_example.cpp)
-target_include_directories(simple_tf3_example PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
-target_link_libraries(simple_tf3_example PRIVATE tf3 pthread ${console_bridge_LIBRARIES})
-set_target_properties(simple_tf3_example PROPERTIES
-  LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
-)
-# Export target trong mọi trường hợp để các target khác có thể export và phụ thuộc vào nó
-install(TARGETS tf3
-  EXPORT tf3-targets
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
+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}
+  )

-if(NOT BUILDING_WITH_CATKIN)
+  ## Mark cpp header files for installation
  install(DIRECTORY include/${PROJECT_NAME}/
-    DESTINATION 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}")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Dependencies: console_bridge, Boost")
+  message(STATUS "=================================")
+endif()
+
+# ========================================================
+# Example executables
+# ========================================================
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/examples/simple_tf3_example.cpp)
+  add_executable(${PROJECT_NAME}_example examples/simple_tf3_example.cpp)
+  target_include_directories(${PROJECT_NAME}_example PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+  target_link_libraries(${PROJECT_NAME}_example PRIVATE 
+    ${PROJECT_NAME}
+    pthread
+    console_bridge::console_bridge
  )
 endif()

-# Export targets - cần trong mọi trường hợp
-install(EXPORT tf3-targets
-        FILE tf3-targets.cmake
-        NAMESPACE tf3::
-        DESTINATION lib/cmake/tf3)
-
-# Tests
+# ========================================================
+# Test executables
+# ========================================================
 option(BUILD_TESTING "Build tests" OFF)
 if(BUILD_TESTING)
  enable_testing()
-  add_executable(test_cache_unittest test/cache_unittest.cpp)
-  target_link_libraries(test_cache_unittest tf3 ${console_bridge_LIBRARIES})
+  
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/cache_unittest.cpp)
+    add_executable(${PROJECT_NAME}_cache_test test/cache_unittest.cpp)
+    target_link_libraries(${PROJECT_NAME}_cache_test PRIVATE 
+      ${PROJECT_NAME}
+      console_bridge::console_bridge
+    )
+    add_test(NAME ${PROJECT_NAME}_cache_test COMMAND ${PROJECT_NAME}_cache_test)
+  endif()

-  add_executable(test_static_cache_unittest test/static_cache_test.cpp)
-  target_link_libraries(test_static_cache_unittest tf3 ${console_bridge_LIBRARIES})
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/static_cache_test.cpp)
+    add_executable(${PROJECT_NAME}_static_cache_test test/static_cache_test.cpp)
+    target_link_libraries(${PROJECT_NAME}_static_cache_test PRIVATE 
+      ${PROJECT_NAME}
+      console_bridge::console_bridge
+    )
+    add_test(NAME ${PROJECT_NAME}_static_cache_test COMMAND ${PROJECT_NAME}_static_cache_test)
+  endif()

-  add_executable(test_simple test/simple_tf3_core.cpp)
-  target_link_libraries(test_simple tf3 ${console_bridge_LIBRARIES})
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/simple_tf3_core.cpp)
+    add_executable(${PROJECT_NAME}_simple_test test/simple_tf3_core.cpp)
+    target_link_libraries(${PROJECT_NAME}_simple_test PRIVATE 
+      ${PROJECT_NAME}
+      console_bridge::console_bridge
+    )
+    add_test(NAME ${PROJECT_NAME}_simple_test COMMAND ${PROJECT_NAME}_simple_test)
+  endif()

-  add_executable(speed_test test/speed_test.cpp)
-  target_link_libraries(speed_test tf3 ${console_bridge_LIBRARIES})
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/speed_test.cpp)
+    add_executable(${PROJECT_NAME}_speed_test test/speed_test.cpp)
+    target_link_libraries(${PROJECT_NAME}_speed_test PRIVATE 
+      ${PROJECT_NAME}
+      console_bridge::console_bridge
+    )
+    add_test(NAME ${PROJECT_NAME}_speed_test COMMAND ${PROJECT_NAME}_speed_test)
+  endif()

-  add_executable(test_transform_datatypes test/test_transform_datatypes.cpp)
-  target_link_libraries(test_transform_datatypes tf3 ${console_bridge_LIBRARIES})
-
-  add_test(NAME test_cache_unittest COMMAND test_cache_unittest)
-  add_test(NAME test_static_cache_unittest COMMAND test_static_cache_unittest)
-  add_test(NAME test_simple COMMAND test_simple)
-  add_test(NAME speed_test COMMAND speed_test)
-  add_test(NAME test_transform_datatypes COMMAND test_transform_datatypes)
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/test_transform_datatypes.cpp)
+    add_executable(${PROJECT_NAME}_transform_datatypes_test test/test_transform_datatypes.cpp)
+    target_link_libraries(${PROJECT_NAME}_transform_datatypes_test PRIVATE 
+      ${PROJECT_NAME}
+      console_bridge::console_bridge
+    )
+    add_test(NAME ${PROJECT_NAME}_transform_datatypes_test COMMAND ${PROJECT_NAME}_transform_datatypes_test)
+  endif()
 endif()
--- a/src/Libraries/tf3/package.xml
+++ b/src/Libraries/tf3/package.xml
@@ -19,5 +19,7 @@
    
  <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>

-
+  <build_depend>console_bridge</build_depend>
+  <run_depend>console_bridge</run_depend>
+  
 </package>
--- a/src/Libraries/voxel_grid
+++ b/src/Libraries/voxel_grid
--- a/src/Libraries/xmlrpcpp
+++ b/src/Libraries/xmlrpcpp