diff --git a/robot_geometry_msgs/CMakeLists.txt b/robot_geometry_msgs/CMakeLists.txt
index 1397dcd..12c7f89 100644
--- a/robot_geometry_msgs/CMakeLists.txt
+++ b/robot_geometry_msgs/CMakeLists.txt
@@ -1,81 +1,155 @@
-cmake_minimum_required(VERSION 3.10)
-project(robot_geometry_msgs)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_geometry_msgs VERSION 1.0.0 LANGUAGES CXX)
 
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# ========================================================
-# 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_geometry_msgs with Catkin")
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building robot_geometry_msgs 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)
+
+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_std_msgs
+    utils
+    robot_time
+  )
+
+else()
+
 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_std_msgs
+    utils
+    robot_time
+  )
 
-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS robot_std_msgs utils robot_time)
-  ## The catkin_package macro generates cmake config files for your package
-  ## Note: Dependencies (robot_std_msgs, utils, etc.) 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
+    CATKIN_DEPENDS robot_std_msgs utils robot_time
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
   )
 endif()
 
-# Thư viện header-only
-add_library(robot_geometry_msgs INTERFACE)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_geometry_msgs/*.h")
 
-# Include path tới thư mục chứa file header
-target_include_directories(robot_geometry_msgs
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+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
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
+      ${catkin_LIBRARIES}
+  )
 
-# Liên kết với robot_std_msgs nếu bạn có file Header.h trong include/robot_std_msgs/
-target_link_libraries(robot_geometry_msgs INTERFACE robot_std_msgs utils)
+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()
 
 # Create alias for backward compatibility
-add_library(geometry_msgs ALIAS robot_geometry_msgs)
+add_library(geometry_msgs ALIAS ${PROJECT_NAME})
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-# 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 robot_geometry_msgs
-  EXPORT robot_geometry_msgs-targets
-  INCLUDES DESTINATION include # Cài đặt include
-)
+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)
-  # Install headers for standalone mode
-  install(DIRECTORY include/
+  ## 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 "Dependencies: robot_std_msgs, utils, robot_time")
+  message(STATUS "=================================")
 endif()
 
-# --- Xuất export set robot_geometry_msgs-targets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_geometry_msgs/robot_geometry_msgs-targets.cmake ---
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(robot_geometry_msgs REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE robot_geometry_msgs::robot_geometry_msgs) ---
-install(EXPORT robot_geometry_msgs-targets 
-  FILE robot_geometry_msgs-targets.cmake
-  NAMESPACE robot_geometry_msgs::
-  DESTINATION lib/cmake/robot_geometry_msgs
-)
-
-add_executable(test_geometry test/main.cpp)
-target_link_libraries(test_geometry PRIVATE robot_geometry_msgs)
-
+# ========================================================
+# Test executable
+# ========================================================
+add_executable(${PROJECT_NAME}_test test/main.cpp)
+target_link_libraries(${PROJECT_NAME}_test PRIVATE ${PROJECT_NAME})
diff --git a/robot_map_msgs/CMakeLists.txt b/robot_map_msgs/CMakeLists.txt
index 82d7abd..68ec6d3 100644
--- a/robot_map_msgs/CMakeLists.txt
+++ b/robot_map_msgs/CMakeLists.txt
@@ -1,65 +1,142 @@
-cmake_minimum_required(VERSION 3.10)
-project(robot_map_msgs)
-
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# ========================================================
-# Catkin specific configuration
-# ========================================================
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_map_msgs VERSION 1.0.0 LANGUAGES CXX)
 
 if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
   set(BUILDING_WITH_CATKIN TRUE)
   message(STATUS "Building robot_map_msgs with Catkin")
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building robot_map_msgs with Standalone CMake")
 endif()
 
-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS robot_std_msgs)
-  ## The catkin_package macro generates cmake config files for your package
+# 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  
+    robot_std_msgs
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_std_msgs
+  )
+
   catkin_package(
     INCLUDE_DIRS include
     # LIBRARIES không cần vì đây là header-only library
+    CATKIN_DEPENDS robot_std_msgs
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
   )
 endif()
 
-# Thư viện header-only
-add_library(robot_map_msgs INTERFACE)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_map_msgs/*.h")
 
-# Include path tới thư mục chứa file header
-target_include_directories(robot_map_msgs
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+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
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
+      ${catkin_LIBRARIES}
+  )
 
-# Liên kết với robot_std_msgs nếu bạn có file Header.h trong include/robot_std_msgs/
-target_link_libraries(robot_map_msgs INTERFACE robot_std_msgs)
+else()
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-# 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 robot_map_msgs
-  EXPORT robot_map_msgs-targets
-  INCLUDES DESTINATION include # Cài đặt include
-)
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
 
-if(NOT BUILDING_WITH_CATKIN)
-  # Install headers for standalone mode
-  install(DIRECTORY include/
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${PACKAGES_DIR}
+  )
+
+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}
+  )
+
+  ## 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
   )
-endif()
 
-# --- Xuất export set robot_map_msgs-targets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_map_msgs/robot_map_msgs-targets.cmake ---
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(robot_map_msgs REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE robot_map_msgs::robot_map_msgs) ---
-install(EXPORT robot_map_msgs-targets 
-  FILE robot_map_msgs-targets.cmake
-  NAMESPACE robot_map_msgs::
-  DESTINATION lib/cmake/robot_map_msgs
-)
\ No newline at end of file
+  # 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")
+  message(STATUS "=================================")
+endif()
diff --git a/robot_nav_msgs/CMakeLists.txt b/robot_nav_msgs/CMakeLists.txt
index c2f9bdc..4e6e463 100644
--- a/robot_nav_msgs/CMakeLists.txt
+++ b/robot_nav_msgs/CMakeLists.txt
@@ -1,76 +1,152 @@
-cmake_minimum_required(VERSION 3.10)
-project(robot_nav_msgs)
-
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# ========================================================
-# Catkin specific configuration
-# ========================================================
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_nav_msgs VERSION 1.0.0 LANGUAGES CXX)
 
 if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
   set(BUILDING_WITH_CATKIN TRUE)
   message(STATUS "Building robot_nav_msgs with Catkin")
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building robot_nav_msgs with Standalone CMake")
 endif()
 
-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS robot_std_msgs robot_geometry_msgs)
-  ## The catkin_package macro generates cmake config files for your package
-  ## Note: Dependencies (robot_std_msgs, robot_geometry_msgs, etc.) 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
+# 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  
+    robot_std_msgs
+    robot_geometry_msgs
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_std_msgs
+    robot_geometry_msgs
+  )
+
   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
+    CATKIN_DEPENDS robot_std_msgs robot_geometry_msgs
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
   )
 endif()
 
-# Thư viện header-only
-add_library(robot_nav_msgs INTERFACE)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_nav_msgs/*.h")
 
-# Include path tới thư mục chứa file header
-target_include_directories(robot_nav_msgs
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+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
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
+      ${catkin_LIBRARIES}
+  )
 
-# Liên kết với robot_std_msgs nếu bạn có file Header.h trong include/robot_std_msgs/
-target_link_libraries(robot_nav_msgs INTERFACE 
-                      robot_std_msgs
-                      robot_nav_msgs
-                      geometry_msgs
-                      )
+else()
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-# 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 robot_nav_msgs
-  EXPORT robot_nav_msgs-targets
-  INCLUDES DESTINATION include # Cài đặt include
-)
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
 
-if(NOT BUILDING_WITH_CATKIN)
-  # Install headers for standalone mode
-  install(DIRECTORY include/
+  # Link dependencies (header-only, chỉ cần include paths)
+  target_link_libraries(${PROJECT_NAME}
+    INTERFACE
+      ${PACKAGES_DIR}
+  )
+
+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}
+  )
+
+  ## 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 "Dependencies: robot_std_msgs, robot_geometry_msgs")
+  message(STATUS "=================================")
 endif()
 
-# --- Xuất export set robot_nav_msgs-targets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_nav_msgs/robot_nav_msgs-targets.cmake ---
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(robot_nav_msgs REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE robot_nav_msgs::robot_nav_msgs) ---
-install(EXPORT robot_nav_msgs-targets 
-  FILE robot_nav_msgs-targets.cmake
-  NAMESPACE robot_nav_msgs::
-  DESTINATION lib/cmake/robot_nav_msgs
-)
-
-add_executable(test_nav test/main.cpp)
-target_link_libraries(test_nav PRIVATE robot_nav_msgs)
+# ========================================================
+# Test executables
+# ========================================================
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/main.cpp)
+  add_executable(${PROJECT_NAME}_test test/main.cpp)
+  target_link_libraries(${PROJECT_NAME}_test PRIVATE ${PROJECT_NAME})
+endif()
diff --git a/robot_protocol_msgs/CMakeLists.txt b/robot_protocol_msgs/CMakeLists.txt
index d1188d0..ed9a2ff 100644
--- a/robot_protocol_msgs/CMakeLists.txt
+++ b/robot_protocol_msgs/CMakeLists.txt
@@ -1,12 +1,6 @@
-cmake_minimum_required(VERSION 3.10)
-project(robot_protocol_msgs)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_protocol_msgs VERSION 1.0.0 LANGUAGES CXX)
 
-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 robot_protocol_msgs with Catkin")
@@ -16,68 +10,151 @@ else()
   message(STATUS "Building robot_protocol_msgs with Standalone CMake")
 endif()
 
-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS robot_std_msgs robot_geometry_msgs)
-  ## The catkin_package macro generates cmake config files for your package
+# 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  
+    robot_std_msgs
+    robot_geometry_msgs
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_std_msgs
+    robot_geometry_msgs
+  )
+
   catkin_package(
     INCLUDE_DIRS include
-    LIBRARIES robot_protocol_msgs
+    # LIBRARIES không cần vì đây là header-only library
     CATKIN_DEPENDS robot_std_msgs robot_geometry_msgs
   )
-endif()
 
-# Thư viện header-only
-add_library(robot_protocol_msgs INTERFACE)
-
-# Include path tới thư mục chứa file header
-target_include_directories(robot_protocol_msgs
-    INTERFACE
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-
-# Liên kết với robot_std_msgs nếu bạn có file Header.h trong include/robot_std_msgs/
-target_link_libraries(robot_protocol_msgs INTERFACE)
-
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-# 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 robot_protocol_msgs
-  EXPORT robot_protocol_msgs-targets
-  INCLUDES DESTINATION include # Cài đặt include
-)
-
-if(NOT BUILDING_WITH_CATKIN)
-  # Install headers for standalone mode
-  install(DIRECTORY include/
-    DESTINATION include
-    FILES_MATCHING PATTERN "*.h"
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
   )
 endif()
 
-# --- Xuất export set robot_protocol_msgs-targets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_protocol_msgs/robot_protocol_msgs-targets.cmake ---
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(robot_protocol_msgs REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE robot_protocol_msgs::robot_protocol_msgs) ---
-install(EXPORT robot_protocol_msgs-targets 
-  FILE robot_protocol_msgs-targets.cmake
-  # NAMESPACE robot_protocol_msgs::
-  DESTINATION lib/cmake/robot_protocol_msgs
-)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_protocol_msgs/*.h")
 
-# ========================================================
-# Install CMake Config File for find_package()
-# ========================================================
-# Generate config file from template
-configure_file(
-  ${CMAKE_CURRENT_SOURCE_DIR}/robot_protocol_msgsConfig.cmake.in
-  ${CMAKE_CURRENT_BINARY_DIR}/robot_protocol_msgsConfig.cmake
-  @ONLY
-)
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
 
-# Install config file
-install(FILES
-  ${CMAKE_CURRENT_BINARY_DIR}/robot_protocol_msgsConfig.cmake
-  DESTINATION lib/cmake/robot_protocol_msgs
-)
+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}
+  )
+
+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()
+
+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}
+  )
+
+  ## 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
+  )
+
+  # ========================================================
+  # Install CMake Config File for find_package() (Standalone only)
+  # ========================================================
+  # Generate config file from template
+  configure_file(
+    ${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}Config.cmake.in
+    ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake
+    @ONLY
+  )
+
+  # Install config file
+  install(FILES
+    ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
+
+  # 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, robot_geometry_msgs")
+  message(STATUS "=================================")
+endif()
diff --git a/robot_sensor_msgs/CMakeLists.txt b/robot_sensor_msgs/CMakeLists.txt
index 68f5cc3..dc67d36 100644
--- a/robot_sensor_msgs/CMakeLists.txt
+++ b/robot_sensor_msgs/CMakeLists.txt
@@ -1,70 +1,152 @@
-cmake_minimum_required(VERSION 3.10)
-project(robot_sensor_msgs)
-
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# ========================================================
-# Catkin specific configuration
-# ========================================================
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_sensor_msgs VERSION 1.0.0 LANGUAGES CXX)
 
 if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
   set(BUILDING_WITH_CATKIN TRUE)
   message(STATUS "Building robot_sensor_msgs with Catkin")
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building robot_sensor_msgs with Standalone CMake")
 endif()
 
-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS robot_std_msgs robot_geometry_msgs)
-  ## The catkin_package macro generates cmake config files for your package
+# 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  
+    robot_std_msgs
+    robot_geometry_msgs
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_std_msgs
+    robot_geometry_msgs
+  )
+
   catkin_package(
     INCLUDE_DIRS include
-    LIBRARIES robot_sensor_msgs
+    # LIBRARIES không cần vì đây là header-only library
     CATKIN_DEPENDS robot_std_msgs robot_geometry_msgs
   )
-endif()
 
-# Thư viện header-only
-add_library(robot_sensor_msgs INTERFACE)
-
-# Include path tới thư mục chứa file header
-target_include_directories(robot_sensor_msgs
-    INTERFACE
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-
-# Liên kết với robot_std_msgs nếu bạn có file Header.h trong include/robot_std_msgs/
-target_link_libraries(robot_sensor_msgs INTERFACE robot_std_msgs)
-
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-# 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 robot_sensor_msgs
-  EXPORT robot_sensor_msgs-targets
-  INCLUDES DESTINATION include # Cài đặt include
-)
-
-if(NOT BUILDING_WITH_CATKIN)
-  # Install headers for standalone mode
-  install(DIRECTORY include/
-    DESTINATION include
-    FILES_MATCHING PATTERN "*.h"
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
   )
 endif()
 
-# --- Xuất export set robot_sensor_msgs-targets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_sensor_msgs/robot_sensor_msgs-targets.cmake ---
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(robot_sensor_msgs REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE robot_sensor_msgs::robot_sensor_msgs) ---
-install(EXPORT robot_sensor_msgs-targets 
-  FILE robot_sensor_msgs-targets.cmake
-  NAMESPACE robot_sensor_msgs::
-  DESTINATION lib/cmake/robot_sensor_msgs
-)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_sensor_msgs/*.h")
 
-# Tạo file test ví dụ
-add_executable(test_battery_state test/main.cpp)
-target_link_libraries(test_battery_state PRIVATE robot_sensor_msgs)
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+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}
+  )
+
+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()
+
+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}
+  )
+
+  ## 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 "Dependencies: robot_std_msgs, robot_geometry_msgs")
+  message(STATUS "=================================")
+endif()
+
+# ========================================================
+# Test executables
+# ========================================================
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/main.cpp)
+  add_executable(${PROJECT_NAME}_test test/main.cpp)
+  target_link_libraries(${PROJECT_NAME}_test PRIVATE ${PROJECT_NAME})
+endif()
diff --git a/robot_std_msgs/CMakeLists.txt b/robot_std_msgs/CMakeLists.txt
index 244eb4f..f38f1e9 100644
--- a/robot_std_msgs/CMakeLists.txt
+++ b/robot_std_msgs/CMakeLists.txt
@@ -1,66 +1,142 @@
-cmake_minimum_required(VERSION 3.10)
-project(robot_std_msgs)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_std_msgs VERSION 1.0.0 LANGUAGES CXX)
 
-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 robot_std_msgs with Catkin")
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building robot_std_msgs with Standalone CMake")
 endif()
 
-if(BUILDING_WITH_CATKIN)
-  find_package(catkin REQUIRED COMPONENTS robot_time)
+# 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  
+    robot_time
+  )
+
+else()
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_time
+  )
 
-  ## The catkin_package macro generates cmake config files for your package
-  ## Note: Dependencies (robot_time, etc.) 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 robot_std_msgs
+    # LIBRARIES không cần vì đây là header-only library
     CATKIN_DEPENDS robot_time
   )
-endif()
 
-add_library(robot_std_msgs INTERFACE)
-
-target_include_directories(robot_std_msgs
-    INTERFACE
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
-
-target_link_libraries(robot_std_msgs INTERFACE robot_time)
-
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-# 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 robot_std_msgs
-  EXPORT robot_std_msgs-targets
-  INCLUDES DESTINATION include # Cài đặt include
-)
-
-if(NOT BUILDING_WITH_CATKIN)
-  # Install headers for standalone mode
-  install(DIRECTORY include/
-    DESTINATION include
-    FILES_MATCHING PATTERN "*.h"
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
   )
 endif()
 
-# --- Xuất export set robot_std_msgs-targets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_std_msgs/robot_std_msgs-targets.cmake ---
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(robot_std_msgs REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE robot_std_msgs::robot_std_msgs) ---
-install(EXPORT robot_std_msgs-targets 
-  FILE robot_std_msgs-targets.cmake
-  NAMESPACE robot_std_msgs::
-  DESTINATION lib/cmake/robot_std_msgs
-)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_std_msgs/*.h")
+
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+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}
+  )
+
+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()
+
+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}
+  )
+
+  ## 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 "Dependencies: robot_time")
+  message(STATUS "=================================")
+endif()
diff --git a/robot_visualization_msgs/CMakeLists.txt b/robot_visualization_msgs/CMakeLists.txt
index df44e7f..0ce2bab 100644
--- a/robot_visualization_msgs/CMakeLists.txt
+++ b/robot_visualization_msgs/CMakeLists.txt
@@ -1,86 +1,142 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_visualization_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_visualization_msgs with Catkin")
-  find_package(catkin REQUIRED)
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building robot_visualization_msgs with Standalone CMake")
 endif()
 
-project(robot_visualization_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  
+    robot_std_msgs
+  )
+
+else()
 
 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_std_msgs
+  )
 
-if(BUILDING_WITH_CATKIN)
-  ## The catkin_package macro generates cmake config files for your package
-  ## Note: Dependencies (robot_std_msgs) is not a Catkin package,
-  ## it is built as a CMake target in the same workspace and will be available
-  ## when this package is built. It is 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
+    CATKIN_DEPENDS robot_std_msgs
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
   )
 endif()
 
-# Thư viện header-only
-add_library(robot_visualization_msgs INTERFACE)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_visualization_msgs/*.h")
 
-# Include path tới thư mục chứa file header
-target_include_directories(robot_visualization_msgs
-    INTERFACE
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
 
-# Add include directories from dependencies explicitly for Catkin build
 if(BUILDING_WITH_CATKIN)
-  # Use relative paths from current source directory
-  # From robot_visualization_msgs (pnkx_robot_nav_core2/src/Libraries/common_msgs/robot_visualization_msgs) 
-  # to robot_std_msgs (pnkx_robot_nav_core2/src/Libraries/common_msgs/robot_std_msgs) is ../robot_std_msgs
-  get_filename_component(ROBOT_STD_MSGS_INCLUDE "${CMAKE_CURRENT_SOURCE_DIR}/../robot_std_msgs/include" ABSOLUTE)
-  target_include_directories(robot_visualization_msgs INTERFACE
-    $<BUILD_INTERFACE:${ROBOT_STD_MSGS_INCLUDE}>
+  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()
 
-# Liên kết với robot_std_msgs nếu bạn có file Header.h trong include/robot_std_msgs/
-target_link_libraries(robot_visualization_msgs INTERFACE robot_std_msgs)
+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 thư viện vào hệ thống khi chạy make install ---
-# 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 robot_visualization_msgs
-  EXPORT robot_visualization_msgs-targets
-  INCLUDES DESTINATION include # Cài đặt include
-)
+  ## 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)
-  # Install headers for standalone mode
-  install(DIRECTORY include/
+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
   )
-endif()
 
-# --- Xuất export set robot_visualization_msgs-targets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_visualization_msgs/robot_visualization_msgs-targets.cmake ---
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
-# --- Find_package(robot_visualization_msgs REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE robot_visualization_msgs::robot_visualization_msgs) ---
-install(EXPORT robot_visualization_msgs-targets 
-  FILE robot_visualization_msgs-targets.cmake
-  NAMESPACE robot_visualization_msgs::
-  DESTINATION lib/cmake/robot_visualization_msgs
-)
+  # 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")
+  message(STATUS "=================================")
+endif()
diff --git a/robot_visualization_msgs/package.xml b/robot_visualization_msgs/package.xml
index 538ceda..caa21a2 100644
--- a/robot_visualization_msgs/package.xml
+++ b/robot_visualization_msgs/package.xml
@@ -19,5 +19,6 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-
+  <build_depend>robot_std_msgs</build_depend>
+  <run_depend>robot_std_msgs</run_depend>
 </package>
\ No newline at end of file
diff --git a/utils/CMakeLists.txt b/utils/CMakeLists.txt
index cab1ac6..ba19126 100644
--- a/utils/CMakeLists.txt
+++ b/utils/CMakeLists.txt
@@ -1,62 +1,122 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(utils 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 utils with Catkin")
-  find_package(catkin REQUIRED)
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building utils with Standalone CMake")
 endif()
 
-# --- Project riêng cho utils ---
-project(utils LANGUAGES CXX)
+# 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}")
+
+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 không cần vì đây là header-only library
   )
-endif()
 
-# --- Tạo INTERFACE library (header-only) ---
-add_library(utils INTERFACE)
-
-# --- Include directories ---
-target_include_directories(utils
-    INTERFACE
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>   # build nội bộ
-        $<INSTALL_INTERFACE:include>                             # dùng khi install/export
-)
-
-# --- Cài đặt header files ---
-if(NOT BUILDING_WITH_CATKIN)
-  install(DIRECTORY include/
-          DESTINATION include
-          FILES_MATCHING PATTERN "*.h"
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
   )
 endif()
 
-# --- Cài đặt target INTERFACE để export ---
-# 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 utils
-        EXPORT utils-targets
-        INCLUDES DESTINATION include
-)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/*.h")
 
-# --- Export target file ---
-install(EXPORT utils-targets
-        FILE utils-targets.cmake
-        NAMESPACE utils::
-        DESTINATION lib/cmake/utils
-)
+# Tạo INTERFACE library (header-only)
+add_library(${PROJECT_NAME} INTERFACE)
+
+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>
+  )
+
+else()
+
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+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}
+  )
+
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/
+    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/
+    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 "=================================")
+endif()