From a0bd43b9cf154f742de5b493e48a9d09a238bad4 Mon Sep 17 00:00:00 2001
From: Mathieu Labbe <matlabbe@gmail.com>
Date: Fri, 27 Mar 2015 18:20:06 -0400
Subject: [PATCH] added "scan_inf_is_valid" parameter

---
 src/CoreWrapper.cpp | 48 +++++++++++++++++++++++++++++++++++++++++++--
 src/CoreWrapper.h   |  1 +
 2 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 5a19fe3f..3ef1ad44 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -90,6 +90,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
 		waitForTransform_(false),
 		useActionForGoal_(false),
+		scanInfIsValid_(false),
 		cloudDecimation_(4),
 		cloudMaxDepth_(4.0), // meters
 		cloudVoxelSize_(0.05), // meters
@@ -162,6 +163,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 	pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
 
+	// scan stuff
+	pnh.param("scan_inf_is_valid", scanInfIsValid_, scanInfIsValid_);
+
 	// cloud map stuff
 	pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
 	pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
@@ -706,7 +710,27 @@ void CoreWrapper::commonDepthCallback(
 		//transform in frameId_ frame
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+
+		if(scanInfIsValid_)
+		{
+			// Filter positive infinities ("Inf"s) to max_range.
+			float epsilon = 0.0001; // a tenth of a millimeter
+			sensor_msgs::LaserScan message = *scanMsg;
+			for( size_t i = 0; i < message.ranges.size(); i++ )
+			{
+				float range = message.ranges[ i ];
+				if( !std::isfinite( range ) && range > 0 )
+				{
+					message.ranges[ i ] = message.range_max - epsilon;
+				}
+			}
+			projection.transformLaserScanToPointCloud(frameId_, message, scanOut, tfListener_);
+		}
+		else
+		{
+			projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+		}
+
 		pcl::PointCloud<pcl::PointXYZ> pclScan;
 		pcl::fromROSMsg(scanOut, pclScan);
 		scan = util3d::laserScanFromPointCloud(pclScan);
@@ -787,7 +811,27 @@ void CoreWrapper::commonStereoCallback(
 		//transform in frameId_ frame
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+
+		if(scanInfIsValid_)
+		{
+			// Filter positive infinities ("Inf"s) to max_range.
+			float epsilon = 0.0001; // a tenth of a millimeter
+			sensor_msgs::LaserScan message = *scanMsg;
+			for( size_t i = 0; i < message.ranges.size(); i++ )
+			{
+				float range = message.ranges[ i ];
+				if( !std::isfinite( range ) && range > 0 )
+				{
+					message.ranges[ i ] = message.range_max - epsilon;
+				}
+			}
+			projection.transformLaserScanToPointCloud(frameId_, message, scanOut, tfListener_);
+		}
+		else
+		{
+			projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+		}
+
 		pcl::PointCloud<pcl::PointXYZ> pclScan;
 		pcl::fromROSMsg(scanOut, pclScan);
 		scan = util3d::laserScanFromPointCloud(pclScan);
diff --git a/src/CoreWrapper.h b/src/CoreWrapper.h
index 41be97c4..591c0574 100644
--- a/src/CoreWrapper.h
+++ b/src/CoreWrapper.h
@@ -237,6 +237,7 @@ private:
 	bool useActionForGoal_;
 
 	// mapping stuff
+	bool scanInfIsValid_;
 	int cloudDecimation_;
 	double cloudMaxDepth_;
 	double cloudVoxelSize_;