Add tests (remove superfluous test cases) and linters
- Code now lints with standard ament linters - Added test cases for LaserScan to PointCloud2 - Removed tests that were commented out + tests for LaserScan to PointCloud
This commit is contained in:
Martin Idel
parent
c6c6e833fe
commit
b88330b64c
|
|
@ -38,7 +38,6 @@ set_property(TARGET laser_geometry PROPERTY POSITION_INDEPENDENT_CODE ON)
|
|||
ament_export_include_directories(include)
|
||||
ament_export_interfaces(laser_geometry)
|
||||
ament_export_libraries(laser_geometry)
|
||||
ament_package(CONFIG_EXTRAS laser_geometry-extras.cmake)
|
||||
|
||||
install(
|
||||
TARGETS laser_geometry
|
||||
|
|
@ -54,3 +53,25 @@ install(
|
|||
DESTINATION include
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
# TODO(Martin-Idel-SI): replace this with ament_lint_auto() and/or add the copyright linter back
|
||||
find_package(ament_cmake_cppcheck REQUIRED)
|
||||
find_package(ament_cmake_cpplint REQUIRED)
|
||||
find_package(ament_cmake_lint_cmake REQUIRED)
|
||||
find_package(ament_cmake_uncrustify REQUIRED)
|
||||
ament_cppcheck()
|
||||
ament_cpplint()
|
||||
ament_lint_cmake()
|
||||
ament_uncrustify()
|
||||
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
find_package(ament_cmake_gmock REQUIRED)
|
||||
|
||||
ament_add_gtest(projection_test
|
||||
test/projection_test.cpp)
|
||||
if(TARGET projection_test)
|
||||
target_link_libraries(projection_test laser_geometry)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
ament_package(CONFIG_EXTRAS laser_geometry-extras.cmake)
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@ find_package(Eigen3 REQUIRED)
|
|||
# Eigen3 uses non-standard variable for
|
||||
# include dirs (case and name): EIGEN3_INCLUDE_DIR.
|
||||
if(NOT Eigen3_INCLUDE_DIRS)
|
||||
if (EIGEN3_INCLUDE_DIR)
|
||||
if(EIGEN3_INCLUDE_DIR)
|
||||
message(STATUS "append ${EIGEN3_INCLUDE_DIR} to (${laser_geometry_INCLUDE_DIRS})")
|
||||
list(APPEND laser_geometry_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
|
||||
message(STATUS "laser_geometry_INCLUDE_DIRS=${laser_geometry_INCLUDE_DIRS}")
|
||||
|
|
|
|||
|
|
@ -29,15 +29,12 @@
|
|||
|
||||
#include <gtest/gtest.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#include "laser_geometry/laser_geometry.h"
|
||||
#include "sensor_msgs/PointCloud.h"
|
||||
#include <math.h>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include <angles/angles.h>
|
||||
|
||||
#include "rostest/permuter.h"
|
||||
#include "laser_geometry/laser_geometry.hpp"
|
||||
#include "sensor_msgs/msg/point_cloud2.hpp"
|
||||
|
||||
#define PROJECTION_TEST_RANGE_MIN (0.23)
|
||||
#define PROJECTION_TEST_RANGE_MAX (40.0)
|
||||
|
|
@ -45,268 +42,58 @@
|
|||
|
||||
class BuildScanException { };
|
||||
|
||||
sensor_msgs::msg::LaserScan build_constant_scan(double range, double intensity,
|
||||
struct ScanOptions {
|
||||
double range_;
|
||||
double intensity_;
|
||||
double ang_min_;
|
||||
double ang_max_;
|
||||
double ang_increment_;
|
||||
rclcpp::Duration scan_time_;
|
||||
|
||||
ScanOptions(double range, double intensity,
|
||||
double ang_min, double ang_max, double ang_increment,
|
||||
ros::Duration scan_time)
|
||||
rclcpp::Duration scan_time) :
|
||||
range_(range),
|
||||
intensity_(intensity),
|
||||
ang_min_(ang_min),
|
||||
ang_max_(ang_max),
|
||||
ang_increment_(ang_increment),
|
||||
scan_time_(scan_time) {}
|
||||
};
|
||||
|
||||
sensor_msgs::msg::LaserScan build_constant_scan(const ScanOptions & options)
|
||||
{
|
||||
if (((ang_max - ang_min) / ang_increment) < 0)
|
||||
if (((options.ang_max_ - options.ang_min_) / options.ang_increment_) < 0)
|
||||
throw (BuildScanException());
|
||||
|
||||
sensor_msgs::msg::LaserScan scan;
|
||||
scan.header.stamp = ros::Time::now();
|
||||
scan.header.stamp = rclcpp::Clock().now();
|
||||
scan.header.frame_id = "laser_frame";
|
||||
scan.angle_min = ang_min;
|
||||
scan.angle_max = ang_max;
|
||||
scan.angle_increment = ang_increment;
|
||||
scan.scan_time = scan_time.toSec();
|
||||
scan.angle_min = options.ang_min_;
|
||||
scan.angle_max = options.ang_max_;
|
||||
scan.angle_increment = options.ang_increment_;
|
||||
scan.scan_time = options.scan_time_.nanoseconds();
|
||||
scan.range_min = PROJECTION_TEST_RANGE_MIN;
|
||||
scan.range_max = PROJECTION_TEST_RANGE_MAX;
|
||||
uint32_t i = 0;
|
||||
for(; ang_min + i * ang_increment < ang_max; i++)
|
||||
for(; options.ang_min_ + i * options.ang_increment_ < options.ang_max_; i++)
|
||||
{
|
||||
scan.ranges.push_back(range);
|
||||
scan.intensities.push_back(intensity);
|
||||
scan.ranges.push_back(options.range_);
|
||||
scan.intensities.push_back(options.intensity_);
|
||||
}
|
||||
|
||||
scan.time_increment = scan_time.toSec()/(double)i;
|
||||
scan.time_increment = options.scan_time_.nanoseconds()/(double)i;
|
||||
|
||||
return scan;
|
||||
};
|
||||
|
||||
|
||||
class TestProjection : public laser_geometry::LaserProjection
|
||||
{
|
||||
public:
|
||||
const boost::numeric::ublas::matrix<double>& getUnitVectors(double angle_min,
|
||||
double angle_max,
|
||||
double angle_increment,
|
||||
unsigned int length)
|
||||
{
|
||||
return getUnitVectors_(angle_min, angle_max, angle_increment, length);
|
||||
}
|
||||
};
|
||||
|
||||
void test_getUnitVectors(double angle_min, double angle_max, double angle_increment, unsigned int length)
|
||||
{
|
||||
double tolerance = 1e-12;
|
||||
TestProjection projector;
|
||||
|
||||
const boost::numeric::ublas::matrix<double> & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length);
|
||||
|
||||
|
||||
|
||||
for (unsigned int i = 0; i < mat.size2(); i++)
|
||||
{
|
||||
EXPECT_NEAR(angles::shortest_angular_distance(atan2(mat(1,i), mat(0,i)),
|
||||
angle_min + i * angle_increment),
|
||||
0,
|
||||
tolerance); // check expected angle
|
||||
EXPECT_NEAR(1.0, mat(1,i)*mat(1,i) + mat(0,i)*mat(0,i), tolerance); //make sure normalized
|
||||
}
|
||||
}
|
||||
|
||||
#if 0
|
||||
|
||||
TEST(laser_geometry, getUnitVectors)
|
||||
{
|
||||
double min_angle = -M_PI/2;
|
||||
double max_angle = M_PI/2;
|
||||
double angle_increment = M_PI/180;
|
||||
|
||||
std::vector<double> min_angles, max_angles, angle_increments;
|
||||
|
||||
rostest::Permuter permuter;
|
||||
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);
|
||||
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);
|
||||
permuter.addOptionSet(min_angles, &min_angle);
|
||||
|
||||
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);
|
||||
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);
|
||||
permuter.addOptionSet(max_angles, &max_angle);
|
||||
|
||||
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
|
||||
permuter.addOptionSet(angle_increments, &angle_increment);
|
||||
|
||||
|
||||
while (permuter.step())
|
||||
{
|
||||
if ((max_angle - min_angle) / angle_increment > 0.0)
|
||||
{
|
||||
unsigned int length = round((max_angle - min_angle)/ angle_increment);
|
||||
try
|
||||
{
|
||||
test_getUnitVectors(min_angle, max_angle, angle_increment, length);
|
||||
test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment);
|
||||
test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1);
|
||||
}
|
||||
catch (BuildScanException &ex)
|
||||
{
|
||||
if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
|
||||
FAIL();
|
||||
}
|
||||
}
|
||||
//else
|
||||
//printf("%f\n", (max_angle - min_angle) / angle_increment);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TEST(laser_geometry, projectLaser)
|
||||
{
|
||||
double tolerance = 1e-12;
|
||||
laser_geometry::LaserProjection projector;
|
||||
|
||||
double min_angle = -M_PI/2;
|
||||
double max_angle = M_PI/2;
|
||||
double angle_increment = M_PI/180;
|
||||
|
||||
double range = 1.0;
|
||||
double intensity = 1.0;
|
||||
|
||||
ros::Duration scan_time = ros::Duration(1/40);
|
||||
ros::Duration increment_time = ros::Duration(1/400);
|
||||
|
||||
|
||||
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
|
||||
std::vector<ros::Duration> increment_times, scan_times;
|
||||
|
||||
rostest::Permuter permuter;
|
||||
|
||||
ranges.push_back(-1.0);
|
||||
ranges.push_back(1.0);
|
||||
ranges.push_back(2.0);
|
||||
ranges.push_back(3.0);
|
||||
ranges.push_back(4.0);
|
||||
ranges.push_back(5.0);
|
||||
ranges.push_back(100.0);
|
||||
permuter.addOptionSet(ranges, &range);
|
||||
|
||||
intensities.push_back(1.0);
|
||||
intensities.push_back(2.0);
|
||||
intensities.push_back(3.0);
|
||||
intensities.push_back(4.0);
|
||||
intensities.push_back(5.0);
|
||||
permuter.addOptionSet(intensities, &intensity);
|
||||
|
||||
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);
|
||||
permuter.addOptionSet(min_angles, &min_angle);
|
||||
|
||||
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);
|
||||
permuter.addOptionSet(max_angles, &max_angle);
|
||||
|
||||
// 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
|
||||
permuter.addOptionSet(angle_increments, &angle_increment);
|
||||
|
||||
scan_times.push_back(ros::Duration(1/40));
|
||||
scan_times.push_back(ros::Duration(1/20));
|
||||
|
||||
permuter.addOptionSet(scan_times, &scan_time);
|
||||
|
||||
while (permuter.step())
|
||||
{
|
||||
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(range, intensity, min_angle, max_angle, angle_increment, scan_time);
|
||||
|
||||
sensor_msgs::msg::PointCloud cloud_out;
|
||||
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
|
||||
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
|
||||
|
||||
projector.projectLaser(scan, cloud_out, -1.0);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
|
||||
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
|
||||
|
||||
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.channels.size(), (unsigned int)3);
|
||||
|
||||
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.channels.size(), (unsigned int)4);
|
||||
|
||||
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.points.size());
|
||||
|
||||
for (unsigned int i = 0; i < cloud_out.points.size(); i++)
|
||||
{
|
||||
EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
|
||||
EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
|
||||
EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
|
||||
EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
|
||||
EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
|
||||
EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
|
||||
EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
|
||||
};
|
||||
}
|
||||
catch (BuildScanException &ex)
|
||||
{
|
||||
if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
|
||||
FAIL();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
TEST(laser_geometry, projectLaser2)
|
||||
{
|
||||
double tolerance = 1e-12;
|
||||
laser_geometry::LaserProjection projector;
|
||||
|
||||
double min_angle = -M_PI/2;
|
||||
double max_angle = M_PI/2;
|
||||
double angle_increment = M_PI/180;
|
||||
|
||||
double range = 1.0;
|
||||
double intensity = 1.0;
|
||||
|
||||
ros::Duration scan_time = ros::Duration(1/40);
|
||||
ros::Duration increment_time = ros::Duration(1/400);
|
||||
|
||||
|
||||
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
|
||||
std::vector<ros::Duration> increment_times, scan_times;
|
||||
|
||||
rostest::Permuter permuter;
|
||||
std::vector<rclcpp::Duration> increment_times, scan_times;
|
||||
|
||||
ranges.push_back(-1.0);
|
||||
ranges.push_back(1.0);
|
||||
|
|
@ -315,46 +102,54 @@ TEST(laser_geometry, projectLaser2)
|
|||
ranges.push_back(4.0);
|
||||
ranges.push_back(5.0);
|
||||
ranges.push_back(100.0);
|
||||
permuter.addOptionSet(ranges, &range);
|
||||
|
||||
intensities.push_back(1.0);
|
||||
intensities.push_back(2.0);
|
||||
intensities.push_back(3.0);
|
||||
intensities.push_back(4.0);
|
||||
intensities.push_back(5.0);
|
||||
permuter.addOptionSet(intensities, &intensity);
|
||||
|
||||
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);
|
||||
permuter.addOptionSet(min_angles, &min_angle);
|
||||
|
||||
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);
|
||||
permuter.addOptionSet(max_angles, &max_angle);
|
||||
|
||||
// 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
|
||||
permuter.addOptionSet(angle_increments, &angle_increment);
|
||||
|
||||
scan_times.push_back(ros::Duration(1/40));
|
||||
scan_times.push_back(ros::Duration(1/20));
|
||||
scan_times.push_back(rclcpp::Duration(1/40));
|
||||
scan_times.push_back(rclcpp::Duration(1/20));
|
||||
|
||||
permuter.addOptionSet(scan_times, &scan_time);
|
||||
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) {
|
||||
options.push_back(ScanOptions(
|
||||
range, intensity, min_angle, max_angle, angle_increment, scan_time));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
while (permuter.step())
|
||||
{
|
||||
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(range, intensity, min_angle, max_angle, angle_increment, scan_time);
|
||||
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);
|
||||
|
|
@ -387,8 +182,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(); f != cloud_out.fields.end(); f++)
|
||||
{
|
||||
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;
|
||||
|
|
@ -398,8 +192,7 @@ TEST(laser_geometry, projectLaser2)
|
|||
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);
|
||||
|
|
@ -412,153 +205,24 @@ TEST(laser_geometry, projectLaser2)
|
|||
}
|
||||
catch (BuildScanException &ex)
|
||||
{
|
||||
if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
|
||||
// make sure it is not a false exception
|
||||
if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) {
|
||||
FAIL();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TEST(laser_geometry, transformLaserScanToPointCloud)
|
||||
{
|
||||
|
||||
tf::Transformer tf;
|
||||
|
||||
double tolerance = 1e-12;
|
||||
laser_geometry::LaserProjection projector;
|
||||
|
||||
double min_angle = -M_PI/2;
|
||||
double max_angle = M_PI/2;
|
||||
double angle_increment = M_PI/180;
|
||||
|
||||
double range = 1.0;
|
||||
double intensity = 1.0;
|
||||
|
||||
ros::Duration scan_time = ros::Duration(1/40);
|
||||
ros::Duration increment_time = ros::Duration(1/400);
|
||||
|
||||
|
||||
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
|
||||
std::vector<ros::Duration> increment_times, scan_times;
|
||||
|
||||
rostest::Permuter permuter;
|
||||
|
||||
ranges.push_back(-1.0);
|
||||
ranges.push_back(1.0);
|
||||
ranges.push_back(2.0);
|
||||
ranges.push_back(3.0);
|
||||
ranges.push_back(4.0);
|
||||
ranges.push_back(5.0);
|
||||
ranges.push_back(100.0);
|
||||
permuter.addOptionSet(ranges, &range);
|
||||
|
||||
intensities.push_back(1.0);
|
||||
intensities.push_back(2.0);
|
||||
intensities.push_back(3.0);
|
||||
intensities.push_back(4.0);
|
||||
intensities.push_back(5.0);
|
||||
permuter.addOptionSet(intensities, &intensity);
|
||||
|
||||
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);
|
||||
|
||||
|
||||
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);
|
||||
|
||||
permuter.addOptionSet(min_angles, &min_angle);
|
||||
permuter.addOptionSet(max_angles, &max_angle);
|
||||
|
||||
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
|
||||
permuter.addOptionSet(angle_increments, &angle_increment);
|
||||
|
||||
scan_times.push_back(ros::Duration(1/40));
|
||||
scan_times.push_back(ros::Duration(1/20));
|
||||
|
||||
permuter.addOptionSet(scan_times, &scan_time);
|
||||
|
||||
while (permuter.step())
|
||||
{
|
||||
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(range, intensity, min_angle, max_angle, angle_increment, scan_time);
|
||||
scan.header.frame_id = "laser_frame";
|
||||
|
||||
sensor_msgs::msg::PointCloud cloud_out;
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
|
||||
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
|
||||
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
|
||||
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index | laser_geometry::channel_option::Distance | laser_geometry::channel_option::Timestamp);
|
||||
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
|
||||
|
||||
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.points.size());
|
||||
|
||||
for (unsigned int i = 0; i < cloud_out.points.size(); i++)
|
||||
{
|
||||
EXPECT_NEAR(cloud_out.points[i].x , (float)((double)(scan.ranges[i]) * cos((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
|
||||
EXPECT_NEAR(cloud_out.points[i].y , (float)((double)(scan.ranges[i]) * sin((double)(scan.angle_min) + i * (double)(scan.angle_increment))), tolerance);
|
||||
EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
|
||||
EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
|
||||
EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
|
||||
EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
|
||||
EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
|
||||
};
|
||||
}
|
||||
catch (BuildScanException &ex)
|
||||
{
|
||||
if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
|
||||
FAIL();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(laser_geometry, transformLaserScanToPointCloud2)
|
||||
{
|
||||
|
||||
tf::Transformer tf;
|
||||
tf2::BufferCore tf2;
|
||||
|
||||
double tolerance = 1e-12;
|
||||
laser_geometry::LaserProjection projector;
|
||||
|
||||
double min_angle = -M_PI/2;
|
||||
double max_angle = M_PI/2;
|
||||
double angle_increment = M_PI/180;
|
||||
|
||||
double range = 1.0;
|
||||
double intensity = 1.0;
|
||||
|
||||
ros::Duration scan_time = ros::Duration(1/40);
|
||||
ros::Duration increment_time = ros::Duration(1/400);
|
||||
|
||||
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
|
||||
std::vector<ros::Duration> increment_times, scan_times;
|
||||
|
||||
rostest::Permuter permuter;
|
||||
std::vector<rclcpp::Duration> increment_times, scan_times;
|
||||
|
||||
ranges.push_back(-1.0);
|
||||
ranges.push_back(1.0);
|
||||
|
|
@ -567,14 +231,12 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
|
|||
ranges.push_back(4.0);
|
||||
ranges.push_back(5.0);
|
||||
ranges.push_back(100.0);
|
||||
permuter.addOptionSet(ranges, &range);
|
||||
|
||||
intensities.push_back(1.0);
|
||||
intensities.push_back(2.0);
|
||||
intensities.push_back(3.0);
|
||||
intensities.push_back(4.0);
|
||||
intensities.push_back(5.0);
|
||||
permuter.addOptionSet(intensities, &intensity);
|
||||
|
||||
min_angles.push_back(-M_PI);
|
||||
min_angles.push_back(-M_PI/1.5);
|
||||
|
|
@ -582,65 +244,61 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
|
|||
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);
|
||||
|
||||
permuter.addOptionSet(min_angles, &min_angle);
|
||||
permuter.addOptionSet(max_angles, &max_angle);
|
||||
|
||||
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
|
||||
permuter.addOptionSet(angle_increments, &angle_increment);
|
||||
|
||||
scan_times.push_back(ros::Duration(1/40));
|
||||
scan_times.push_back(ros::Duration(1/20));
|
||||
scan_times.push_back(rclcpp::Duration(1/40));
|
||||
scan_times.push_back(rclcpp::Duration(1/20));
|
||||
|
||||
permuter.addOptionSet(scan_times, &scan_time);
|
||||
|
||||
while (permuter.step())
|
||||
{
|
||||
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) {
|
||||
options.push_back(ScanOptions(
|
||||
range, intensity, min_angle, max_angle, angle_increment, scan_time));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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(range, intensity, min_angle, max_angle, angle_increment, scan_time);
|
||||
// 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, tf, -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::None);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -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::Index);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4);
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -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, -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, tf);
|
||||
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, tf, -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);
|
||||
EXPECT_EQ(cloud_out.fields.size(), (unsigned int)5);
|
||||
|
||||
projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, -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, tf, -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);
|
||||
|
||||
|
|
@ -684,16 +342,16 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
|
|||
}
|
||||
catch (BuildScanException &ex)
|
||||
{
|
||||
if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
|
||||
// make sure it is not a false exception
|
||||
if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) {
|
||||
FAIL();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char **argv){
|
||||
ros::Time::init();
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user