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

Starting to add unit tests for PointCloud2

Browse Source 
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35266 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Jeremy Leibs 2011-01-15 02:50:28 +00:00
parent 92e6c455c6
commit 3a00224b10
3 changed files with 149 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
include/laser_geometry/laser_geometry.h
View File

@ -63,6 +63,7 @@ namespace laser_geometry
*/ */
enum ChannelOption enum ChannelOption
{ {
None = 0x00, //!< Enable no channels
Intensity = 0x01, //!< Enable "intensities" channel Intensity = 0x01, //!< Enable "intensities" channel
Index = 0x02, //!< Enable "index" channel Index = 0x02, //!< Enable "index" channel
Distance = 0x04, //!< Enable "distances" channel Distance = 0x04, //!< Enable "distances" channel
@ -232,14 +233,14 @@ namespace laser_geometry
* channel_option::Intensity, channel_option::Index, * channel_option::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp. * 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, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
double range_cutoff = -1.0, double range_cutoff = -1.0,
int channel_options = channel_option::Default) 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: protected:
@ -280,7 +281,7 @@ namespace laser_geometry
bool preservative); bool preservative);
//! Internal hidden representation of transformLaserScanToPointCloud2 //! 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, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,

2
src/laser_geometry.cpp
View File

@ -483,7 +483,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.data.resize (cloud_out.row_step * cloud_out.height); 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, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,

144
test/projection_test.cpp
View File

@ -275,6 +275,7 @@ TEST(laser_geometry, projectLaser)
} }
} }
} }
TEST(laser_geometry, transformLaserScanToPointCloud) 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<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::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<sensor_msgs::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 == "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){ int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();