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;}

156
test/projection_test.py
View File

@@ -1,156 +0,0 @@
# Copyright (c) 2014, Enrique Fernandez
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the copyright holder nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
from itertools import product
from laser_geometry import LaserProjection
import numpy as np
import pytest
import rclpy
import rclpy.duration
import rclpy.time
from sensor_msgs.msg import LaserScan
import sensor_msgs_py.point_cloud2 as pc2
PROJECTION_TEST_RANGE_MIN = 0.23
PROJECTION_TEST_RANGE_MAX = 40.00
class BuildScanException(Exception):
pass
def build_constant_scan(
range_val, intensity_val,
angle_min, angle_max, angle_increment, scan_time):
count = np.uint(np.ceil((angle_max - angle_min) / angle_increment))
if count < 0:
raise BuildScanException
scan = LaserScan()
scan.header.stamp = rclpy.time.Time(seconds=10.10).to_msg()
scan.header.frame_id = 'laser_frame'
scan.angle_min = angle_min
scan.angle_max = angle_max
scan.angle_increment = angle_increment
scan.scan_time = scan_time.nanoseconds / 1e9
scan.range_min = PROJECTION_TEST_RANGE_MIN
scan.range_max = PROJECTION_TEST_RANGE_MAX
scan.ranges = [range_val for _ in range(count)]
scan.intensities = [intensity_val for _ in range(count)]
scan.time_increment = scan_time.nanoseconds / 1e9 / count
return scan
def test_project_laser():
tolerance = 6 # decimal places
projector = LaserProjection()
ranges = [-1.0, 1.0, 5.0, 100.0]
intensities = np.arange(1.0, 4.0).tolist()
min_angles = -np.pi / np.array([1.0, 1.5, 8.0])
max_angles = -min_angles
angle_increments = np.pi / np.array([180., 360., 720.])
scan_times = [rclpy.duration.Duration(seconds=1./i) for i in [40, 20]]
for range_val, intensity_val, \
angle_min, angle_max, angle_increment, scan_time in \
product(
ranges, intensities,
min_angles, max_angles,
angle_increments, scan_times):
try:
scan = build_constant_scan(
range_val, intensity_val,
angle_min, angle_max, angle_increment, scan_time)
except BuildScanException:
assert (angle_max - angle_min)/angle_increment <= 0
cloud_out = projector.projectLaser(
scan, -1.0,
LaserProjection.ChannelOption.INTENSITY |
LaserProjection.ChannelOption.INDEX |
LaserProjection.ChannelOption.DISTANCE |
LaserProjection.ChannelOption.TIMESTAMP)
assert len(cloud_out.fields) == 7, 'PointCloud2 with channel INDEX: fields size != 7'
valid_points = 0
for i in range(len(scan.ranges)):
ri = scan.ranges[i]
if (PROJECTION_TEST_RANGE_MIN <= ri and ri <= PROJECTION_TEST_RANGE_MAX):
valid_points += 1
assert valid_points == cloud_out.width, 'Valid points != PointCloud2 width'
idx_x = idx_y = idx_z = 0
idx_intensity = idx_index = 0
idx_distance = idx_stamps = 0
i = 0
for f in cloud_out.fields:
if f.name == 'x':
idx_x = i
elif f.name == 'y':
idx_y = i
elif f.name == 'z':
idx_z = i
elif f.name == 'intensity':
idx_intensity = i
elif f.name == 'index':
idx_index = i
elif f.name == 'distances':
idx_distance = i
elif f.name == 'stamps':
idx_stamps = i
i += 1
i = 0
for point in pc2.read_points(cloud_out):
ri = scan.ranges[i]
ai = scan.angle_min + i * scan.angle_increment
assert point[idx_x] == pytest.approx(ri * np.cos(ai), abs=tolerance), 'x not equal'
assert point[idx_y] == pytest.approx(ri * np.sin(ai), tolerance), 'y not equal'
assert point[idx_z] == pytest.approx(0, tolerance), 'z not equal'
assert point[idx_intensity] == pytest.approx(
scan.intensities[i],
tolerance), 'Intensity not equal'
assert point[idx_index] == pytest.approx(i, tolerance), 'Index not equal'
assert point[idx_distance] == pytest.approx(ri, tolerance), 'Distance not equal'
assert point[idx_stamps] == pytest.approx(
i * scan.time_increment, tolerance), 'Timestamp not equal'
i += 1
if __name__ == '__main__':
pytest.main()