diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h index dc7392b..2e8be56 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.h @@ -63,6 +63,7 @@ namespace laser_geometry */ enum ChannelOption { + None = 0x00, //!< Enable no channels Intensity = 0x01, //!< Enable "intensities" channel Index = 0x02, //!< Enable "index" channel Distance = 0x04, //!< Enable "distances" channel @@ -232,14 +233,14 @@ namespace laser_geometry * channel_option::Intensity, channel_option::Index, * channel_option::Distance, channel_option::Timestamp. */ - void transformLaserScanToPointCloud2(const std::string &target_frame, + void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, double range_cutoff = -1.0, int channel_options = channel_option::Default) { - transformLaserScanToPointCloud2_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); + transformLaserScanToPointCloud_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); } protected: @@ -280,7 +281,7 @@ namespace laser_geometry bool preservative); //! Internal hidden representation of transformLaserScanToPointCloud2 - void transformLaserScanToPointCloud2_ (const std::string &target_frame, + void transformLaserScanToPointCloud_ (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index bdd05ec..5c0787c 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -483,7 +483,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do cloud_out.data.resize (cloud_out.row_step * cloud_out.height); } - void LaserProjection::transformLaserScanToPointCloud2_ (const std::string &target_frame, + void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, diff --git a/test/projection_test.cpp b/test/projection_test.cpp index 3215131..77c1cb5 100644 --- a/test/projection_test.cpp +++ b/test/projection_test.cpp @@ -275,6 +275,7 @@ TEST(laser_geometry, projectLaser) } } } + TEST(laser_geometry, transformLaserScanToPointCloud) { @@ -391,6 +392,149 @@ TEST(laser_geometry, transformLaserScanToPointCloud) } } } + + + +TEST(laser_geometry, transformLaserScanToPointCloud2) +{ + + 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::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time); + scan.header.frame_id = "laser_frame"; + + sensor_msgs::PointCloud2 cloud_out; + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::None); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)1); + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)2); + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)2); + + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)3); + projector.transformLaserScanToPointCloud(scan.header.frame_id, scan, cloud_out, tf, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index); + EXPECT_EQ(cloud_out.fields.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); + EXPECT_EQ(cloud_out.fields.size(), (unsigned int)4); + + 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.fields.size(), (unsigned int)5); + + 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); + + for (unsigned int i = 0; i < cloud_out.width; i++) + { + 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 ranges_offset = 0; + uint32_t stamps_offset = 0; + 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; + if (f->name == "intensity") intensity_offset = f->offset; + if (f->name == "index") index_offset = f->offset; + if (f->name == "ranges") ranges_offset = f->offset; + if (f->name == "stamps") stamps_offset = f->offset; + } + /* + 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.fields[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order + EXPECT_NEAR(cloud_out.fields[1].values[i], i, tolerance);//index + EXPECT_NEAR(cloud_out.fields[2].values[i], scan.ranges[i], tolerance);//ranges + EXPECT_NEAR(cloud_out.fields[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps + */ + }; + } + catch (std::runtime_error &ex) + { + if ((max_angle - max_angle) / angle_increment > 0.0) //make sure it is not a false exception + EXPECT_FALSE(ex.what()); + } + } +} + + + + int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS();