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
sensor_msgs
robot_sensor_msgs
geometry_msgs
robot_time
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 "tf3/buffer_core.h"
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_geometry_msgs/TransformStamped.h>
#include "laser_geometry/visibility_control.hpp"
@ -104,7 +104,7 @@ public:
LaserProjection()
: 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
* point cloud. The generated cloud will be in the same frame
@ -122,15 +122,15 @@ public:
*/
LASER_GEOMETRY_PUBLIC
void projectLaser(
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
const robot_sensor_msgs::LaserScan & scan_in,
robot_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::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
* point cloud, accounting for movement of the laser over the
@ -155,8 +155,8 @@ public:
LASER_GEOMETRY_PUBLIC
void transformLaserScanToPointCloud(
const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
const robot_sensor_msgs::LaserScan & scan_in,
robot_sensor_msgs::PointCloud2 & cloud_out,
tf3::BufferCore & tf,
double range_cutoff = -1.0,
int channel_options = channel_option::Default)
@ -168,16 +168,16 @@ public:
private:
// Internal hidden representation of projectLaser
void projectLaser_(
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
const robot_sensor_msgs::LaserScan & scan_in,
robot_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::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
const robot_sensor_msgs::LaserScan & scan_in,
robot_sensor_msgs::PointCloud2 & cloud_out,
tf3::BufferCore & tf,
double range_cutoff,
int channel_options);
@ -185,8 +185,8 @@ private:
// Function used by the several forms of transformLaserScanToPointCloud_
void transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
const robot_sensor_msgs::LaserScan & scan_in,
robot_sensor_msgs::PointCloud2 & cloud_out,
tf3::Quaternion quat_start,
tf3::Vector3 origin_start,
tf3::Quaternion quat_end,

38
src/laser_geometry.cpp
View File

@ -38,8 +38,8 @@
#include <robot/time.h>
#include <robot/duration.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <tf3/buffer_core.h>
#include<tf3/convert.h>
@ -48,8 +48,8 @@
namespace laser_geometry
{
void LaserProjection::projectLaser_(
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
const robot_sensor_msgs::LaserScan & scan_in,
robot_sensor_msgs::PointCloud2 & cloud_out,
double range_cutoff,
int channel_options)
{
@ -89,15 +89,15 @@ void LaserProjection::projectLaser_(
cloud_out.fields.resize(3);
cloud_out.fields[0].name = "x";
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[1].name = "y";
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[2].name = "z";
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;
// 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();
cloud_out.fields.resize(field_size + 1);
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].count = 1;
offset += 4;
@ -121,7 +121,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
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].count = 1;
offset += 4;
@ -132,7 +132,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
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].count = 1;
offset += 4;
@ -143,7 +143,7 @@ void LaserProjection::projectLaser_(
size_t field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
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].count = 1;
offset += 4;
@ -155,19 +155,19 @@ void LaserProjection::projectLaser_(
cloud_out.fields.resize(field_size + 3);
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].count = 1;
offset += 4;
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].count = 1;
offset += 4;
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].count = 1;
offset += 4;
@ -269,8 +269,8 @@ void LaserProjection::projectLaser_(
void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
const robot_sensor_msgs::LaserScan & scan_in,
robot_sensor_msgs::PointCloud2 & cloud_out,
tf3::Quaternion quat_start,
tf3::Vector3 origin_start,
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 (!requested_index) {
sensor_msgs::PointCloud2 cloud_without_index;
robot_sensor_msgs::PointCloud2 cloud_without_index;
// copy basic meta data
cloud_without_index.header = cloud_out.header;
@ -419,8 +419,8 @@ void LaserProjection::transformLaserScanToPointCloud_(
void LaserProjection::transformLaserScanToPointCloud_(
const std::string & target_frame,
const sensor_msgs::LaserScan & scan_in,
sensor_msgs::PointCloud2 & cloud_out,
const robot_sensor_msgs::LaserScan & scan_in,
robot_sensor_msgs::PointCloud2 & cloud_out,
tf3::BufferCore & tf,
double range_cutoff,
int channel_options)

6
src/laser_geometry/laser_geometry.py
View File

@ -30,8 +30,8 @@
import numpy as np
import rclpy
import rclpy.logging
from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2
from robot_sensor_msgs.msg import PointCloud2
import robot_sensor_msgs_py.point_cloud2 as pc2
class LaserProjection:
@ -86,7 +86,7 @@ class LaserProjection:
self, scan_in,
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
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 "laser_geometry/laser_geometry.hpp"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <robot_sensor_msgs/PointCloud2.h>
#include <robot_sensor_msgs/LaserScan.h>
#define PROJECTION_TEST_RANGE_MIN (0.23f)
#define PROJECTION_TEST_RANGE_MAX (40.0f)
@ -70,13 +70,13 @@ struct ScanOptions
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) {
throw (BuildScanException());
}
sensor_msgs::LaserScan scan;
robot_sensor_msgs::LaserScan scan;
robot::Time now = robot::Time::now();
scan.header.stamp.sec = now.sec;
scan.header.stamp.nsec = now.nsec;
@ -100,7 +100,7 @@ sensor_msgs::LaserScan build_constant_scan(const ScanOptions & options)
}
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]);
}
@ -157,9 +157,9 @@ TEST(laser_geometry, projectLaser2) {
try {
// printf("%f %f %f %f %f %f\n",
// 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);
EXPECT_EQ(cloud_out.fields.size(), 4u);
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 distance_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++)
{
if (f->name == "x") {x_offset = f->offset;}
@ -319,11 +319,11 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
try {
// printf("%f %f %f %f %f %f\n",
// 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";
sensor_msgs::PointCloud2 cloud_out;
robot_sensor_msgs::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud(
scan.header.frame_id, scan, cloud_out, tf3, -1.0,
laser_geometry::channel_option::None);
@ -377,7 +377,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
uint32_t index_offset = 0;
uint32_t distance_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++)
{
if (f->name == "x") {x_offset = f->offset;}