fix bug nan in function transformLaserScanToPointCloud_

CMakeLists.txt

@@ -36,7 +36,7 @@ if (NOT BUILDING_WITH_CATKIN)
     tf3
     data_convert
   )
-
+  find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()
 
 # ========================================================
@@ -46,21 +46,24 @@ else()
     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)
+  find_package(Eigen3 REQUIRED)
+
   catkin_package(
     INCLUDE_DIRS include
     LIBRARIES ${PROJECT_NAME}
-    CATKIN_DEPENDS robot_sensor_msgs geometry_msgs robot_time tf3 data_convert
-    DEPENDS Eigen3
+    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()
 
@@ -80,6 +83,7 @@ if(BUILDING_WITH_CATKIN)
 
   target_link_libraries(${PROJECT_NAME}
     PUBLIC ${catkin_LIBRARIES}
+    ${TF3_LIBRARY}
   )
 
 else()
@@ -93,6 +97,7 @@ else()
   target_link_libraries(${PROJECT_NAME}
     PUBLIC
       ${PACKAGES_DIR}
+      ${TF3_LIBRARY}
   )
 
   set_target_properties(${PROJECT_NAME} PROPERTIES
@@ -174,7 +179,8 @@ if(BUILD_TESTING)
       ${PROJECT_NAME}
       GTest::GTest
       GTest::Main
-      pthread
+      Boost::system Boost::thread
+      ${TF3_LIBRARY}
     )
     add_test(NAME ${PROJECT_NAME}_test COMMAND ${PROJECT_NAME}_test)
   endif()

package.xml

@@ -28,9 +28,6 @@
   <build_depend>robot_time</build_depend>
   <run_depend>robot_time</run_depend>
 
-  <build_depend>tf3</build_depend>
-  <run_depend>tf3</run_depend>
-
   <build_depend>data_convert</build_depend>
   <run_depend>data_convert</run_depend>
 

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,