point_cloud_assembler: fixed output cloud format when input is organized clouds (#560)

src/MsgConversion.cpp

@@ -2149,7 +2149,8 @@ bool convertScan3dMsg(
 		int maxPoints,
 		float maxRange)
 {
-	UASSERT(scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height);
+	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;

src/nodelets/icp_odometry.cpp

@@ -449,7 +449,8 @@ private:
 
 	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
 	{
-		UASSERT(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height);
+		UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height,
+				uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str());
 		
 		if(scanReceived_)
 		{

src/nodelets/obstacles_detection.cpp

@@ -292,6 +292,9 @@ private:
 			}
 		}
 
+		UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+				uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
 		pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::fromROSMsg(*cloudMsg, *inputCloud);
 		if(inputCloud->isOrganized())

src/nodelets/point_cloud_assembler.cpp

@@ -229,6 +229,9 @@ private:
 	{
 		if(cloudPub_.getNumSubscribers())
 		{
+			UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+					uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
 			if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
 			{
 				cloudsSkipped_ = 0;
@@ -268,6 +271,12 @@ private:
 					pcl_conversions::toPCL(output, *newCloud);
 				}
 
+				if(!newCloud->is_dense)
+				{
+					// remove nans
+					newCloud = rtabmap::util3d::removeNaNFromPointCloud(newCloud);
+				}
+
 				clouds_.push_back(newCloud);
 
 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
@@ -299,6 +308,8 @@ private:
 #else
 							pcl::concatenatePointCloud(*assembled, *(*iter), *assembledTmp);
 #endif
+							//Make sure row_step is the sum of both
+							assembledTmp->row_step = assembled->row_step + (*iter)->row_step;
 							assembled = assembledTmp;
 						}
 					}

src/nodelets/pointcloud_to_depthimage.cpp

@@ -198,6 +198,9 @@ private:
 				}
 			}
 
+			UASSERT_MSG(pointCloud2Msg->data.size() == pointCloud2Msg->row_step*pointCloud2Msg->height,
+					uFormat("data=%d row_step=%d height=%d", pointCloud2Msg->data.size(), pointCloud2Msg->row_step, pointCloud2Msg->height).c_str());
+
 			pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
 			pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
 

src/nodelets/rgbdicp_odometry.cpp

@@ -343,7 +343,9 @@ private:
 				}
 				else if(cloudMsg.get() != 0)
 				{
-					UASSERT(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height);
+					UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height,
+							uFormat("data=%d row_step=%d height=%d", cloudMsg->data.size(), cloudMsg->row_step, cloudMsg->height).c_str());
+
 
 					bool containNormals = false;
 					if(scanVoxelSize_ == 0.0f)