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

Uncrustify

Browse Source
This commit is contained in:
Martin Idel
2018-03-14 13:36:50 +01:00
parent b88330b64c
commit 433181e63e
3 changed files with 730 additions and 689 deletions
Download Patch File Download Diff File Expand all files Collapse all files

380
test/projection_test.cpp
View File

@@ -1,10 +1,10 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* 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
@@ -13,7 +13,7 @@
* * Neither the name of the Willow Garage, Inc. 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
@@ -37,12 +37,15 @@
#include "sensor_msgs/msg/point_cloud2.hpp"
#define PROJECTION_TEST_RANGE_MIN (0.23)
#define PROJECTION_TEST_RANGE_MAX (40.0)
#define PROJECTION_TEST_RANGE_MAX (40.0)
class BuildScanException { };
class BuildScanException
{
};
struct ScanOptions {
struct ScanOptions
{
double range_;
double intensity_;
double ang_min_;
@@ -50,10 +53,11 @@ struct ScanOptions {
double ang_increment_;
rclcpp::Duration scan_time_;
ScanOptions(double range, double intensity,
ScanOptions(
double range, double intensity,
double ang_min, double ang_max, double ang_increment,
rclcpp::Duration scan_time) :
range_(range),
rclcpp::Duration scan_time)
: range_(range),
intensity_(intensity),
ang_min_(ang_min),
ang_max_(ang_max),
@@ -63,8 +67,9 @@ struct ScanOptions {
sensor_msgs::msg::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());
}
sensor_msgs::msg::LaserScan scan;
scan.header.stamp = rclcpp::Clock().now();
@@ -76,21 +81,19 @@ sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
scan.range_min = PROJECTION_TEST_RANGE_MIN;
scan.range_max = PROJECTION_TEST_RANGE_MAX;
uint32_t i = 0;
for(; options.ang_min_ + i * options.ang_increment_ < options.ang_max_; i++)
{
for (; options.ang_min_ + i * options.ang_increment_ < options.ang_max_; i++) {
scan.ranges.push_back(options.range_);
scan.intensities.push_back(options.intensity_);
}
scan.time_increment = options.scan_time_.nanoseconds()/(double)i;
scan.time_increment = options.scan_time_.nanoseconds() / (double)i;
return scan;
};
}
TEST(laser_geometry, projectLaser2)
{
TEST(laser_geometry, projectLaser2) {
double tolerance = 1e-12;
laser_geometry::LaserProjection projector;
laser_geometry::LaserProjection projector;
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<rclcpp::Duration> increment_times, scan_times;
@@ -110,34 +113,34 @@ TEST(laser_geometry, projectLaser2)
intensities.push_back(5.0);
min_angles.push_back(-M_PI);
min_angles.push_back(-M_PI/1.5);
min_angles.push_back(-M_PI/2);
min_angles.push_back(-M_PI/4);
min_angles.push_back(-M_PI/8);
min_angles.push_back(-M_PI / 1.5);
min_angles.push_back(-M_PI / 2);
min_angles.push_back(-M_PI / 4);
min_angles.push_back(-M_PI / 8);
max_angles.push_back(M_PI);
max_angles.push_back(M_PI/1.5);
max_angles.push_back(M_PI/2);
max_angles.push_back(M_PI/4);
max_angles.push_back(M_PI/8);
max_angles.push_back(M_PI / 1.5);
max_angles.push_back(M_PI / 2);
max_angles.push_back(M_PI / 4);
max_angles.push_back(M_PI / 8);
// angle_increments.push_back(-M_PI/180); // -one degree
angle_increments.push_back(M_PI/180); // one degree
angle_increments.push_back(M_PI/360); // half degree
angle_increments.push_back(M_PI/720); // quarter degree
angle_increments.push_back(M_PI / 180); // one degree
angle_increments.push_back(M_PI / 360); // half degree
angle_increments.push_back(M_PI / 720); // quarter degree
scan_times.push_back(rclcpp::Duration(1/40));
scan_times.push_back(rclcpp::Duration(1/20));
scan_times.push_back(rclcpp::Duration(1 / 40));
scan_times.push_back(rclcpp::Duration(1 / 20));
std::vector<ScanOptions> options;
for(auto range : ranges) {
for(auto intensity : intensities) {
for(auto min_angle : min_angles) {
for(auto max_angle : max_angles) {
for(auto angle_increment : angle_increments) {
for(auto scan_time : scan_times) {
for (auto range : ranges) {
for (auto intensity : intensities) {
for (auto min_angle : min_angles) {
for (auto max_angle : max_angles) {
for (auto angle_increment : angle_increments) {
for (auto scan_time : scan_times) {
options.push_back(ScanOptions(
range, intensity, min_angle, max_angle, angle_increment, scan_time));
range, intensity, min_angle, max_angle, angle_increment, scan_time));
}
}
}
@@ -145,66 +148,86 @@ TEST(laser_geometry, projectLaser2)
}
}
for (auto option : options){
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);
for (auto option : options) {
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::msg::PointCloud2 cloud_out;
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
sensor_msgs::msg::PointCloud2 cloud_out;
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
projector.projectLaser(scan, cloud_out, -1.0);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.projectLaser(scan, cloud_out, -1.0);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.projectLaser(scan, cloud_out, -1.0,
laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
projector.projectLaser(scan, cloud_out, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
projector.projectLaser(scan, cloud_out, -1.0,
laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance |
laser_geometry::channel_option::Timestamp);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
unsigned int valid_points = 0;
for (unsigned int i = 0; i < scan.ranges.size(); i++)
if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
valid_points ++;
unsigned int valid_points = 0;
for (unsigned int i = 0; i < scan.ranges.size(); i++) {
if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX &&
scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
{
valid_points++;
}
}
EXPECT_EQ(valid_points, cloud_out.width);
EXPECT_EQ(valid_points, cloud_out.width);
uint32_t x_offset = 0;
uint32_t y_offset = 0;
uint32_t z_offset = 0;
uint32_t intensity_offset = 0;
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(); f != cloud_out.fields.end(); f++) {
if (f->name == "x") x_offset = f->offset;
if (f->name == "y") y_offset = f->offset;
if (f->name == "z") z_offset = f->offset;
if (f->name == "intensity") intensity_offset = f->offset;
if (f->name == "index") index_offset = f->offset;
if (f->name == "distances") distance_offset = f->offset;
if (f->name == "stamps") stamps_offset = f->offset;
}
uint32_t x_offset = 0;
uint32_t y_offset = 0;
uint32_t z_offset = 0;
uint32_t intensity_offset = 0;
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();
f != cloud_out.fields.end(); f++)
{
if (f->name == "x") {x_offset = f->offset;}
if (f->name == "y") {y_offset = f->offset;}
if (f->name == "z") {z_offset = f->offset;}
if (f->name == "intensity") {intensity_offset = f->offset;}
if (f->name == "index") {index_offset = f->offset;}
if (f->name == "distances") {distance_offset = f->offset;}
if (f->name == "stamps") {stamps_offset = f->offset;}
}
for (unsigned int i = 0; i < cloud_out.width; i++) {
for (unsigned int i = 0; i < cloud_out.width; i++) {
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
};
}
catch (BuildScanException &ex)
{
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset],
(float)((double)(scan.ranges[i]) *
cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset],
(float)((double)(scan.ranges[i]) *
sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset],
scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i,
tolerance); // index
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset],
scan.ranges[i], tolerance); // ranges
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset],
(float)i * scan.time_increment, tolerance); // timestamps
}
} catch (BuildScanException & ex) {
// make sure it is not a false exception
if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) {
FAIL();
@@ -213,13 +236,12 @@ TEST(laser_geometry, projectLaser2)
}
}
TEST(laser_geometry, transformLaserScanToPointCloud2)
{
TEST(laser_geometry, transformLaserScanToPointCloud2) {
tf2::BufferCore tf2;
double tolerance = 1e-12;
laser_geometry::LaserProjection projector;
laser_geometry::LaserProjection projector;
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<rclcpp::Duration> increment_times, scan_times;
@@ -239,35 +261,34 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
intensities.push_back(5.0);
min_angles.push_back(-M_PI);
min_angles.push_back(-M_PI/1.5);
min_angles.push_back(-M_PI/2);
min_angles.push_back(-M_PI/4);
min_angles.push_back(-M_PI/8);
min_angles.push_back(-M_PI / 1.5);
min_angles.push_back(-M_PI / 2);
min_angles.push_back(-M_PI / 4);
min_angles.push_back(-M_PI / 8);
max_angles.push_back(M_PI);
max_angles.push_back(M_PI/1.5);
max_angles.push_back(M_PI/2);
max_angles.push_back(M_PI/4);
max_angles.push_back(M_PI/8);
max_angles.push_back(M_PI / 1.5);
max_angles.push_back(M_PI / 2);
max_angles.push_back(M_PI / 4);
max_angles.push_back(M_PI / 8);
angle_increments.push_back(-M_PI/180); // -one degree
angle_increments.push_back(M_PI/180); // one degree
angle_increments.push_back(M_PI/360); // half degree
angle_increments.push_back(M_PI/720); // quarter degree
scan_times.push_back(rclcpp::Duration(1/40));
scan_times.push_back(rclcpp::Duration(1/20));
angle_increments.push_back(-M_PI / 180); // -one degree
angle_increments.push_back(M_PI / 180); // one degree
angle_increments.push_back(M_PI / 360); // half degree
angle_increments.push_back(M_PI / 720); // quarter degree
scan_times.push_back(rclcpp::Duration(1 / 40));
scan_times.push_back(rclcpp::Duration(1 / 20));
std::vector<ScanOptions> options;
for(auto range : ranges) {
for(auto intensity : intensities) {
for(auto min_angle : min_angles) {
for(auto max_angle : max_angles) {
for(auto angle_increment : angle_increments) {
for(auto scan_time : scan_times) {
for (auto range : ranges) {
for (auto intensity : intensities) {
for (auto min_angle : min_angles) {
for (auto max_angle : max_angles) {
for (auto angle_increment : angle_increments) {
for (auto scan_time : scan_times) {
options.push_back(ScanOptions(
range, intensity, min_angle, max_angle, angle_increment, scan_time));
range, intensity, min_angle, max_angle, angle_increment, scan_time));
}
}
}
@@ -275,73 +296,93 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
}
}
for (auto option : options){
try
{
// printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
for (auto option : options) {
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);
scan.header.frame_id = "laser_frame";
sensor_msgs::msg::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::None);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
sensor_msgs::msg::PointCloud2 cloud_out;
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::None);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity |
laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)6);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf2, -1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index |
laser_geometry::channel_option::Distance |
laser_geometry::channel_option::Timestamp);
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)7);
EXPECT_EQ(cloud_out.is_dense, false);
EXPECT_EQ(cloud_out.is_dense, false);
unsigned int valid_points = 0;
for (unsigned int i = 0; i < scan.ranges.size(); i++)
if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
valid_points ++;
EXPECT_EQ(valid_points, cloud_out.width);
unsigned int valid_points = 0;
for (unsigned int i = 0; i < scan.ranges.size(); i++) {
if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX &&
scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
{
valid_points++;
}
}
EXPECT_EQ(valid_points, cloud_out.width);
uint32_t x_offset = 0;
uint32_t y_offset = 0;
uint32_t z_offset = 0;
uint32_t intensity_offset = 0;
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(); f != cloud_out.fields.end(); f++)
{
if (f->name == "x") x_offset = f->offset;
if (f->name == "y") y_offset = f->offset;
if (f->name == "z") z_offset = f->offset;
if (f->name == "intensity") intensity_offset = f->offset;
if (f->name == "index") index_offset = f->offset;
if (f->name == "distances") distance_offset = f->offset;
if (f->name == "stamps") stamps_offset = f->offset;
}
uint32_t x_offset = 0;
uint32_t y_offset = 0;
uint32_t z_offset = 0;
uint32_t intensity_offset = 0;
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();
f != cloud_out.fields.end(); f++)
{
if (f->name == "x") {x_offset = f->offset;}
if (f->name == "y") {y_offset = f->offset;}
if (f->name == "z") {z_offset = f->offset;}
if (f->name == "intensity") {intensity_offset = f->offset;}
if (f->name == "index") {index_offset = f->offset;}
if (f->name == "distances") {distance_offset = f->offset;}
if (f->name == "stamps") {stamps_offset = f->offset;}
}
for (unsigned int i = 0; i < cloud_out.width; i++)
{
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + x_offset] , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + y_offset] , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + z_offset] , 0, tolerance);
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + intensity_offset] , scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
EXPECT_NEAR(*(uint32_t*)&cloud_out.data[i*cloud_out.point_step + index_offset], i, tolerance);//index
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + distance_offset], scan.ranges[i], tolerance);//ranges
EXPECT_NEAR(*(float*)&cloud_out.data[i*cloud_out.point_step + stamps_offset], (float)i * scan.time_increment, tolerance);//timestamps
for (unsigned int i = 0; i < cloud_out.width; i++) {
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + x_offset],
(float)((double)(scan.ranges[i]) *
cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + y_offset],
(float)((double)(scan.ranges[i]) *
sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + z_offset], 0, tolerance);
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + intensity_offset],
scan.intensities[i], tolerance); // intensity
EXPECT_NEAR(*(uint32_t *)&cloud_out.data[i * cloud_out.point_step + index_offset], i,
tolerance); // index
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + distance_offset],
scan.ranges[i], tolerance); // ranges
EXPECT_NEAR(*(float *)&cloud_out.data[i * cloud_out.point_step + stamps_offset],
(float)i * scan.time_increment, tolerance); // timestamps
};
}
catch (BuildScanException &ex)
{
}
} catch (BuildScanException & ex) {
// make sure it is not a false exception
if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) {
FAIL();
@@ -351,7 +392,8 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
}
int main(int argc, char **argv){
int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}