fix bug nan in function transformLaserScanToPointCloud_

CMakeLists.txt

@@ -1,85 +1,187 @@
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.0.2)
-project(robot_laser_geometry)
+project(robot_laser_geometry VERSION 1.0.0 LANGUAGES CXX)
 
+if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
+  set(BUILDING_WITH_CATKIN TRUE)
+  message(STATUS "Building robot_laser_geometry with Catkin")
+
+else()
+  set(BUILDING_WITH_CATKIN FALSE)
+  message(STATUS "Building robot_laser_geometry 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)
-include(CTest)
 
 # Find dependencies
 find_package(Eigen3 REQUIRED)
 find_package(GTest REQUIRED)
 
-# Include directories
+if (NOT BUILDING_WITH_CATKIN)
-include_directories(
-    include
-    ${EIGEN3_INCLUDE_DIRS}
-    ${GTEST_INCLUDE_DIRS}
-)
 
-# Create library
+  # Enable Position Independent Code
-add_library(robot_laser_geometry SHARED src/laser_geometry.cpp)
+  set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-target_include_directories(robot_laser_geometry
+  # Cấu hình RPATH để tránh cycle trong runtime search path
-    PUBLIC
+  set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-        $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
+  set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
-)
 
-target_link_libraries(robot_laser_geometry PUBLIC
+  set(PACKAGES_DIR  
     robot_sensor_msgs
     geometry_msgs
     robot_time
     tf3
     data_convert
   )
-
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
-if(TARGET Eigen3::Eigen)
-    target_link_libraries(robot_laser_geometry PUBLIC Eigen3::Eigen)
 else()
-    target_include_directories(robot_laser_geometry PUBLIC ${EIGEN3_INCLUDE_DIRS})
+
+# ========================================================
+# Catkin specific configuration
+# ========================================================
+  find_package(catkin REQUIRED COMPONENTS         
+    robot_sensor_msgs
+    geometry_msgs
+    robot_time
+    data_convert
+  )
+
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
+  find_package(Eigen3 REQUIRED)
+
+  catkin_package(
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    CATKIN_DEPENDS robot_sensor_msgs geometry_msgs robot_time data_convert
+    DEPENDS Eigen3 
+  )
+
+  include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${TF3_INCLUDE_DIR}
+  )
+endif()
+
+# Libraries
+add_library(${PROJECT_NAME} SHARED
+  src/laser_geometry.cpp
+)
+
+if(BUILDING_WITH_CATKIN)
+  add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC ${catkin_LIBRARIES}
+    ${TF3_LIBRARY}
+  )
+
+else()
+
+  target_include_directories(${PROJECT_NAME}
+    PUBLIC
+      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+      $<INSTALL_INTERFACE:include>
+  )
+
+  target_link_libraries(${PROJECT_NAME}
+    PUBLIC
+      ${PACKAGES_DIR}
+      ${TF3_LIBRARY}
+  )
+
+  set_target_properties(${PROJECT_NAME} PROPERTIES
+    LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
+    BUILD_RPATH "${CMAKE_BINARY_DIR}"
+    INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
+  )
+
+endif()
+
+# Link Eigen3
+if(TARGET Eigen3::Eigen)
+  target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen)
+else()
+  target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIRS})
 endif()
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
-target_compile_definitions(robot_laser_geometry PRIVATE "ROBOT_LASER_GEOMETRY_BUILDING_LIBRARY")
+target_compile_definitions(${PROJECT_NAME} PRIVATE "ROBOT_LASER_GEOMETRY_BUILDING_LIBRARY")
 
-# --- Cài đặt thư viện vào hệ thống khi chạy make install ---
+if(BUILDING_WITH_CATKIN)
-install(TARGETS robot_laser_geometry
+  ## Mark libraries for installation
-  EXPORT robot_laser_geometry-targets
+  ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-  ARCHIVE DESTINATION lib      # Thư viện tĩnh .a
+  install(TARGETS ${PROJECT_NAME}
-  LIBRARY DESTINATION lib      # Thư viện động .so
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION bin      # File thực thi (nếu có)
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  INCLUDES DESTINATION include # Cài đặt include
+    RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
   )
 
-install(
+  ## Mark cpp header files for installation
-    DIRECTORY include/
+  install(DIRECTORY include/${PROJECT_NAME}/
-    DESTINATION include/${PROJECT_NAME}
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+    PATTERN ".svn" EXCLUDE
   )
 
-# --- Xuất export set robot_laser_geometry-targets thành file CMake module ---
+else()
-# --- Tạo file lib/cmake/robot_laser_geometry/laser_geometry-targets.cmake ---
+
-# --- File này chứa cấu hình giúp project khác có thể dùng ---
+  install(TARGETS ${PROJECT_NAME}
-# --- Find_package(robot_laser_geometry REQUIRED) ---
+    EXPORT ${PROJECT_NAME}-targets
-# --- Target_link_libraries(my_app PRIVATE robot_laser_geometry::robot_laser_geometry) ---
+    ARCHIVE DESTINATION lib
-install(EXPORT robot_laser_geometry-targets 
+    LIBRARY DESTINATION lib
-  FILE robot_laser_geometry-targets.cmake
+    RUNTIME DESTINATION bin
-  NAMESPACE robot_laser_geometry::
-  DESTINATION lib/cmake/robot_laser_geometry
   )
 
+  # Export targets
+  install(EXPORT ${PROJECT_NAME}-targets
+    FILE ${PROJECT_NAME}-targets.cmake
+    NAMESPACE ${PROJECT_NAME}::
+    DESTINATION lib/cmake/${PROJECT_NAME}
+  )
 
-# Tests
+  ## Mark cpp header files for installation
+  install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+    PATTERN ".svn" EXCLUDE
+  )
+
+  # Print configuration info
+  message(STATUS "=================================")
+  message(STATUS "Project: ${PROJECT_NAME}")
+  message(STATUS "Version: ${PROJECT_VERSION}")
+  message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
+  message(STATUS "Dependencies: robot_sensor_msgs, geometry_msgs, robot_time, tf3, data_convert, Eigen3")
+  message(STATUS "=================================")
+endif()
+
+# ========================================================
+# Test executables
+# ========================================================
 if(BUILD_TESTING)
   enable_testing()
   
-    add_executable(projection_test test/projection_test.cpp)
+  if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/projection_test.cpp)
-    target_link_libraries(projection_test
+    add_executable(${PROJECT_NAME}_test test/projection_test.cpp)
-        robot_laser_geometry
+    target_link_libraries(${PROJECT_NAME}_test PRIVATE 
+      ${PROJECT_NAME}
       GTest::GTest
       GTest::Main
-        pthread
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
     )
-    add_test(NAME projection_test COMMAND projection_test)
+    add_test(NAME ${PROJECT_NAME}_test COMMAND ${PROJECT_NAME}_test)
+  endif()
 endif()

package.xml

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

src/laser_geometry.cpp

@@ -463,6 +463,15 @@ void LaserProjection::transformLaserScanToPointCloud_(
   tf3::Vector3 origin_end(end_transform.transform.translation.x,
     end_transform.transform.translation.y,
     end_transform.transform.translation.z);
+
+  if(target_frame == scan_in.header.frame_id)
+  {
+    quat_start = tf3::Quaternion(0, 0, 0, 1);
+    quat_end = tf3::Quaternion(0, 0, 0, 1);
+    origin_start = tf3::Vector3(0, 0, 0);
+    origin_end = tf3::Vector3( 0, 0, 0);
+  }
+  
   transformLaserScanToPointCloud_(
     target_frame, scan_in, cloud_out,
     quat_start, origin_start,