From b88330b64c15ed76dd0bed35334bd43857a700e2 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 14 Mar 2018 13:23:00 +0100 Subject: [PATCH] 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 --- CMakeLists.txt | 23 +- laser_geometry-extras.cmake | 4 +- test/projection_test.cpp | 520 ++++++------------------------------ 3 files changed, 113 insertions(+), 434 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 06c4f24..aa7b344 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/laser_geometry-extras.cmake b/laser_geometry-extras.cmake index aa495ef..0790820 100644 --- a/laser_geometry-extras.cmake +++ b/laser_geometry-extras.cmake @@ -21,9 +21,9 @@ 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}) + list(APPEND laser_geometry_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) message(STATUS "laser_geometry_INCLUDE_DIRS=${laser_geometry_INCLUDE_DIRS}") else() message(FATAL_ERROR "Eigen3_INCLUDE_DIRS not found") diff --git a/test/projection_test.cpp b/test/projection_test.cpp index d664789..ab7aedd 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -29,15 +29,12 @@ #include #include - -#include "laser_geometry/laser_geometry.h" -#include "sensor_msgs/PointCloud.h" #include +#include "rclcpp/rclcpp.hpp" -#include - -#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, - double ang_min, double ang_max, double ang_increment, - ros::Duration scan_time) +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, + 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& 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 & 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 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 ranges, intensities, min_angles, max_angles, angle_increments; - std::vector 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 ranges, intensities, min_angles, max_angles, angle_increments; - std::vector increment_times, scan_times; - - rostest::Permuter permuter; + std::vector 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 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::iterator f = cloud_out.fields.begin(); f != cloud_out.fields.end(); f++) - { + for (std::vector::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,126 +205,10 @@ TEST(laser_geometry, projectLaser2) } catch (BuildScanException &ex) { - if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception - 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 ranges, intensities, min_angles, max_angles, angle_increments; - std::vector 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(); + // make sure it is not a false exception + if ((option.ang_max_ - option.ang_min_) / option.ang_increment_ > 0.0) { + FAIL(); + } } } } @@ -539,26 +216,13 @@ TEST(laser_geometry, transformLaserScanToPointCloud) 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 ranges, intensities, min_angles, max_angles, angle_increments; - std::vector increment_times, scan_times; - - rostest::Permuter permuter; + std::vector increment_times, scan_times; ranges.push_back(-1.0); ranges.push_back(1.0); @@ -567,21 +231,18 @@ 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); 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); @@ -589,58 +250,55 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) 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 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); - scan.header.frame_id = "laser_frame"; + { + // 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); @@ -649,7 +307,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud2) 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 ++; + valid_points ++; EXPECT_EQ(valid_points, cloud_out.width); uint32_t x_offset = 0; @@ -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 - FAIL(); + // 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(); }