From 5e9993268f0396f2926bce7507041ae3828513db Mon Sep 17 00:00:00 2001 From: duongtd Date: Mon, 17 Nov 2025 15:03:25 +0700 Subject: [PATCH] update to tf3 --- CMakeLists.txt | 22 ++++++------ include/laser_geometry/data_convert.h | 32 ++++++++--------- include/laser_geometry/laser_geometry.hpp | 16 ++++----- src/laser_geometry.cpp | 42 +++++++++++------------ test/projection_test.cpp | 16 ++++----- 5 files changed, 64 insertions(+), 64 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5badc62..0e9b55e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,17 +11,17 @@ find_package(Eigen3 REQUIRED) find_package(GTest REQUIRED) # Add subdirectories for message dependencies -if (NOT TARGET sensor_msgs) - add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build) -endif() +# if (NOT TARGET sensor_msgs) +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build) +# endif() -if (NOT TARGET geometry_msgs) - add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build) -endif() +# if (NOT TARGET geometry_msgs) +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build) +# endif() -if (NOT TARGET robot_time) - add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../robot_time ${CMAKE_BINARY_DIR}/robot_time_build) -endif() +# if (NOT TARGET robot_time) +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../robot_time ${CMAKE_BINARY_DIR}/robot_time_build) +# endif() # Include directories include_directories( @@ -43,7 +43,7 @@ target_link_libraries(laser_geometry PUBLIC sensor_msgs geometry_msgs robot_time - tf2 + tf3 ) if(TARGET Eigen3::Eigen) @@ -59,7 +59,7 @@ target_compile_definitions(laser_geometry PRIVATE "LASER_GEOMETRY_BUILDING_LIBRA # Install targets install( TARGETS laser_geometry - EXPORT laser_geometry + # EXPORT laser_geometry ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/include/laser_geometry/data_convert.h b/include/laser_geometry/data_convert.h index 7370797..0ddb26c 100644 --- a/include/laser_geometry/data_convert.h +++ b/include/laser_geometry/data_convert.h @@ -2,13 +2,13 @@ #define DATA_CONVERT_H #include -#include -#include +#include +#include #include namespace laser_geometry { - robot::Time convertTime(tf2::Time time) + robot::Time convertTime(tf3::Time time) { robot::Time time_tmp; time_tmp.sec = time.sec; @@ -16,56 +16,56 @@ namespace laser_geometry return time_tmp; } - tf2::Time convertTime(robot::Time time) + tf3::Time convertTime(robot::Time time) { - tf2::Time time_tmp; + tf3::Time time_tmp; time_tmp.sec = time.sec; time_tmp.nsec = time.nsec; return time_tmp; } - tf2::Transform convertToTf2Transform(const geometry_msgs::Transform& msg) + tf3::Transform convertToTf2Transform(const geometry_msgs::Transform& msg) { - tf2::Quaternion q( + tf3::Quaternion q( msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w ); - tf2::Vector3 t( + tf3::Vector3 t( msg.translation.x, msg.translation.y, msg.translation.z ); - tf2::Transform tf(q, t); + tf3::Transform tf(q, t); return tf; } - tf2::Transform convertToTf2Transform(const tf2::TransformMsg& msg) + tf3::Transform convertToTf2Transform(const tf3::TransformMsg& msg) { - tf2::Quaternion q( + tf3::Quaternion q( msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w ); - tf2::Vector3 t( + tf3::Vector3 t( msg.translation.x, msg.translation.y, msg.translation.z ); - tf2::Transform tf(q, t); + tf3::Transform tf(q, t); return tf; } - tf2::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) + tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) { - tf2::TransformStampedMsg out; + tf3::TransformStampedMsg out; out.header.seq = msg.header.seq; out.header.stamp = convertTime(msg.header.stamp); out.header.frame_id = msg.header.frame_id; @@ -79,7 +79,7 @@ namespace laser_geometry out.transform.rotation.w = msg.transform.rotation.w; return out; } - geometry_msgs::TransformStamped convertToTransformStamped(const tf2::TransformStampedMsg& msg) + geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) { geometry_msgs::TransformStamped out; out.header.seq = msg.header.seq; diff --git a/include/laser_geometry/laser_geometry.hpp b/include/laser_geometry/laser_geometry.hpp index 8bee1c0..21c73ca 100644 --- a/include/laser_geometry/laser_geometry.hpp +++ b/include/laser_geometry/laser_geometry.hpp @@ -39,7 +39,7 @@ #include // NOLINT (cpplint cannot handle include order here) -#include "tf2/buffer_core.h" +#include "tf3/buffer_core.h" #include #include #include @@ -142,7 +142,7 @@ public: * \param target_frame The frame of the resulting point cloud * \param scan_in The input laser scan * \param cloud_out The output point cloud - * \param tf a tf2::BufferCore object to use to perform the + * \param tf a tf3::BufferCore object to use to perform the * transform * \param range_cutoff An additional range cutoff which can be * applied to discard everything above it. @@ -157,7 +157,7 @@ public: const std::string & target_frame, const sensor_msgs::LaserScan & scan_in, sensor_msgs::PointCloud2 & cloud_out, - tf2::BufferCore & tf, + tf3::BufferCore & tf, double range_cutoff = -1.0, int channel_options = channel_option::Default) { @@ -178,7 +178,7 @@ private: const std::string & target_frame, const sensor_msgs::LaserScan & scan_in, sensor_msgs::PointCloud2 & cloud_out, - tf2::BufferCore & tf, + tf3::BufferCore & tf, double range_cutoff, int channel_options); @@ -187,10 +187,10 @@ private: const std::string & target_frame, const sensor_msgs::LaserScan & scan_in, sensor_msgs::PointCloud2 & cloud_out, - tf2::Quaternion quat_start, - tf2::Vector3 origin_start, - tf2::Quaternion quat_end, - tf2::Vector3 origin_end, + tf3::Quaternion quat_start, + tf3::Vector3 origin_start, + tf3::Quaternion quat_end, + tf3::Vector3 origin_end, double range_cutoff, int channel_options); diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index b7756b8..7cae956 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -41,9 +41,9 @@ #include #include -#include -#include -#include "tf2/LinearMath/Transform.h" +#include +#include +#include "tf3/LinearMath/Transform.h" namespace laser_geometry { @@ -271,10 +271,10 @@ void LaserProjection::transformLaserScanToPointCloud_( const std::string & target_frame, const sensor_msgs::LaserScan & scan_in, sensor_msgs::PointCloud2 & cloud_out, - tf2::Quaternion quat_start, - tf2::Vector3 origin_start, - tf2::Quaternion quat_end, - tf2::Vector3 origin_end, + tf3::Quaternion quat_start, + tf3::Vector3 origin_start, + tf3::Quaternion quat_end, + tf3::Vector3 origin_end, double range_cutoff, int channel_options) { @@ -318,7 +318,7 @@ void LaserProjection::transformLaserScanToPointCloud_( cloud_out.header.frame_id = target_frame; - tf2::Transform cur_transform; + tf3::Transform cur_transform; double ranges_norm = 1 / (static_cast(scan_in.ranges.size()) - 1.0); @@ -337,15 +337,15 @@ void LaserProjection::transformLaserScanToPointCloud_( // TODO(anon): Make a function that performs both the slerp and linear interpolation needed to // interpolate a Full Transform (Quaternion + Vector) // Interpolate translation - tf2::Vector3 v(0, 0, 0); + tf3::Vector3 v(0, 0, 0); v.setInterpolate3(origin_start, origin_end, ratio); cur_transform.setOrigin(v); // Compute the slerp-ed rotation cur_transform.setRotation(slerp(quat_start, quat_end, ratio)); - tf2::Vector3 point_in(pstep[0], pstep[1], pstep[2]); - tf2::Vector3 point_out = cur_transform * point_in; + tf3::Vector3 point_in(pstep[0], pstep[1], pstep[2]); + tf3::Vector3 point_out = cur_transform * point_in; // Copy transformed point into cloud pstep[0] = static_cast(point_out.x()); @@ -356,7 +356,7 @@ void LaserProjection::transformLaserScanToPointCloud_( if (has_viewpoint) { auto vpstep = reinterpret_cast(&cloud_out.data[i * cloud_out.point_step + vp_x_offset]); - point_in = tf2::Vector3(vpstep[0], vpstep[1], vpstep[2]); + point_in = tf3::Vector3(vpstep[0], vpstep[1], vpstep[2]); point_out = cur_transform * point_in; // Copy transformed point into cloud @@ -421,7 +421,7 @@ void LaserProjection::transformLaserScanToPointCloud_( const std::string & target_frame, const sensor_msgs::LaserScan & scan_in, sensor_msgs::PointCloud2 & cloud_out, - tf2::BufferCore & tf, + tf3::BufferCore & tf, double range_cutoff, int channel_options) { @@ -436,31 +436,31 @@ void LaserProjection::transformLaserScanToPointCloud_( end_time = start_time + duration; } - tf2::Time st = convertTime(start_time); - tf2::Time e = convertTime(end_time); + tf3::Time st = convertTime(start_time); + tf3::Time e = convertTime(end_time); - tf2::TransformStampedMsg start_transform_msg = tf.lookupTransform( + tf3::TransformStampedMsg start_transform_msg = tf.lookupTransform( target_frame, scan_in.header.frame_id, st); - tf2::TransformStampedMsg end_transform_msg = tf.lookupTransform( + 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); - tf2::Quaternion quat_start(start_transform.transform.rotation.x, + tf3::Quaternion quat_start(start_transform.transform.rotation.x, start_transform.transform.rotation.y, start_transform.transform.rotation.z, start_transform.transform.rotation.w); - tf2::Quaternion quat_end(end_transform.transform.rotation.x, + tf3::Quaternion quat_end(end_transform.transform.rotation.x, end_transform.transform.rotation.y, end_transform.transform.rotation.z, end_transform.transform.rotation.w); - tf2::Vector3 origin_start(start_transform.transform.translation.x, + tf3::Vector3 origin_start(start_transform.transform.translation.x, start_transform.transform.translation.y, start_transform.transform.translation.z); - tf2::Vector3 origin_end(end_transform.transform.translation.x, + tf3::Vector3 origin_end(end_transform.transform.translation.x, end_transform.transform.translation.y, end_transform.transform.translation.z); transformLaserScanToPointCloud_( diff --git a/test/projection_test.cpp b/test/projection_test.cpp index af7a717..cba025a 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -256,7 +256,7 @@ TEST(laser_geometry, projectLaser2) { // Needs to publish a transform to "laser_frame" in order to work. #if 0 TEST(laser_geometry, transformLaserScanToPointCloud2) { - tf2::BufferCore tf2; + tf3::BufferCore tf3; double tolerance = 1e-12; laser_geometry::LaserProjection projector; @@ -325,34 +325,34 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) { sensor_msgs::PointCloud2 cloud_out; projector.transformLaserScanToPointCloud( - scan.header.frame_id, scan, cloud_out, tf2, -1.0, + scan.header.frame_id, scan, cloud_out, tf3, -1.0, laser_geometry::channel_option::None); EXPECT_EQ(cloud_out.fields.size(), 3u); projector.transformLaserScanToPointCloud( - scan.header.frame_id, scan, cloud_out, tf2, -1.0, + scan.header.frame_id, scan, cloud_out, tf3, -1.0, laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.fields.size(), 4u); projector.transformLaserScanToPointCloud( - scan.header.frame_id, scan, cloud_out, tf2, -1.0, + scan.header.frame_id, scan, cloud_out, tf3, -1.0, laser_geometry::channel_option::Intensity); EXPECT_EQ(cloud_out.fields.size(), 4u); - projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2); + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf3); EXPECT_EQ(cloud_out.fields.size(), 5u); projector.transformLaserScanToPointCloud( - scan.header.frame_id, scan, cloud_out, tf2, -1.0, + scan.header.frame_id, scan, cloud_out, tf3, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); EXPECT_EQ(cloud_out.fields.size(), 5u); projector.transformLaserScanToPointCloud( - scan.header.frame_id, scan, cloud_out, tf2, -1.0, + scan.header.frame_id, scan, cloud_out, tf3, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance); EXPECT_EQ(cloud_out.fields.size(), 6u); projector.transformLaserScanToPointCloud( - scan.header.frame_id, scan, cloud_out, tf2, -1.0, + scan.header.frame_id, scan, cloud_out, tf3, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);