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

Fixing the tests in laser_geometry -- now they break

Browse Source 
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35275 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Jeremy Leibs
2011-01-16 04:45:46 +00:00
parent b59d080add
commit ae9141d36d
3 changed files with 186 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

194
test/projection_test.cpp
View File

@@ -42,12 +42,16 @@
#define PROJECTION_TEST_RANGE_MIN (0.23)
#define PROJECTION_TEST_RANGE_MAX (40.0)
class BuildScanException { };
sensor_msgs::LaserScan build_constant_scan(double range, double intensity,
double ang_min, double ang_max, double ang_increment,
ros::Duration scan_time)
{
if ((ang_max - ang_min) / ang_increment < 0)
throw (std::runtime_error("cannnot build a scan with non convergent ends"));
if (((ang_max - ang_min) / ang_increment) < 0)
throw (BuildScanException());
sensor_msgs::LaserScan scan;
scan.header.stamp = ros::Time::now();
scan.header.frame_id = "laser_frame";
@@ -57,12 +61,13 @@ sensor_msgs::LaserScan build_constant_scan(double range, double intensity,
scan.scan_time = scan_time.toSec();
scan.range_min = PROJECTION_TEST_RANGE_MIN;
scan.range_max = PROJECTION_TEST_RANGE_MAX;
unsigned int i = 0;
uint32_t i = 0;
for(; ang_min + i * ang_increment < ang_max; i++)
{
scan.ranges.push_back(range);
scan.intensities.push_back(intensity);
}
scan.time_increment = scan_time.toSec()/(double)i;
return scan;
@@ -100,6 +105,8 @@ void test_getUnitVectors(double angle_min, double angle_max, double angle_increm
}
}
#if 0
TEST(laser_geometry, getUnitVectors)
{
double min_angle = -M_PI/2;
@@ -153,10 +160,10 @@ TEST(laser_geometry, getUnitVectors)
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 (std::runtime_error &ex)
catch (BuildScanException &ex)
{
if ((max_angle - max_angle) / angle_increment > 0.0)//make sure it is not a false exception
EXPECT_FALSE(ex.what());
if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
FAIL();
}
}
//else
@@ -231,12 +238,9 @@ TEST(laser_geometry, projectLaser)
{
try
{
//printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
// 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);
// WE NEVER GET HERE!!
FAIL();
sensor_msgs::PointCloud cloud_out;
projector.projectLaser(scan, cloud_out, -1.0, laser_geometry::channel_option::Index);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
@@ -257,7 +261,8 @@ TEST(laser_geometry, projectLaser)
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.points.size());
for (unsigned int i = 0; i < cloud_out.points.size(); i++)
@@ -271,14 +276,150 @@ TEST(laser_geometry, projectLaser)
EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
};
}
catch (std::runtime_error &ex)
catch (BuildScanException &ex)
{
if ((max_angle - max_angle) / angle_increment > 0.0) //make sure it is not a false exception
EXPECT_FALSE(ex.what());
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;
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::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
sensor_msgs::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, 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);
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 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;
}
for (unsigned int i = 0; i < cloud_out.width; 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, transformLaserScanToPointCloud)
{
@@ -388,16 +529,14 @@ TEST(laser_geometry, transformLaserScanToPointCloud)
EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
};
}
catch (std::runtime_error &ex)
catch (BuildScanException &ex)
{
if ((max_angle - max_angle) / angle_increment > 0.0) //make sure it is not a false exception
EXPECT_FALSE(ex.what());
if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
FAIL();
}
}
}
TEST(laser_geometry, transformLaserScanToPointCloud2)
{
@@ -491,14 +630,14 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
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);
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);
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;
@@ -516,6 +655,10 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
if (f->name == "ranges") ranges_offset = f->offset;
if (f->name == "stamps") stamps_offset = f->offset;
}
for (unsigned int i = 0; i < cloud_out.width; 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);
@@ -527,18 +670,17 @@ TEST(laser_geometry, transformLaserScanToPointCloud2)
*/
};
}
catch (std::runtime_error &ex)
catch (BuildScanException &ex)
{
if ((max_angle - max_angle) / angle_increment > 0.0) //make sure it is not a false exception
EXPECT_FALSE(ex.what());
if ((max_angle - min_angle) / angle_increment > 0.0)//make sure it is not a false exception
FAIL();
}
}
}
int main(int argc, char **argv){
ros::Time::init();
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}