From 2175503845a8989096601aba0dff2084439db507 Mon Sep 17 00:00:00 2001
From: Tully Foote <tfoote@willowgarage.com>
Date: Mon, 12 Oct 2009 22:28:38 +0000
Subject: [PATCH] patching laser projection to work with inverted mode #3041

git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@25120 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
---
 src/laser_geometry.cpp   |  7 -------
 test/projection_test.cpp | 33 +++++++++++++++++++++++----------
 2 files changed, 23 insertions(+), 17 deletions(-)

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<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
     std::stringstream anglestring;
     anglestring <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length;
diff --git a/test/projection_test.cpp b/test/projection_test.cpp
index 10e9b57..e5be16d 100644
--- a/test/projection_test.cpp
+++ b/test/projection_test.cpp
@@ -46,6 +46,8 @@ 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"));
   sensor_msgs::LaserScan scan;
   scan.header.stamp = ros::Time::now();
   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/360); // half 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);
 
 
   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));
-      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());
     }
   }