diff --git a/CMakeLists.txt b/CMakeLists.txt index 0e9b55e..f6942b0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,6 +44,7 @@ target_link_libraries(laser_geometry PUBLIC geometry_msgs robot_time tf3 + data_convert ) if(TARGET Eigen3::Eigen) diff --git a/build/CMakeCache.txt b/build/CMakeCache.txt index 67ad273..a996c17 100644 --- a/build/CMakeCache.txt +++ b/build/CMakeCache.txt @@ -310,7 +310,7 @@ geometry_msgs_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/common_msgs/geometry laser_geometry_BINARY_DIR:STATIC=/home/duongtd/robotics_core/laser_geometry/build //Dependencies for the target -laser_geometry_LIB_DEPENDS:STATIC=general;robot_time;general;tf2; +laser_geometry_LIB_DEPENDS:STATIC=general;robot_time;general;tf3; //Value Computed by CMake laser_geometry_SOURCE_DIR:STATIC=/home/duongtd/robotics_core/laser_geometry diff --git a/build/CMakeFiles/laser_geometry.dir/CXX.includecache b/build/CMakeFiles/laser_geometry.dir/CXX.includecache index ed3764e..32d688c 100644 --- a/build/CMakeFiles/laser_geometry.dir/CXX.includecache +++ b/build/CMakeFiles/laser_geometry.dir/CXX.includecache @@ -55,9 +55,9 @@ sys/time.h ../include/laser_geometry/data_convert.h geometry_msgs/TransformStamped.h - -tf2/utils.h +tf3/utils.h - -tf2/compat.h +tf3/compat.h - robot/time.h - @@ -73,8 +73,8 @@ string - Eigen/Core - -tf2/buffer_core.h -../include/laser_geometry/tf2/buffer_core.h +tf3/buffer_core.h +../include/laser_geometry/tf3/buffer_core.h sensor_msgs/LaserScan.h - sensor_msgs/PointCloud2.h @@ -155,12 +155,12 @@ sensor_msgs/LaserScan.h - sensor_msgs/PointCloud2.h - -tf2/buffer_core.h +tf3/buffer_core.h - -tf2/convert.h +tf3/convert.h - -tf2/LinearMath/Transform.h -/home/duongtd/robotics_core/laser_geometry/src/tf2/LinearMath/Transform.h +tf3/LinearMath/Transform.h +/home/duongtd/robotics_core/laser_geometry/src/tf3/LinearMath/Transform.h /usr/include/eigen3/Eigen/Core src/Core/util/DisableStupidWarnings.h diff --git a/build/CMakeFiles/laser_geometry.dir/link.txt b/build/CMakeFiles/laser_geometry.dir/link.txt index 858f90d..153e7b0 100644 --- a/build/CMakeFiles/laser_geometry.dir/link.txt +++ b/build/CMakeFiles/laser_geometry.dir/link.txt @@ -1 +1 @@ -/usr/bin/c++ -fPIC -shared -Wl,-soname,liblaser_geometry.so -o liblaser_geometry.so CMakeFiles/laser_geometry.dir/src/laser_geometry.cpp.o robot_time_build/librobot_time.a -ltf2 robot_time_build/librobot_time.a +/usr/bin/c++ -fPIC -shared -Wl,-soname,liblaser_geometry.so -o liblaser_geometry.so CMakeFiles/laser_geometry.dir/src/laser_geometry.cpp.o robot_time_build/librobot_time.a -ltf3 robot_time_build/librobot_time.a diff --git a/build/CMakeFiles/projection_test.dir/CXX.includecache b/build/CMakeFiles/projection_test.dir/CXX.includecache index 7ee94ee..dd6d2f0 100644 --- a/build/CMakeFiles/projection_test.dir/CXX.includecache +++ b/build/CMakeFiles/projection_test.dir/CXX.includecache @@ -63,8 +63,8 @@ string - Eigen/Core - -tf2/buffer_core.h -../include/laser_geometry/tf2/buffer_core.h +tf3/buffer_core.h +../include/laser_geometry/tf3/buffer_core.h sensor_msgs/LaserScan.h - sensor_msgs/PointCloud2.h diff --git a/build/CMakeFiles/projection_test.dir/link.txt b/build/CMakeFiles/projection_test.dir/link.txt index d43c91a..51c4ba7 100644 --- a/build/CMakeFiles/projection_test.dir/link.txt +++ b/build/CMakeFiles/projection_test.dir/link.txt @@ -1 +1 @@ -/usr/bin/c++ CMakeFiles/projection_test.dir/test/projection_test.cpp.o -o projection_test -Wl,-rpath,/home/duongtd/robotics_core/laser_geometry/build liblaser_geometry.so -lpthread robot_time_build/librobot_time.a -ltf2 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread +/usr/bin/c++ CMakeFiles/projection_test.dir/test/projection_test.cpp.o -o projection_test -Wl,-rpath,/home/duongtd/robotics_core/laser_geometry/build liblaser_geometry.so -lpthread robot_time_build/librobot_time.a -ltf3 /usr/local/lib/libgtest_main.a /usr/local/lib/libgtest.a -lpthread diff --git a/include/laser_geometry/data_convert.h b/include/laser_geometry/data_convert.h deleted file mode 100644 index 0ddb26c..0000000 --- a/include/laser_geometry/data_convert.h +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef DATA_CONVERT_H -#define DATA_CONVERT_H - -#include -#include -#include -#include - -namespace laser_geometry -{ - robot::Time convertTime(tf3::Time time) - { - robot::Time time_tmp; - time_tmp.sec = time.sec; - time_tmp.nsec = time.nsec; - return time_tmp; - } - - tf3::Time convertTime(robot::Time time) - { - tf3::Time time_tmp; - time_tmp.sec = time.sec; - time_tmp.nsec = time.nsec; - return time_tmp; - } - - - tf3::Transform convertToTf2Transform(const geometry_msgs::Transform& msg) - { - tf3::Quaternion q( - msg.rotation.x, - msg.rotation.y, - msg.rotation.z, - msg.rotation.w - ); - - tf3::Vector3 t( - msg.translation.x, - msg.translation.y, - msg.translation.z - ); - - tf3::Transform tf(q, t); - return tf; - } - - tf3::Transform convertToTf2Transform(const tf3::TransformMsg& msg) - { - tf3::Quaternion q( - msg.rotation.x, - msg.rotation.y, - msg.rotation.z, - msg.rotation.w - ); - - tf3::Vector3 t( - msg.translation.x, - msg.translation.y, - msg.translation.z - ); - - tf3::Transform tf(q, t); - return tf; - } - - tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) - { - tf3::TransformStampedMsg out; - out.header.seq = msg.header.seq; - out.header.stamp = convertTime(msg.header.stamp); - out.header.frame_id = msg.header.frame_id; - out.child_frame_id = msg.child_frame_id; - out.transform.translation.x = msg.transform.translation.x; - out.transform.translation.y = msg.transform.translation.y; - out.transform.translation.z = msg.transform.translation.z; - out.transform.rotation.x = msg.transform.rotation.x; - out.transform.rotation.y = msg.transform.rotation.y; - out.transform.rotation.z = msg.transform.rotation.z; - out.transform.rotation.w = msg.transform.rotation.w; - return out; - } - geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) - { - geometry_msgs::TransformStamped out; - out.header.seq = msg.header.seq; - out.header.stamp = convertTime(msg.header.stamp); - out.header.frame_id = msg.header.frame_id; - out.child_frame_id = msg.child_frame_id; - out.transform.translation.x = msg.transform.translation.x; - out.transform.translation.y = msg.transform.translation.y; - out.transform.translation.z = msg.transform.translation.z; - out.transform.rotation.x = msg.transform.rotation.x; - out.transform.rotation.y = msg.transform.rotation.y; - out.transform.rotation.z = msg.transform.rotation.z; - out.transform.rotation.w = msg.transform.rotation.w; - return out; - } -} - -#endif // DATA_CONVERT_H \ No newline at end of file diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 7cae956..2fb9aff 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -29,7 +29,7 @@ // POSSIBILITY OF SUCH DAMAGE. #include "laser_geometry/laser_geometry.hpp" -#include "laser_geometry/data_convert.h" +#include "data_convert/data_convert.h" #include @@ -436,8 +436,8 @@ void LaserProjection::transformLaserScanToPointCloud_( end_time = start_time + duration; } - tf3::Time st = convertTime(start_time); - tf3::Time e = convertTime(end_time); + tf3::Time st = data_convert::convertTime(start_time); + tf3::Time e = data_convert::convertTime(end_time); tf3::TransformStampedMsg start_transform_msg = tf.lookupTransform( target_frame, scan_in.header.frame_id, st); @@ -445,8 +445,8 @@ void LaserProjection::transformLaserScanToPointCloud_( tf3::TransformStampedMsg end_transform_msg = tf.lookupTransform( target_frame, scan_in.header.frame_id, e); - geometry_msgs::TransformStamped start_transform = convertToTransformStamped(start_transform_msg); - geometry_msgs::TransformStamped end_transform = convertToTransformStamped(end_transform_msg); + geometry_msgs::TransformStamped start_transform = data_convert::convertToTransformStamped(start_transform_msg); + geometry_msgs::TransformStamped end_transform = data_convert::convertToTransformStamped(end_transform_msg); tf3::Quaternion quat_start(start_transform.transform.rotation.x, start_transform.transform.rotation.y,