From ec9c3f236c370fd336695b14162c25e42233ee19 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 27 Jun 2021 19:37:07 -0400
Subject: [PATCH] 0.20.13: Refacoring MsgConversion by using
 laserScanFromPointCloud() from rtabmap library.

---
 CMakeLists.txt        |   2 +-
 package.xml           |   2 +-
 src/MsgConversion.cpp | 102 +-----------------------------------------
 3 files changed, 4 insertions(+), 102 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0bb10185..ae31d5ea 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -31,7 +31,7 @@ find_package(find_object_2d)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.20.11 REQUIRED)
+find_package(RTABMap 0.20.13 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 
diff --git a/package.xml b/package.xml
index 8038798d..50d6cbae 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.20.11</version>
+  <version>0.20.13</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>
diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp
index 49d2bda8..e8a83838 100644
--- a/src/MsgConversion.cpp
+++ b/src/MsgConversion.cpp
@@ -2207,39 +2207,6 @@ bool convertScan3dMsg(
 	UASSERT_MSG(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height,
 			uFormat("data=%d row_step=%d height=%d", scan3dMsg.data.size(), scan3dMsg.row_step, scan3dMsg.height).c_str());
 
-	bool hasNormals = false;
-	bool hasColors = false;
-	bool hasIntensity = false;
-	for(unsigned int i=0; i<scan3dMsg.fields.size(); ++i)
-	{
-		if(scan3dMsg.fields[i].name.compare("normal_x") == 0)
-		{
-			hasNormals = true;
-		}
-		if(scan3dMsg.fields[i].name.compare("rgb") == 0 || scan3dMsg.fields[i].name.compare("rgba") == 0)
-		{
-			hasColors = true;
-		}
-		if(scan3dMsg.fields[i].name.compare("intensity") == 0)
-		{
-			if(scan3dMsg.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
-			{
-				hasIntensity = true;
-			}
-			else
-			{
-				static bool warningShown = false;
-				if(!warningShown)
-				{
-					ROS_WARN("The input scan cloud has an \"intensity\" field "
-							"but the datatype (%d) is not supported. Intensity will be ignored. "
-							"This message is only shown once.", scan3dMsg.fields[i].datatype);
-					warningShown = true;
-				}
-			}
-		}
-	}
-
 	rtabmap::Transform scanLocalTransform = getTransform(frameId, scan3dMsg.header.frame_id, scan3dMsg.header.stamp, listener, waitForTransform);
 	if(scanLocalTransform.isNull())
 	{
@@ -2267,73 +2234,8 @@ bool convertScan3dMsg(
 			scanLocalTransform = sensorT * scanLocalTransform;
 		}
 	}
-
-	if(hasNormals)
-	{
-		if(hasColors)
-		{
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-			pcl::fromROSMsg(scan3dMsg, *pclScan);
-			if(!pclScan->is_dense)
-			{
-				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
-			}
-			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
-		}
-		else if(hasIntensity)
-		{
-			pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
-			pcl::fromROSMsg(scan3dMsg, *pclScan);
-			if(!pclScan->is_dense)
-			{
-				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
-			}
-			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
-		}
-		else
-		{
-			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
-			pcl::fromROSMsg(scan3dMsg, *pclScan);
-			if(!pclScan->is_dense)
-			{
-				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
-			}
-			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
-		}
-	}
-	else
-	{
-		if(hasColors)
-		{
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGB>);
-			pcl::fromROSMsg(scan3dMsg, *pclScan);
-			if(!pclScan->is_dense)
-			{
-				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
-			}
-			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
-		}
-		else if(hasIntensity)
-		{
-			pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
-			pcl::fromROSMsg(scan3dMsg, *pclScan);
-			if(!pclScan->is_dense)
-			{
-				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
-			}
-			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
-		}
-		else
-		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::fromROSMsg(scan3dMsg, *pclScan);
-			if(!pclScan->is_dense)
-			{
-				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
-			}
-			scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
-		}
-	}
+	scan = rtabmap::util3d::laserScanFromPointCloud(scan3dMsg);
+	scan = rtabmap::LaserScan(scan, maxPoints, maxRange, scanLocalTransform);
 	return true;
 }