diff --git a/robot_tf3_geometry_msgs/CMakeLists.txt b/robot_tf3_geometry_msgs/CMakeLists.txt
index 8211276..febe311 100755
--- a/robot_tf3_geometry_msgs/CMakeLists.txt
+++ b/robot_tf3_geometry_msgs/CMakeLists.txt
@@ -1,105 +1,181 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_tf3_geometry_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_tf3_geometry_msgs with Catkin")
-  find_package(catkin REQUIRED)
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building robot_tf3_geometry_msgs with Standalone CMake")
 endif()
 
-project(robot_tf3_geometry_msgs)
-
+# 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 dependencies
+find_package(Boost REQUIRED COMPONENTS thread)
+find_package(Eigen3 REQUIRED)
+find_package(GTest REQUIRED)
+
+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_geometry_msgs
+    data_convert
+    tf3
+  )
+
+else()
 
 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_geometry_msgs
+    data_convert
+    tf3
+  )
 
-if(BUILDING_WITH_CATKIN)
   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_geometry_msgs data_convert tf3
     DEPENDS Eigen3 Boost
   )
-endif()
 
-# Find dependencies
-find_package(Boost COMPONENTS thread REQUIRED)
-find_package(GTest REQUIRED)
-# Finding Eigen3
-find_package(Eigen3 REQUIRED)
-
-# Include directories
-include_directories(
-  include
-  ${EIGEN3_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIRS}
-  ${GTEST_INCLUDE_DIRS}
-)
-
-# Install headers
-if(NOT BUILDING_WITH_CATKIN)
-  install(DIRECTORY include/${PROJECT_NAME}/
-    DESTINATION include/${PROJECT_NAME}
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
   )
 endif()
 
-add_library(robot_tf3_geometry_msgs INTERFACE
-)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_tf3_geometry_msgs/*.h")
 
-target_include_directories(robot_tf3_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_tf3_geometry_msgs INTERFACE 
-                      geometry_msgs
-                      data_convert
-                      )
+else()
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-install(TARGETS robot_tf3_geometry_msgs
-  EXPORT robot_tf3_geometry_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>
+  )
 
-# --- Xuất export set robot_tf3_geometry_msgs-targets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_tf3_geometry_msgs/robot_tf3_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_tf3_geometry_msgs REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE tf3_robot_geometry_msgs::robot_tf3_geometry_msgs) ---
-install(EXPORT robot_tf3_geometry_msgs-targets 
-  FILE robot_tf3_geometry_msgs-targets.cmake
-  DESTINATION lib/cmake/robot_tf3_geometry_msgs
-)
-  
-#   # Test: tomsg_frommsg
-  add_executable(test_tomsg_frommsg test/test_tomsg_frommsg.cpp)
-  target_link_libraries(test_tomsg_frommsg 
-    ${GTEST_LIBRARIES}
-    robot_tf3_geometry_msgs
+  # 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: geometry_msgs, data_convert, tf3, Eigen3, Boost")
+  message(STATUS "=================================")
+endif()
+
+# ========================================================
+# Test executables
+# ========================================================
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/test_tomsg_frommsg.cpp)
+  add_executable(${PROJECT_NAME}_tomsg_frommsg_test test/test_tomsg_frommsg.cpp)
+  target_link_libraries(${PROJECT_NAME}_tomsg_frommsg_test PRIVATE 
+    ${PROJECT_NAME}
+    GTest::GTest
+    GTest::Main
     Threads::Threads
     tf3
     data_convert
   )
-#   # Test: tf2_geometry_msgs
-  add_executable(test_geometry_msgs test/test_tf2_geometry_msgs.cpp)
-  target_link_libraries(test_geometry_msgs 
-    ${GTEST_LIBRARIES}
+endif()
+
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_geometry_msgs.cpp)
+  add_executable(${PROJECT_NAME}_geometry_msgs_test test/test_tf2_geometry_msgs.cpp)
+  target_link_libraries(${PROJECT_NAME}_geometry_msgs_test PRIVATE 
+    ${PROJECT_NAME}
+    GTest::GTest
+    GTest::Main
     Threads::Threads
     tf3
-    robot_tf3_geometry_msgs
     data_convert
   )
+endif()
diff --git a/robot_tf3_geometry_msgs/include/robot_tf3_geometry_msgs/tf3_geometry_msgs.h b/robot_tf3_geometry_msgs/include/robot_tf3_geometry_msgs/tf3_geometry_msgs.h
index b28cf5c..6583d16 100755
--- a/robot_tf3_geometry_msgs/include/robot_tf3_geometry_msgs/tf3_geometry_msgs.h
+++ b/robot_tf3_geometry_msgs/include/robot_tf3_geometry_msgs/tf3_geometry_msgs.h
@@ -111,7 +111,11 @@ void fromMsg(const robot_geometry_msgs::Vector3& in, tf3::Vector3& out)
  */
 template <>
 inline
-  const tf3::Time& getTimestamp(const robot_geometry_msgs::Vector3Stamped& t) {return data_convert::convertTime(t.header.stamp);}
+  const tf3::Time& getTimestamp(const robot_geometry_msgs::Vector3Stamped& t) {
+    static thread_local tf3::Time stamp;
+    stamp = data_convert::convertTime(t.header.stamp);
+    return stamp;
+}
 
 /** \brief Extract a frame ID from the header of a Vector message.
  * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@@ -219,7 +223,11 @@ void fromMsg(const robot_geometry_msgs::Point& in, tf3::Vector3& out)
  */
 template <>
 inline
-  const tf3::Time& getTimestamp(const robot_geometry_msgs::PointStamped& t)  {return data_convert::convertTime(t.header.stamp);}
+  const tf3::Time& getTimestamp(const robot_geometry_msgs::PointStamped& t)  {
+    static thread_local tf3::Time stamp;
+    stamp = data_convert::convertTime(t.header.stamp);
+    return stamp;
+}
 
 /** \brief Extract a frame ID from the header of a Point message.
  * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@@ -328,7 +336,11 @@ void fromMsg(const robot_geometry_msgs::Quaternion& in, tf3::Quaternion& out)
  */
 template <>
 inline
-const tf3::Time& getTimestamp(const robot_geometry_msgs::QuaternionStamped& t)  {return data_convert::convertTime(t.header.stamp);}
+const tf3::Time& getTimestamp(const robot_geometry_msgs::QuaternionStamped& t)  {
+  static thread_local tf3::Time stamp;
+  stamp = data_convert::convertTime(t.header.stamp);
+  return stamp;
+}
 
 /** \brief Extract a frame ID from the header of a Quaternion message.
  * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@@ -461,7 +473,11 @@ void fromMsg(const robot_geometry_msgs::Pose& in, tf3::Transform& out)
  */
 template <>
 inline
-  const tf3::Time& getTimestamp(const robot_geometry_msgs::PoseStamped& t)  {return data_convert::convertTime(t.header.stamp);}
+  const tf3::Time& getTimestamp(const robot_geometry_msgs::PoseStamped& t)  {
+    static thread_local tf3::Time stamp;
+    stamp = data_convert::convertTime(t.header.stamp);
+    return stamp;
+}
 
 /** \brief Extract a frame ID from the header of a Pose message.
  * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@@ -535,7 +551,11 @@ void fromMsg(const robot_geometry_msgs::PoseStamped& msg, tf3::Stamped<tf3::Tran
  */
 template <>
 inline
-  const tf3::Time& getTimestamp(const robot_geometry_msgs::PoseWithCovarianceStamped& t)  {return data_convert::convertTime(t.header.stamp);}
+  const tf3::Time& getTimestamp(const robot_geometry_msgs::PoseWithCovarianceStamped& t)  {
+    static thread_local tf3::Time stamp;
+    stamp = data_convert::convertTime(t.header.stamp);
+    return stamp;
+}
 
 /** \brief Extract a frame ID from the header of a PoseWithCovarianceStamped message.
  * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@@ -659,7 +679,11 @@ void fromMsg(const tf3::TransformMsg& in, tf3::Transform& out)
  */
 template <>
 inline
-const tf3::Time& getTimestamp(const robot_geometry_msgs::TransformStamped& t)  {return data_convert::convertTime(t.header.stamp);}
+const tf3::Time& getTimestamp(const robot_geometry_msgs::TransformStamped& t)  {
+  static thread_local tf3::Time stamp;
+  stamp = data_convert::convertTime(t.header.stamp);
+  return stamp;
+}
 
 /** \brief Extract a frame ID from the header of a Transform message.
  * This function is a specialization of the getFrameId template defined in tf3/convert.h.
@@ -1014,7 +1038,11 @@ inline
 /**********************/
 template <>
 inline
-const tf3::Time& getTimestamp(const robot_geometry_msgs::WrenchStamped& t) {return data_convert::convertTime(t.header.stamp);}
+const tf3::Time& getTimestamp(const robot_geometry_msgs::WrenchStamped& t) {
+  static thread_local tf3::Time stamp;
+  stamp = data_convert::convertTime(t.header.stamp);
+  return stamp;
+}
 
 
 template <>
diff --git a/robot_tf3_geometry_msgs/package.xml b/robot_tf3_geometry_msgs/package.xml
index 4dc9ec9..58e93c0 100644
--- a/robot_tf3_geometry_msgs/package.xml
+++ b/robot_tf3_geometry_msgs/package.xml
@@ -19,5 +19,10 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-
+  <build_depend>robot_geometry_msgs</build_depend>
+  <build_depend>data_convert</build_depend>
+  <build_depend>tf3</build_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+  <run_depend>data_convert</run_depend>
+  <run_depend>tf3</run_depend>
 </package>
\ No newline at end of file
diff --git a/robot_tf3_sensor_msgs/CMakeLists.txt b/robot_tf3_sensor_msgs/CMakeLists.txt
index 6b84993..e103ab6 100755
--- a/robot_tf3_sensor_msgs/CMakeLists.txt
+++ b/robot_tf3_sensor_msgs/CMakeLists.txt
@@ -1,98 +1,176 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_tf3_sensor_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_tf3_sensor_msgs with Catkin")
-  find_package(catkin REQUIRED)
+
 else()
   set(BUILDING_WITH_CATKIN FALSE)
   message(STATUS "Building robot_tf3_sensor_msgs with Standalone CMake")
 endif()
 
-project(robot_tf3_sensor_msgs)
-
+# 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 dependencies
+find_package(Boost REQUIRED COMPONENTS thread)
+find_package(Eigen3 REQUIRED)
+find_package(GTest REQUIRED)
+
+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_sensor_msgs
+    data_convert
+    tf3
+    robot_geometry_msgs
+  )
+
+else()
 
 # ========================================================
 # Catkin specific configuration
 # ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_sensor_msgs
+    data_convert
+    tf3
+    robot_geometry_msgs
+  )
 
-if(BUILDING_WITH_CATKIN)
   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_sensor_msgs data_convert tf3 robot_geometry_msgs
     DEPENDS Eigen3 Boost
   )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+  )
 endif()
 
-# Find dependencies
-find_package(Boost COMPONENTS thread REQUIRED)
-find_package(GTest REQUIRED)
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_tf3_sensor_msgs/*.h")
 
-# Finding Eigen3
-find_package(Eigen3 REQUIRED)
+# 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})
 
-# Include directories
-include_directories(
-  include
-  ${EIGEN3_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIRS}
-  ${GTEST_INCLUDE_DIRS}
-)
+  # Set include directories
+  target_include_directories(${PROJECT_NAME}
+    INTERFACE 
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
 
-add_library(robot_tf3_sensor_msgs INTERFACE
-)
-
-target_include_directories(robot_tf3_sensor_msgs
+  # 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}
+  )
 
-target_link_libraries(robot_tf3_sensor_msgs INTERFACE 
-                      robot_sensor_msgs
-                      data_convert
-                      )
+else()
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
-install(TARGETS robot_tf3_sensor_msgs
-  EXPORT robot_tf3_sensor_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>
+  )
 
-# --- Xuất export set robot_tf3_sensor_msgs-targets thành file CMake module ---
-# --- Tạo file lib/cmake/robot_tf3_sensor_msgs/robot_tf3_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_tf3_sensor_msgs REQUIRED) ---
-# --- Target_link_libraries(my_app PRIVATE robot_tf3_sensor_msgs::robot_tf3_sensor_msgs) ---
-install(EXPORT robot_tf3_sensor_msgs-targets 
-  FILE robot_tf3_sensor_msgs-targets.cmake
-  NAMESPACE robot_tf3_sensor_msgs::
-  DESTINATION lib/cmake/robot_tf3_sensor_msgs
-)
-  
-add_executable(test_tf2_sensor_msgs test/test_tf2_sensor_msgs.cpp)
-target_include_directories(test_tf2_sensor_msgs PUBLIC 
-  ${EIGEN3_INCLUDE_DIRS}
-  ${GTEST_INCLUDE_DIRS}
-  robot_tf3_sensor_msgs
-  geometry_msgs
-  tf3
-)
-target_link_libraries(test_tf2_sensor_msgs 
-  ${GTEST_LIBRARIES}
-  Threads::Threads
-  robot_tf3_sensor_msgs
-  geometry_msgs
-  tf3
-  data_convert
-)
+  # 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_sensor_msgs, data_convert, tf3, robot_geometry_msgs, Eigen3, Boost")
+  message(STATUS "=================================")
+endif()
+
+# ========================================================
+# Test executables
+# ========================================================
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_sensor_msgs.cpp)
+  add_executable(${PROJECT_NAME}_test test/test_tf2_sensor_msgs.cpp)
+  target_include_directories(${PROJECT_NAME}_test PUBLIC 
+    ${EIGEN3_INCLUDE_DIRS}
+    ${GTEST_INCLUDE_DIRS}
+  )
+  target_link_libraries(${PROJECT_NAME}_test PRIVATE 
+    ${PROJECT_NAME}
+    GTest::GTest
+    GTest::Main
+    Threads::Threads
+    robot_geometry_msgs
+    tf3
+    data_convert
+  )
+endif()
diff --git a/robot_tf3_sensor_msgs/include/robot_tf3_sensor_msgs/tf3_sensor_msgs.h b/robot_tf3_sensor_msgs/include/robot_tf3_sensor_msgs/tf3_sensor_msgs.h
index 0bdff74..ee170f2 100755
--- a/robot_tf3_sensor_msgs/include/robot_tf3_sensor_msgs/tf3_sensor_msgs.h
+++ b/robot_tf3_sensor_msgs/include/robot_tf3_sensor_msgs/tf3_sensor_msgs.h
@@ -52,7 +52,11 @@ namespace tf3
  */
 template <>
 inline
-const tf3::Time& getTimestamp(const robot_sensor_msgs::PointCloud2& p) {return data_convert::convertTime(p.header.stamp);}
+const tf3::Time& getTimestamp(const robot_sensor_msgs::PointCloud2& p) {
+  static thread_local tf3::Time cached_time;
+  cached_time = data_convert::convertTime(p.header.stamp);
+  return cached_time;
+}
 
 /** \brief Extract a frame ID from the header of a PointCloud2 message.
  * This function is a specialization of the getFrameId template defined in tf3/convert.h.
diff --git a/robot_tf3_sensor_msgs/package.xml b/robot_tf3_sensor_msgs/package.xml
index ab7d351..8b5b267 100644
--- a/robot_tf3_sensor_msgs/package.xml
+++ b/robot_tf3_sensor_msgs/package.xml
@@ -19,6 +19,13 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-
-
+  <build_depend>robot_sensor_msgs</build_depend>
+  <build_depend>tf3</build_depend>
+  <build_depend>robot_geometry_msgs</build_depend>
+  <build_depend>data_convert</build_depend>
+  
+  <run_depend>robot_sensor_msgs</run_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
+  <run_depend>data_convert</run_depend>
+  <run_depend>tf3</run_depend>
 </package>
\ No newline at end of file