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)
# 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

32
include/laser_geometry/data_convert.h
View File

@ -2,13 +2,13 @@
#define DATA_CONVERT_H
#include <geometry_msgs/TransformStamped.h>
#include <tf2/utils.h>
#include <tf2/compat.h>
#include <tf3/utils.h>
#include <tf3/compat.h>
#include <robot/time.h>
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;

16
include/laser_geometry/laser_geometry.hpp
View File

@ -39,7 +39,7 @@
#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/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
@ -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);

42
src/laser_geometry.cpp
View File

@ -41,9 +41,9 @@
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2/buffer_core.h>
#include<tf2/convert.h>
#include "tf2/LinearMath/Transform.h"
#include <tf3/buffer_core.h>
#include<tf3/convert.h>
#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<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
// 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<float>(point_out.x());
@ -356,7 +356,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
if (has_viewpoint) {
auto vpstep =
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;
// 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_(

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.
#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);