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

patching laser projection to work with inverted mode #3041

Browse Source 
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@25120 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Tully Foote 2009-10-12 22:28:38 +00:00
parent 136cf0c3b8
commit 2175503845
2 changed files with 23 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/laser_geometry.cpp
View File

@ -152,13 +152,6 @@ namespace laser_geometry
const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(float angle_min, float angle_max, float angle_increment, unsigned int length) const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(float angle_min, float angle_max, float angle_increment, unsigned int length)
{ {
if (angle_min >= angle_max)
{
std::stringstream ss;
ss << "LaserProjection min angle " << angle_min << " greater than max angle "<< angle_max;
ROS_ERROR("%s", ss.str().c_str());
throw std::runtime_error(ss.str()); //This would result in a bad alloc anyway so throwing instead
}
//construct string for lookup in the map //construct string for lookup in the map
std::stringstream anglestring; std::stringstream anglestring;
anglestring <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length; anglestring <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length;

33
test/projection_test.cpp
View File

@ -46,6 +46,8 @@ sensor_msgs::LaserScan build_constant_scan(double range, double intensity,
double ang_min, double ang_max, double ang_increment, double ang_min, double ang_max, double ang_increment,
ros::Duration scan_time) ros::Duration scan_time)
{ {
if ((ang_max - ang_min) / ang_increment < 0)
throw (std::runtime_error("cannnot build a scan with non convergent ends"));
sensor_msgs::LaserScan scan; sensor_msgs::LaserScan scan;
scan.header.stamp = ros::Time::now(); scan.header.stamp = ros::Time::now();
scan.header.frame_id = "laser_frame"; scan.header.frame_id = "laser_frame";
@ -134,22 +136,31 @@ TEST(laser_geometry, getUnitVectors)
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/360); // half degree
angle_increments.push_back(M_PI/720); // quarter 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); permuter.addOptionSet(angle_increments, &angle_increment);
while (permuter.step()) while (permuter.step())
{ {
try if ((max_angle - min_angle) / angle_increment > 0.0)
{ {
test_getUnitVectors(min_angle, max_angle, angle_increment, round((max_angle - min_angle)/ angle_increment)); unsigned int length = round((max_angle - min_angle)/ angle_increment);
test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment); try
test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1); {
} test_getUnitVectors(min_angle, max_angle, angle_increment, length);
catch (std::runtime_error &ex) 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);
if (min_angle < max_angle) }
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()); EXPECT_FALSE(ex.what());
}
} }
//else
//printf("%f\n", (max_angle - min_angle) / angle_increment);
} }
} }
@ -205,6 +216,7 @@ TEST(laser_geometry, projectLaser)
max_angles.push_back(M_PI/8); max_angles.push_back(M_PI/8);
permuter.addOptionSet(max_angles, &max_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/180); // one degree
angle_increments.push_back(M_PI/360); // half 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/720); // quarter degree
@ -258,7 +270,7 @@ TEST(laser_geometry, projectLaser)
} }
catch (std::runtime_error &ex) catch (std::runtime_error &ex)
{ {
if (min_angle < max_angle) if ((max_angle - max_angle) / angle_increment > 0.0) //make sure it is not a false exception
EXPECT_FALSE(ex.what()); EXPECT_FALSE(ex.what());
} }
} }
@ -319,6 +331,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud)
permuter.addOptionSet(min_angles, &min_angle); permuter.addOptionSet(min_angles, &min_angle);
permuter.addOptionSet(max_angles, &max_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/180); // one degree
angle_increments.push_back(M_PI/360); // half 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/720); // quarter degree
@ -373,7 +386,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud)
} }
catch (std::runtime_error &ex) catch (std::runtime_error &ex)
{ {
if (min_angle < max_angle) if ((max_angle - max_angle) / angle_increment > 0.0) //make sure it is not a false exception
EXPECT_FALSE(ex.what()); EXPECT_FALSE(ex.what());
} }
} }