diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 9e40738..c4826e4 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -152,13 +152,6 @@ namespace laser_geometry const boost::numeric::ublas::matrix& 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 std::stringstream anglestring; anglestring < 0.0) { - test_getUnitVectors(min_angle, max_angle, angle_increment, round((max_angle - min_angle)/ angle_increment)); - 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) - { - if (min_angle < max_angle) + unsigned int length = round((max_angle - min_angle)/ angle_increment); + try + { + test_getUnitVectors(min_angle, max_angle, angle_increment, length); + 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) + { + if ((max_angle - max_angle) / angle_increment > 0.0)//make sure it is not a false exception 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); 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 @@ -258,7 +270,7 @@ TEST(laser_geometry, projectLaser) } 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()); } } @@ -319,6 +331,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud) 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 @@ -373,7 +386,7 @@ TEST(laser_geometry, transformLaserScanToPointCloud) } 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()); } }