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

hiep sua ten file

Browse Source
This commit is contained in:
HiepLM 2025-12-30 09:57:01 +07:00
parent a183d4bb7b
commit 1fefb2a389
5 changed files with 48 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -27,7 +27,7 @@ target_include_directories(laser_geometry
) )
target_link_libraries(laser_geometry PUBLIC target_link_libraries(laser_geometry PUBLIC
sensor_msgs robot_sensor_msgs
geometry_msgs geometry_msgs
robot_time robot_time
tf3 tf3

28
include/laser_geometry/laser_geometry.hpp
View File

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

38
src/laser_geometry.cpp
View File

@ -38,8 +38,8 @@
#include <robot/time.h> #include <robot/time.h>
#include <robot/duration.h> #include <robot/duration.h>
#include <sensor_msgs/LaserScan.h> #include <robot_sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
#include <tf3/buffer_core.h> #include <tf3/buffer_core.h>
#include<tf3/convert.h> #include<tf3/convert.h>
@ -48,8 +48,8 @@
namespace laser_geometry namespace laser_geometry
{ {
void LaserProjection::projectLaser_( void LaserProjection::projectLaser_(
const sensor_msgs::LaserScan & scan_in, const robot_sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out, robot_sensor_msgs::PointCloud2 & cloud_out,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
{ {
@ -89,15 +89,15 @@ void LaserProjection::projectLaser_(
cloud_out.fields.resize(3); cloud_out.fields.resize(3);
cloud_out.fields[0].name = "x"; cloud_out.fields[0].name = "x";
cloud_out.fields[0].offset = 0; cloud_out.fields[0].offset = 0;
cloud_out.fields[0].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[0].datatype = robot_sensor_msgs::PointField::FLOAT32;
cloud_out.fields[0].count = 1; cloud_out.fields[0].count = 1;
cloud_out.fields[1].name = "y"; cloud_out.fields[1].name = "y";
cloud_out.fields[1].offset = 4; cloud_out.fields[1].offset = 4;
cloud_out.fields[1].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[1].datatype = robot_sensor_msgs::PointField::FLOAT32;
cloud_out.fields[1].count = 1; cloud_out.fields[1].count = 1;
cloud_out.fields[2].name = "z"; cloud_out.fields[2].name = "z";
cloud_out.fields[2].offset = 8; cloud_out.fields[2].offset = 8;
cloud_out.fields[2].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[2].datatype = robot_sensor_msgs::PointField::FLOAT32;
cloud_out.fields[2].count = 1; cloud_out.fields[2].count = 1;
// Define 4 indices in the channel array for each possible value type // Define 4 indices in the channel array for each possible value type
@ -110,7 +110,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size(); size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "intensity"; cloud_out.fields[field_size].name = "intensity";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
@ -121,7 +121,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size(); size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "index"; cloud_out.fields[field_size].name = "index";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::INT32; cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::INT32;
cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
@ -132,7 +132,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size(); size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "distances"; cloud_out.fields[field_size].name = "distances";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
@ -143,7 +143,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size(); size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1); cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "stamps"; cloud_out.fields[field_size].name = "stamps";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
@ -155,19 +155,19 @@ void LaserProjection::projectLaser_(
cloud_out.fields.resize(field_size + 3); cloud_out.fields.resize(field_size + 3);
cloud_out.fields[field_size].name = "vp_x"; cloud_out.fields[field_size].name = "vp_x";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size].datatype = robot_sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset; cloud_out.fields[field_size].offset = offset;
cloud_out.fields[field_size].count = 1; cloud_out.fields[field_size].count = 1;
offset += 4; offset += 4;
cloud_out.fields[field_size + 1].name = "vp_y"; cloud_out.fields[field_size + 1].name = "vp_y";
cloud_out.fields[field_size + 1].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size + 1].datatype = robot_sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size + 1].offset = offset; cloud_out.fields[field_size + 1].offset = offset;
cloud_out.fields[field_size + 1].count = 1; cloud_out.fields[field_size + 1].count = 1;
offset += 4; offset += 4;
cloud_out.fields[field_size + 2].name = "vp_z"; cloud_out.fields[field_size + 2].name = "vp_z";
cloud_out.fields[field_size + 2].datatype = sensor_msgs::PointField::FLOAT32; cloud_out.fields[field_size + 2].datatype = robot_sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size + 2].offset = offset; cloud_out.fields[field_size + 2].offset = offset;
cloud_out.fields[field_size + 2].count = 1; cloud_out.fields[field_size + 2].count = 1;
offset += 4; offset += 4;
@ -269,8 +269,8 @@ void LaserProjection::projectLaser_(
void LaserProjection::transformLaserScanToPointCloud_( void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in, const robot_sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out, robot_sensor_msgs::PointCloud2 & cloud_out,
tf3::Quaternion quat_start, tf3::Quaternion quat_start,
tf3::Vector3 origin_start, tf3::Vector3 origin_start,
tf3::Quaternion quat_end, tf3::Quaternion quat_end,
@ -368,7 +368,7 @@ void LaserProjection::transformLaserScanToPointCloud_(
// if the user didn't request the index field, then we need to copy the PointCloud and drop it // if the user didn't request the index field, then we need to copy the PointCloud and drop it
if (!requested_index) { if (!requested_index) {
sensor_msgs::PointCloud2 cloud_without_index; robot_sensor_msgs::PointCloud2 cloud_without_index;
// copy basic meta data // copy basic meta data
cloud_without_index.header = cloud_out.header; cloud_without_index.header = cloud_out.header;
@ -419,8 +419,8 @@ void LaserProjection::transformLaserScanToPointCloud_(
void LaserProjection::transformLaserScanToPointCloud_( void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame, const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in, const robot_sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out, robot_sensor_msgs::PointCloud2 & cloud_out,
tf3::BufferCore & tf, tf3::BufferCore & tf,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)

6
src/laser_geometry/laser_geometry.py
View File

@ -30,8 +30,8 @@
import numpy as np import numpy as np
import rclpy import rclpy
import rclpy.logging import rclpy.logging
from sensor_msgs.msg import PointCloud2 from robot_sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2 import robot_sensor_msgs_py.point_cloud2 as pc2
class LaserProjection: class LaserProjection:
@ -86,7 +86,7 @@ class LaserProjection:
self, scan_in, self, scan_in,
range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT): range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT):
""" """
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2. Project a robot_sensor_msgs::LaserScan into a robot_sensor_msgs::PointCloud2.
Project a single laser scan from a linear array into a 3D Project a single laser scan from a linear array into a 3D
point cloud. The generated cloud will be in the same frame point cloud. The generated cloud will be in the same frame

22
test/projection_test.cpp
View File

@ -37,8 +37,8 @@
#include <robot/duration.h> #include <robot/duration.h>
#include "laser_geometry/laser_geometry.hpp" #include "laser_geometry/laser_geometry.hpp"
#include <sensor_msgs/PointCloud2.h> #include <robot_sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h> #include <robot_sensor_msgs/LaserScan.h>
#define PROJECTION_TEST_RANGE_MIN (0.23f) #define PROJECTION_TEST_RANGE_MIN (0.23f)
#define PROJECTION_TEST_RANGE_MAX (40.0f) #define PROJECTION_TEST_RANGE_MAX (40.0f)
@ -70,13 +70,13 @@ struct ScanOptions
scan_time_(scan_time) {} scan_time_(scan_time) {}
}; };
sensor_msgs::LaserScan build_constant_scan(const ScanOptions & options) robot_sensor_msgs::LaserScan build_constant_scan(const ScanOptions & options)
{ {
if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) { if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) {
throw (BuildScanException()); throw (BuildScanException());
} }
sensor_msgs::LaserScan scan; robot_sensor_msgs::LaserScan scan;
robot::Time now = robot::Time::now(); robot::Time now = robot::Time::now();
scan.header.stamp.sec = now.sec; scan.header.stamp.sec = now.sec;
scan.header.stamp.nsec = now.nsec; scan.header.stamp.nsec = now.nsec;
@ -100,7 +100,7 @@ sensor_msgs::LaserScan build_constant_scan(const ScanOptions & options)
} }
template<typename T> template<typename T>
T cloudData(sensor_msgs::PointCloud2 cloud_out, uint32_t index) T cloudData(robot_sensor_msgs::PointCloud2 cloud_out, uint32_t index)
{ {
return *reinterpret_cast<T *>(&cloud_out.data[index]); return *reinterpret_cast<T *>(&cloud_out.data[index]);
} }
@ -157,9 +157,9 @@ TEST(laser_geometry, projectLaser2) {
try { try {
// printf("%f %f %f %f %f %f\n", // printf("%f %f %f %f %f %f\n",
// range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
sensor_msgs::LaserScan scan = build_constant_scan(option); robot_sensor_msgs::LaserScan scan = build_constant_scan(option);
sensor_msgs::PointCloud2 cloud_out; robot_sensor_msgs::PointCloud2 cloud_out;
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), 4u); EXPECT_EQ(cloud_out.fields.size(), 4u);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity); projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
@ -205,7 +205,7 @@ TEST(laser_geometry, projectLaser2) {
uint32_t index_offset = 0; uint32_t index_offset = 0;
uint32_t distance_offset = 0; uint32_t distance_offset = 0;
uint32_t stamps_offset = 0; uint32_t stamps_offset = 0;
for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); for (std::vector<robot_sensor_msgs::PointField>::iterator f = cloud_out.fields.begin();
f != cloud_out.fields.end(); f++) f != cloud_out.fields.end(); f++)
{ {
if (f->name == "x") {x_offset = f->offset;} if (f->name == "x") {x_offset = f->offset;}
@ -319,11 +319,11 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
try { try {
// printf("%f %f %f %f %f %f\n", // printf("%f %f %f %f %f %f\n",
// range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec()); // range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
sensor_msgs::LaserScan scan = build_constant_scan(option); robot_sensor_msgs::LaserScan scan = build_constant_scan(option);
scan.header.frame_id = "laser_frame"; scan.header.frame_id = "laser_frame";
sensor_msgs::PointCloud2 cloud_out; robot_sensor_msgs::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud( projector.transformLaserScanToPointCloud(
scan.header.frame_id, scan, cloud_out, tf3, -1.0, scan.header.frame_id, scan, cloud_out, tf3, -1.0,
laser_geometry::channel_option::None); laser_geometry::channel_option::None);
@ -377,7 +377,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
uint32_t index_offset = 0; uint32_t index_offset = 0;
uint32_t distance_offset = 0; uint32_t distance_offset = 0;
uint32_t stamps_offset = 0; uint32_t stamps_offset = 0;
for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin(); for (std::vector<robot_sensor_msgs::PointField>::iterator f = cloud_out.fields.begin();
f != cloud_out.fields.end(); f++) f != cloud_out.fields.end(); f++)
{ {
if (f->name == "x") {x_offset = f->offset;} if (f->name == "x") {x_offset = f->offset;}