DuongTD/laser_geometry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update to tf3

Browse Source
This commit is contained in:
duongtd 2025-11-17 15:03:25 +07:00
parent 624f24fdd4
commit 5e9993268f
5 changed files with 64 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
CMakeLists.txt
View File

@ -11,17 +11,17 @@ find_package(Eigen3 REQUIRED)
find_package(GTest REQUIRED) find_package(GTest REQUIRED)
# Add subdirectories for message dependencies # Add subdirectories for message dependencies
if (NOT TARGET sensor_msgs) # if (NOT TARGET sensor_msgs)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build) # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build)
endif() # endif()
if (NOT TARGET geometry_msgs) # if (NOT TARGET geometry_msgs)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build) # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build)
endif() # endif()
if (NOT TARGET robot_time) # if (NOT TARGET robot_time)
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../robot_time ${CMAKE_BINARY_DIR}/robot_time_build) # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../robot_time ${CMAKE_BINARY_DIR}/robot_time_build)
endif() # endif()
# Include directories # Include directories
include_directories( include_directories(
@ -43,7 +43,7 @@ target_link_libraries(laser_geometry PUBLIC
sensor_msgs sensor_msgs
geometry_msgs geometry_msgs
robot_time robot_time
tf2 tf3
) )
if(TARGET Eigen3::Eigen) if(TARGET Eigen3::Eigen)
@ -59,7 +59,7 @@ target_compile_definitions(laser_geometry PRIVATE "LASER_GEOMETRY_BUILDING_LIBRA
# Install targets # Install targets
install( install(
TARGETS laser_geometry TARGETS laser_geometry
EXPORT laser_geometry # EXPORT laser_geometry
ARCHIVE DESTINATION lib ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib LIBRARY DESTINATION lib
RUNTIME DESTINATION bin RUNTIME DESTINATION bin

32
include/laser_geometry/data_convert.h
View File

@ -2,13 +2,13 @@
#define DATA_CONVERT_H #define DATA_CONVERT_H
#include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/TransformStamped.h>
#include <tf2/utils.h> #include <tf3/utils.h>
#include <tf2/compat.h> #include <tf3/compat.h>
#include <robot/time.h> #include <robot/time.h>
namespace laser_geometry namespace laser_geometry
{ {
robot::Time convertTime(tf2::Time time) robot::Time convertTime(tf3::Time time)
{ {
robot::Time time_tmp; robot::Time time_tmp;
time_tmp.sec = time.sec; time_tmp.sec = time.sec;
@ -16,56 +16,56 @@ namespace laser_geometry
return time_tmp; 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.sec = time.sec;
time_tmp.nsec = time.nsec; time_tmp.nsec = time.nsec;
return time_tmp; 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.x,
msg.rotation.y, msg.rotation.y,
msg.rotation.z, msg.rotation.z,
msg.rotation.w msg.rotation.w
); );
tf2::Vector3 t( tf3::Vector3 t(
msg.translation.x, msg.translation.x,
msg.translation.y, msg.translation.y,
msg.translation.z msg.translation.z
); );
tf2::Transform tf(q, t); tf3::Transform tf(q, t);
return tf; 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.x,
msg.rotation.y, msg.rotation.y,
msg.rotation.z, msg.rotation.z,
msg.rotation.w msg.rotation.w
); );
tf2::Vector3 t( tf3::Vector3 t(
msg.translation.x, msg.translation.x,
msg.translation.y, msg.translation.y,
msg.translation.z msg.translation.z
); );
tf2::Transform tf(q, t); tf3::Transform tf(q, t);
return tf; 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.seq = msg.header.seq;
out.header.stamp = convertTime(msg.header.stamp); out.header.stamp = convertTime(msg.header.stamp);
out.header.frame_id = msg.header.frame_id; out.header.frame_id = msg.header.frame_id;
@ -79,7 +79,7 @@ namespace laser_geometry
out.transform.rotation.w = msg.transform.rotation.w; out.transform.rotation.w = msg.transform.rotation.w;
return out; return out;
} }
geometry_msgs::TransformStamped convertToTransformStamped(const tf2::TransformStampedMsg& msg) geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
{ {
geometry_msgs::TransformStamped out; geometry_msgs::TransformStamped out;
out.header.seq = msg.header.seq; out.header.seq = msg.header.seq;

16
include/laser_geometry/laser_geometry.hpp
View File

@ -39,7 +39,7 @@
#include <Eigen/Core> // NOLINT (cpplint cannot handle include order here) #include <Eigen/Core> // NOLINT (cpplint cannot handle include order here)
#include "tf2/buffer_core.h" #include "tf3/buffer_core.h"
#include <sensor_msgs/LaserScan.h> #include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/TransformStamped.h>
@ -142,7 +142,7 @@ public:
* \param target_frame The frame of the resulting point cloud * \param target_frame The frame of the resulting point cloud
* \param scan_in The input laser scan * \param scan_in The input laser scan
* \param cloud_out The output point cloud * \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 * transform
* \param range_cutoff An additional range cutoff which can be * \param range_cutoff An additional range cutoff which can be
* applied to discard everything above it. * applied to discard everything above it.
@ -157,7 +157,7 @@ public:
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in, const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out, sensor_msgs::PointCloud2 & cloud_out,
tf2::BufferCore & tf, tf3::BufferCore & tf,
double range_cutoff = -1.0, double range_cutoff = -1.0,
int channel_options = channel_option::Default) int channel_options = channel_option::Default)
{ {
@ -178,7 +178,7 @@ private:
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in, const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out, sensor_msgs::PointCloud2 & cloud_out,
tf2::BufferCore & tf, tf3::BufferCore & tf,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);
@ -187,10 +187,10 @@ private:
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in, const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out, sensor_msgs::PointCloud2 & cloud_out,
tf2::Quaternion quat_start, tf3::Quaternion quat_start,
tf2::Vector3 origin_start, tf3::Vector3 origin_start,
tf2::Quaternion quat_end, tf3::Quaternion quat_end,
tf2::Vector3 origin_end, tf3::Vector3 origin_end,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);

42
src/laser_geometry.cpp
View File

@ -41,9 +41,9 @@
#include <sensor_msgs/LaserScan.h> #include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <tf2/buffer_core.h> #include <tf3/buffer_core.h>
#include<tf2/convert.h> #include<tf3/convert.h>
#include "tf2/LinearMath/Transform.h" #include "tf3/LinearMath/Transform.h"
namespace laser_geometry namespace laser_geometry
{ {
@ -271,10 +271,10 @@ void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in, const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out, sensor_msgs::PointCloud2 & cloud_out,
tf2::Quaternion quat_start, tf3::Quaternion quat_start,
tf2::Vector3 origin_start, tf3::Vector3 origin_start,
tf2::Quaternion quat_end, tf3::Quaternion quat_end,
tf2::Vector3 origin_end, tf3::Vector3 origin_end,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
{ {
@ -318,7 +318,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
cloud_out.header.frame_id = target_frame; cloud_out.header.frame_id = target_frame;
tf2::Transform cur_transform; tf3::Transform cur_transform;
double ranges_norm = 1 / (static_cast<double>(scan_in.ranges.size()) - 1.0); double ranges_norm = 1 / (static_cast<double>(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 // TODO(anon): Make a function that performs both the slerp and linear interpolation needed to
// interpolate a Full Transform (Quaternion + Vector) // interpolate a Full Transform (Quaternion + Vector)
// Interpolate translation // Interpolate translation
tf2::Vector3 v(0, 0, 0); tf3::Vector3 v(0, 0, 0);
v.setInterpolate3(origin_start, origin_end, ratio); v.setInterpolate3(origin_start, origin_end, ratio);
cur_transform.setOrigin(v); cur_transform.setOrigin(v);
// Compute the slerp-ed rotation // Compute the slerp-ed rotation
cur_transform.setRotation(slerp(quat_start, quat_end, ratio)); cur_transform.setRotation(slerp(quat_start, quat_end, ratio));
tf2::Vector3 point_in(pstep[0], pstep[1], pstep[2]); tf3::Vector3 point_in(pstep[0], pstep[1], pstep[2]);
tf2::Vector3 point_out = cur_transform * point_in; tf3::Vector3 point_out = cur_transform * point_in;
// Copy transformed point into cloud // Copy transformed point into cloud
pstep[0] = static_cast<float>(point_out.x()); pstep[0] = static_cast<float>(point_out.x());
@ -356,7 +356,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
if (has_viewpoint) { if (has_viewpoint) {
auto vpstep = auto vpstep =
reinterpret_cast<float *>(&cloud_out.data[i * cloud_out.point_step + vp_x_offset]); reinterpret_cast<float *>(&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; point_out = cur_transform * point_in;
// Copy transformed point into cloud // Copy transformed point into cloud
@ -421,7 +421,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in, const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out, sensor_msgs::PointCloud2 & cloud_out,
tf2::BufferCore & tf, tf3::BufferCore & tf,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
{ {
@ -436,31 +436,31 @@ void LaserProjection::transformLaserScanToPointCloud_(
end_time = start_time + duration; end_time = start_time + duration;
} }
tf2::Time st = convertTime(start_time); tf3::Time st = convertTime(start_time);
tf2::Time e = convertTime(end_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); 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); target_frame, scan_in.header.frame_id, e);
geometry_msgs::TransformStamped start_transform = convertToTransformStamped(start_transform_msg); geometry_msgs::TransformStamped start_transform = convertToTransformStamped(start_transform_msg);
geometry_msgs::TransformStamped end_transform = convertToTransformStamped(end_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.y,
start_transform.transform.rotation.z, start_transform.transform.rotation.z,
start_transform.transform.rotation.w); 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.y,
end_transform.transform.rotation.z, end_transform.transform.rotation.z,
end_transform.transform.rotation.w); 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.y,
start_transform.transform.translation.z); 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.y,
end_transform.transform.translation.z); end_transform.transform.translation.z);
transformLaserScanToPointCloud_( transformLaserScanToPointCloud_(

16
test/projection_test.cpp
View File

@ -256,7 +256,7 @@ TEST(laser_geometry, projectLaser2) {
// Needs to publish a transform to "laser_frame" in order to work. // Needs to publish a transform to "laser_frame" in order to work.
#if 0 #if 0
TEST(laser_geometry, transformLaserScanToPointCloud2) { TEST(laser_geometry, transformLaserScanToPointCloud2) {
tf2::BufferCore tf2; tf3::BufferCore tf3;
double tolerance = 1e-12; double tolerance = 1e-12;
laser_geometry::LaserProjection projector; laser_geometry::LaserProjection projector;
@ -325,34 +325,34 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
sensor_msgs::PointCloud2 cloud_out; sensor_msgs::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud( 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); laser_geometry::channel_option::None);
EXPECT_EQ(cloud_out.fields.size(), 3u); EXPECT_EQ(cloud_out.fields.size(), 3u);
projector.transformLaserScanToPointCloud( 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); laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), 4u); EXPECT_EQ(cloud_out.fields.size(), 4u);
projector.transformLaserScanToPointCloud( 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::Intensity);
EXPECT_EQ(cloud_out.fields.size(), 4u); 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); EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.transformLaserScanToPointCloud( 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::Intensity |
laser_geometry::channel_option::Index); laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), 5u); EXPECT_EQ(cloud_out.fields.size(), 5u);
projector.transformLaserScanToPointCloud( 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::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance); laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.fields.size(), 6u); EXPECT_EQ(cloud_out.fields.size(), 6u);
projector.transformLaserScanToPointCloud( 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::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance | laser_geometry::channel_option::Distance |
laser_geometry::channel_option::Timestamp); laser_geometry::channel_option::Timestamp);