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

first commit

Browse Source
This commit is contained in:
duongtd
2025-11-17 10:43:24 +07:00
parent ce9ce90379
commit 624f24fdd4
249 changed files with 13400 additions and 572 deletions
Download Patch File Download Diff File Expand all files Collapse all files

100
include/laser_geometry/data_convert.h Normal file
View File

@@ -0,0 +1,100 @@
#ifndef DATA_CONVERT_H
#define DATA_CONVERT_H
#include <geometry_msgs/TransformStamped.h>
#include <tf2/utils.h>
#include <tf2/compat.h>
#include <robot/time.h>
namespace laser_geometry
{
robot::Time convertTime(tf2::Time time)
{
robot::Time time_tmp;
time_tmp.sec = time.sec;
time_tmp.nsec = time.nsec;
return time_tmp;
}
tf2::Time convertTime(robot::Time time)
{
tf2::Time time_tmp;
time_tmp.sec = time.sec;
time_tmp.nsec = time.nsec;
return time_tmp;
}
tf2::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
{
tf2::Quaternion q(
msg.rotation.x,
msg.rotation.y,
msg.rotation.z,
msg.rotation.w
);
tf2::Vector3 t(
msg.translation.x,
msg.translation.y,
msg.translation.z
);
tf2::Transform tf(q, t);
return tf;
}
tf2::Transform convertToTf2Transform(const tf2::TransformMsg& msg)
{
tf2::Quaternion q(
msg.rotation.x,
msg.rotation.y,
msg.rotation.z,
msg.rotation.w
);
tf2::Vector3 t(
msg.translation.x,
msg.translation.y,
msg.translation.z
);
tf2::Transform tf(q, t);
return tf;
}
tf2::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
{
tf2::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 tf2::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

31
include/laser_geometry/laser_geometry.hpp
View File

@@ -39,9 +39,10 @@
#include <Eigen/Core> // NOLINT (cpplint cannot handle include order here)
#include "tf2/buffer_core.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "tf2/buffer_core.h"
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include "laser_geometry/visibility_control.hpp"
namespace laser_geometry
@@ -103,7 +104,7 @@ public:
LaserProjection()
: angle_min_(0), angle_max_(0) {}
// Project a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2
// Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2
/*!
* Project a single laser scan from a linear array into a 3D
* point cloud. The generated cloud will be in the same frame
@@ -121,15 +122,15 @@ public:
*/
LASER_GEOMETRY_PUBLIC
void projectLaser(
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
double range_cutoff = -1.0,
int channel_options = channel_option::Default)
{
projectLaser_(scan_in, cloud_out, range_cutoff, channel_options);
}
// Transform a sensor_msgs::msg::LaserScan into a sensor_msgs::msg::PointCloud2 in target frame
// Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame
/*!
* Transform a single laser scan from a linear array into a 3D
* point cloud, accounting for movement of the laser over the
@@ -154,8 +155,8 @@ public:
LASER_GEOMETRY_PUBLIC
void transformLaserScanToPointCloud(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
tf2::BufferCore & tf,
double range_cutoff = -1.0,
int channel_options = channel_option::Default)
@@ -167,16 +168,16 @@ public:
private:
// Internal hidden representation of projectLaser
void projectLaser_(
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
double range_cutoff,
int channel_options);
// Internal hidden representation of transformLaserScanToPointCloud2
void transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
tf2::BufferCore & tf,
double range_cutoff,
int channel_options);
@@ -184,8 +185,8 @@ private:
// Function used by the several forms of transformLaserScanToPointCloud_
void transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::msg::LaserScan & scan_in,
sensor_msgs::msg::PointCloud2 & cloud_out,
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
tf2::Quaternion quat_start,
tf2::Vector3 origin_start,
tf2::Quaternion quat_end,