update tf3

robot_tf3_geometry_msgs/CMakeLists.txt

@@ -1,75 +1,186 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
-project(robot_tf3_geometry_msgs)
+project(robot_tf3_geometry_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_tf3_geometry_msgs with Catkin")
+
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_tf3_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_POSITION_INDEPENDENT_CODE ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
 
 # Find dependencies
-find_package(Boost COMPONENTS thread REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system thread)
-find_package(GTest REQUIRED)
-# Finding Eigen3
 find_package(Eigen3 REQUIRED)
+find_package(GTest REQUIRED)
 
-# Include directories
+if (NOT BUILDING_WITH_CATKIN)
-include_directories(
-  include
-  ${EIGEN3_INCLUDE_DIRS}
-  ${Boost_INCLUDE_DIRS}
-  ${GTEST_INCLUDE_DIRS}
-)
 
-# Install headers
+  # Enable Position Independent Code
-install(DIRECTORY include/${PROJECT_NAME}/
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-  DESTINATION include/${PROJECT_NAME}
-)
 
-add_library(robot_tf3_geometry_msgs INTERFACE
+  # 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}")
 
-target_include_directories(robot_tf3_geometry_msgs
+  set(PACKAGES_DIR  
-    INTERFACE
+    robot_geometry_msgs
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    data_convert
-        $<INSTALL_INTERFACE:include>
+    tf3
-)
+  )
 
-# Liên kết với robot_std_msgs nếu bạn có file Header.h trong include/robot_std_msgs/
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
-target_link_libraries(robot_tf3_geometry_msgs INTERFACE 
-                      geometry_msgs
-                      data_convert
-                      )
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
+else()
-install(TARGETS robot_tf3_geometry_msgs
-  EXPORT robot_tf3_geometry_msgs-targets
-  INCLUDES DESTINATION include # Cài đặt 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 ---
+# Catkin specific configuration
-# --- 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) ---
+  find_package(catkin REQUIRED COMPONENTS         
-# --- Target_link_libraries(my_app PRIVATE tf3_robot_geometry_msgs::robot_tf3_geometry_msgs) ---
+    robot_geometry_msgs
-install(EXPORT robot_tf3_geometry_msgs-targets 
+    data_convert
-  FILE robot_tf3_geometry_msgs-targets.cmake
+  )
-  DESTINATION lib/cmake/robot_tf3_geometry_msgs
+
-)
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
   
-#   # Test: tomsg_frommsg
+  catkin_package(
-  add_executable(test_tomsg_frommsg test/test_tomsg_frommsg.cpp)
+    INCLUDE_DIRS include
-  target_link_libraries(test_tomsg_frommsg 
+    # LIBRARIES không cần vì đây là header-only library
-    ${GTEST_LIBRARIES}
+    CATKIN_DEPENDS robot_geometry_msgs data_convert
-    robot_tf3_geometry_msgs
+    DEPENDS Eigen3 Boost
-    Threads::Threads
+  )
-    tf3
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+    ${TF3_INCLUDE_DIR}
+  )
+endif()
+
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_tf3_geometry_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}
+      ${TF3_LIBRARY}
+  )
+
+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
+    Boost::system Boost::thread
+    ${TF3_LIBRARY}
     data_convert
   )
-#   # Test: tf2_geometry_msgs
+endif()
-  add_executable(test_geometry_msgs test/test_tf2_geometry_msgs.cpp)
+
-  target_link_libraries(test_geometry_msgs 
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/test_tf2_geometry_msgs.cpp)
-    ${GTEST_LIBRARIES}
+  add_executable(${PROJECT_NAME}_geometry_msgs_test test/test_tf2_geometry_msgs.cpp)
-    Threads::Threads
+  target_link_libraries(${PROJECT_NAME}_geometry_msgs_test PRIVATE 
-    tf3
+    ${PROJECT_NAME}
-    robot_tf3_geometry_msgs
+    GTest::GTest
+    GTest::Main
+    Boost::system Boost::thread
+    ${TF3_LIBRARY}
     data_convert
   )
+endif()

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 <>

robot_tf3_geometry_msgs/package.xml

@@ -19,8 +19,8 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>libconsole-bridge-dev</build_depend>
+  <build_depend>robot_geometry_msgs</build_depend>
-
+  <build_depend>data_convert</build_depend>
-  <run_depend>libconsole-bridge-dev</run_depend>
+  <run_depend>robot_geometry_msgs</run_depend>
-
+  <run_depend>data_convert</run_depend>
 </package>

robot_tf3_sensor_msgs/CMakeLists.txt

@@ -1,70 +1,181 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
-project(robot_tf3_sensor_msgs)
+project(robot_tf3_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_tf3_sensor_msgs with Catkin")
+
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_tf3_sensor_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_POSITION_INDEPENDENT_CODE ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
 
 # Find dependencies
-find_package(Boost COMPONENTS thread REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system thread)
+find_package(Eigen3 REQUIRED)
 find_package(GTest REQUIRED)
 
-# Finding Eigen3
+if (NOT BUILDING_WITH_CATKIN)
-find_package(Eigen3 REQUIRED)
 
+  # Enable Position Independent Code
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-# Include directories
+  # Cấu hình RPATH để tránh cycle trong runtime search path
-include_directories(
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
-  include
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-  ${EIGEN3_INCLUDE_DIRS}
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
-  ${Boost_INCLUDE_DIRS}
-  ${GTEST_INCLUDE_DIRS}
-)
 
-add_library(robot_tf3_sensor_msgs INTERFACE
+  set(PACKAGES_DIR  
-)
+    robot_sensor_msgs
+    data_convert
+    tf3
+    robot_geometry_msgs
+  )
 
-target_include_directories(robot_tf3_sensor_msgs
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
-    INTERFACE
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-)
 
-target_link_libraries(robot_tf3_sensor_msgs INTERFACE 
+else()
-                      robot_sensor_msgs
-                      data_convert
-                      )
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
+# ========================================================
-install(TARGETS robot_tf3_sensor_msgs
+# Catkin specific configuration
-  EXPORT robot_tf3_sensor_msgs-targets
+# ========================================================
-  INCLUDES DESTINATION include # Cài đặt include
+  find_package(catkin REQUIRED COMPONENTS         
-)
+    robot_sensor_msgs
+    data_convert
+    robot_geometry_msgs
+  )
 
-# --- Xuất export set robot_tf3_sensor_msgs-targets thành file CMake module ---
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
-# --- 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)
+  catkin_package(
-target_include_directories(test_tf2_sensor_msgs PUBLIC 
+    INCLUDE_DIRS include
-  ${EIGEN3_INCLUDE_DIRS}
+    # LIBRARIES không cần vì đây là header-only library
-  ${GTEST_INCLUDE_DIRS}
+    CATKIN_DEPENDS robot_sensor_msgs data_convert robot_geometry_msgs
-  robot_tf3_sensor_msgs
+    DEPENDS Eigen3 Boost
-  geometry_msgs
+  )
-  tf3
+
-)
+  include_directories(
-target_link_libraries(test_tf2_sensor_msgs 
+    include
-  ${GTEST_LIBRARIES}
+    ${catkin_INCLUDE_DIRS}
-  Threads::Threads
+    ${EIGEN3_INCLUDE_DIRS}
-  robot_tf3_sensor_msgs
+    ${Boost_INCLUDE_DIRS}
-  geometry_msgs
+    ${TF3_INCLUDE_DIR}
-  tf3
+  )
-  data_convert
+endif()
-)
+
+# Tìm tất cả header files
+file(GLOB_RECURSE HEADERS "include/robot_tf3_sensor_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}
+      ${TF3_LIBRARY}
+  )
+
+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
+    Boost::system Boost::thread
+    robot_geometry_msgs
+    ${TF3_LIBRARY}
+    data_convert
+  )
+endif()

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.

robot_tf3_sensor_msgs/package.xml

@@ -19,8 +19,11 @@
     
   <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
 
-  <build_depend>libconsole-bridge-dev</build_depend>
+  <build_depend>robot_sensor_msgs</build_depend>
-
+  <build_depend>robot_geometry_msgs</build_depend>
-  <run_depend>libconsole-bridge-dev</run_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>
 </package>