Fixing the tests in laser_geometry -- now they break
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
parent
b59d080add
commit
ae9141d36d
|
|
@ -68,7 +68,7 @@ namespace laser_geometry
|
|||
Index = 0x02, //!< Enable "index" channel
|
||||
Distance = 0x04, //!< Enable "distances" channel
|
||||
Timestamp = 0x08, //!< Enable "stamps" channel
|
||||
Viewpoint = 0x16, //!< Enable "viewpoint" channel
|
||||
Viewpoint = 0x10, //!< Enable "viewpoint" channel
|
||||
Default = (Intensity | Index) //!< Enable "intensities" and "index" channels
|
||||
};
|
||||
}
|
||||
|
|
@ -181,7 +181,7 @@ namespace laser_geometry
|
|||
double range_cutoff,
|
||||
int channel_options = channel_option::Default)
|
||||
{
|
||||
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options, false);
|
||||
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options);
|
||||
}
|
||||
|
||||
//! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame
|
||||
|
|
@ -209,7 +209,7 @@ namespace laser_geometry
|
|||
tf::Transformer& tf,
|
||||
int channel_options = channel_option::Default)
|
||||
{
|
||||
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options, false);
|
||||
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options);
|
||||
}
|
||||
|
||||
//! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame
|
||||
|
|
@ -277,8 +277,7 @@ namespace laser_geometry
|
|||
const sensor_msgs::LaserScan& scan_in,
|
||||
tf::Transformer & tf,
|
||||
double range_cutoff,
|
||||
int channel_options,
|
||||
bool preservative);
|
||||
int channel_options);
|
||||
|
||||
//! Internal hidden representation of transformLaserScanToPointCloud2
|
||||
void transformLaserScanToPointCloud_ (const std::string &target_frame,
|
||||
|
|
@ -292,7 +291,7 @@ namespace laser_geometry
|
|||
std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
|
||||
float angle_min_;
|
||||
float angle_max_;
|
||||
Eigen3::ArrayXXd co_sine_map_;
|
||||
// Eigen3::ArrayXXd co_sine_map_;
|
||||
boost::mutex guv_mutex_;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -189,7 +189,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
|
||||
void
|
||||
LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in,
|
||||
tf::Transformer& tf, double range_cutoff, int mask, bool preservative)
|
||||
tf::Transformer& tf, double range_cutoff, int mask)
|
||||
{
|
||||
cloud_out.header = scan_in.header;
|
||||
|
||||
|
|
@ -207,7 +207,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
|
||||
pointIn.frame_id_ = scan_in.header.frame_id;
|
||||
|
||||
projectLaser_ (scan_in, cloud_out, range_cutoff, true, mask);
|
||||
projectLaser_ (scan_in, cloud_out, range_cutoff, false, mask);
|
||||
|
||||
cloud_out.header.frame_id = target_frame;
|
||||
|
||||
|
|
@ -234,7 +234,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
}
|
||||
|
||||
//check just in case
|
||||
ROS_ASSERT(index_channel_idx > 0);
|
||||
ROS_ASSERT(index_channel_idx >= 0);
|
||||
|
||||
for(unsigned int i = 0; i < cloud_out.points.size(); ++i)
|
||||
{
|
||||
|
|
@ -280,8 +280,8 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
int channel_options)
|
||||
{
|
||||
size_t n_pts = scan_in.ranges.size ();
|
||||
|
||||
Eigen3::ArrayXXd ranges (n_pts, 2), output (n_pts, 2);
|
||||
Eigen3::ArrayXXd ranges (n_pts, 2);
|
||||
Eigen3::ArrayXXd output (n_pts, 2);
|
||||
|
||||
// Get the ranges into Eigen format
|
||||
for (size_t i = 0; i < n_pts; ++i)
|
||||
|
|
@ -291,10 +291,13 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
}
|
||||
|
||||
// Check if our existing co_sine_map is valid
|
||||
#if 0
|
||||
if (co_sine_map_.rows () != (int)n_pts || angle_min_ != scan_in.angle_min || angle_max_ != scan_in.angle_max )
|
||||
{
|
||||
#endif
|
||||
ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one.");
|
||||
co_sine_map_ = Eigen3::ArrayXXd (n_pts, 2);
|
||||
// co_sine_map_ = Eigen3::ArrayXXd (n_pts, 2);
|
||||
Eigen3::ArrayXXd co_sine_map_ (n_pts, 2);
|
||||
angle_min_ = scan_in.angle_min;
|
||||
angle_max_ = scan_in.angle_max;
|
||||
// Spherical->Cartesian projection
|
||||
|
|
@ -303,7 +306,10 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
co_sine_map_ (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment);
|
||||
co_sine_map_ (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment);
|
||||
}
|
||||
#if 0
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
output = ranges * co_sine_map_;
|
||||
|
||||
|
|
@ -407,7 +413,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
|
|||
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
|
||||
|
||||
//check to see if we want to keep the point
|
||||
if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
|
||||
if (scan_in.ranges[i] <= range_cutoff && scan_in.ranges[i] >= scan_in.range_min)
|
||||
{
|
||||
|
||||
float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step];
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user