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

48
test/projection_test.cpp
View File

@@ -33,10 +33,12 @@
#include <cmath>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include <robot/time.h>
#include <robot/duration.h>
#include "laser_geometry/laser_geometry.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#define PROJECTION_TEST_RANGE_MIN (0.23f)
#define PROJECTION_TEST_RANGE_MAX (40.0f)
@@ -54,12 +56,12 @@ struct ScanOptions
float ang_min_;
float ang_max_;
float ang_increment_;
rclcpp::Duration scan_time_;
robot::Duration scan_time_;
ScanOptions(
float range, float intensity,
float ang_min, float ang_max, float ang_increment,
rclcpp::Duration scan_time)
robot::Duration scan_time)
: range_(range),
intensity_(intensity),
ang_min_(ang_min),
@@ -68,19 +70,21 @@ struct ScanOptions
scan_time_(scan_time) {}
};
sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
sensor_msgs::LaserScan build_constant_scan(const ScanOptions & options)
{
if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0) {
throw (BuildScanException());
}
sensor_msgs::msg::LaserScan scan;
scan.header.stamp = rclcpp::Clock().now();
sensor_msgs::LaserScan scan;
robot::Time now = robot::Time::now();
scan.header.stamp.sec = now.sec;
scan.header.stamp.nsec = now.nsec;
scan.header.frame_id = "laser_frame";
scan.angle_min = options.ang_min_;
scan.angle_max = options.ang_max_;
scan.angle_increment = options.ang_increment_;
scan.scan_time = static_cast<float>(options.scan_time_.seconds());
scan.scan_time = static_cast<float>(options.scan_time_.toSec());
scan.range_min = PROJECTION_TEST_RANGE_MIN;
scan.range_max = PROJECTION_TEST_RANGE_MAX;
uint32_t i = 0;
@@ -90,13 +94,13 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
}
scan.time_increment =
static_cast<float>(options.scan_time_.seconds() / static_cast<double>(i));
static_cast<float>(options.scan_time_.toSec() / static_cast<double>(i));
return scan;
}
template<typename T>
T cloudData(sensor_msgs::msg::PointCloud2 cloud_out, uint32_t index)
T cloudData(sensor_msgs::PointCloud2 cloud_out, uint32_t index)
{
return *reinterpret_cast<T *>(&cloud_out.data[index]);
}
@@ -106,7 +110,7 @@ TEST(laser_geometry, projectLaser2) {
laser_geometry::LaserProjection projector;
std::vector<float> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<rclcpp::Duration> increment_times, scan_times;
std::vector<robot::Duration> increment_times, scan_times;
ranges.push_back(-1.0f);
ranges.push_back(1.0f);
@@ -129,8 +133,8 @@ TEST(laser_geometry, projectLaser2) {
angle_increments.push_back(PI / 180); // one degree
angle_increments.push_back(PI / 720); // quarter degree
scan_times.push_back(rclcpp::Duration::from_seconds(1. / 40));
scan_times.push_back(rclcpp::Duration::from_seconds(1. / 20));
scan_times.push_back(robot::Duration(1. / 40));
scan_times.push_back(robot::Duration(1. / 20));
std::vector<ScanOptions> options;
for (auto range : ranges) {
@@ -153,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::msg::LaserScan scan = build_constant_scan(option);
sensor_msgs::LaserScan scan = build_constant_scan(option);
sensor_msgs::msg::PointCloud2 cloud_out;
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);
@@ -201,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::msg::PointField>::iterator f = cloud_out.fields.begin();
for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin();
f != cloud_out.fields.end(); f++)
{
if (f->name == "x") {x_offset = f->offset;}
@@ -258,7 +262,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
laser_geometry::LaserProjection projector;
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<rclcpp::Duration> increment_times, scan_times;
std::vector<robot::Duration> increment_times, scan_times;
ranges.push_back(-1.0);
ranges.push_back(1.0);
@@ -291,8 +295,8 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) {
angle_increments.push_back(M_PI / 360); // half degree
angle_increments.push_back(M_PI / 720); // quarter degree
scan_times.push_back(rclcpp::Duration::from_seconds(1. / 40));
scan_times.push_back(rclcpp::Duration::from_seconds(1. / 20));
scan_times.push_back(robot::Duration(1. / 40));
scan_times.push_back(robot::Duration(1. / 20));
std::vector<ScanOptions> options;
for (auto range : ranges) {
@@ -315,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::msg::LaserScan scan = build_constant_scan(option);
sensor_msgs::LaserScan scan = build_constant_scan(option);
scan.header.frame_id = "laser_frame";
sensor_msgs::msg::PointCloud2 cloud_out;
sensor_msgs::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud(
scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::None);
@@ -373,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::msg::PointField>::iterator f = cloud_out.fields.begin();
for (std::vector<sensor_msgs::PointField>::iterator f = cloud_out.fields.begin();
f != cloud_out.fields.end(); f++)
{
if (f->name == "x") {x_offset = f->offset;}