Merge pull request #2 from introlab/master

Merge master to fork
2026-10-04 00:37:46 +08:00 · 2019-11-27 10:29:40 -05:00
parent 3e0c501c32 954836c58b
commit 1952f8496e
8 changed files with 242 additions and 77 deletions
@@ -1,47 +1,107 @@
 sudo: true
-dist: trusty
 language: cpp

 compiler:
  - gcc

-addons:
-  apt:
-    packages:
-      - cmake
-      - libopencv-dev
-      - libqt4-dev
-      - libsqlite3-dev
+matrix:
+  include:

-install:
-    - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
-    - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-    - sudo apt-get update
-    - sudo apt-get install dpkg
-    - sudo apt-get -y install ros-indigo-ros-base libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev ros-indigo-costmap-2d ros-indigo-octomap-msgs ros-indigo-rviz ros-indigo-cv-bridge ros-indigo-move-base-msgs ros-indigo-costmap-2d ros-indigo-image-geometry ros-indigo-message-filters ros-indigo-image-transport ros-indigo-eigen-conversions ros-indigo-stereo-msgs ros-indigo-nav-msgs ros-indigo-sensor-msgs ros-indigo-tf-conversions ros-indigo-laser-geometry ros-indigo-pcl-conversions ros-indigo-pcl-ros ros-indigo-dynamic-reconfigure ros-indigo-nodelet
+    - dist: trusty
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get install dpkg
+        - sudo apt-get -y install ros-indigo-rtabmap-ros
+        - sudo apt-get -y remove ros-indigo-rtabmap

-script: 
-    - source /opt/ros/indigo/setup.bash
-    - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
-    - cd ..
-    - mkdir -p catkin_ws/src
-    - cd catkin_ws/src
-    - catkin_init_workspace
-    - cd ..
-    - catkin_make
-    - cd ..
-    - mv rtabmap_ros catkin_ws/src/.
-    - git clone https://github.com/introlab/rtabmap.git
-    - cd rtabmap
-    - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
-    - mkdir -p build && cd build
-    - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. 
-    - make
-    - make install
-    - cd ../../catkin_ws
-    - source devel/setup.bash
-    - catkin_make
-    - catkin_make install
+      script: 
+        - source /opt/ros/indigo/setup.bash
+        - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
+        - cd ..
+        - mkdir -p catkin_ws/src
+        - cd catkin_ws/src
+        - catkin_init_workspace
+        - cd ..
+        - catkin_make
+        - cd ..
+        - mv rtabmap_ros catkin_ws/src/.
+        - git clone https://github.com/introlab/rtabmap.git
+        - cd rtabmap
+        - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
+        - mkdir -p build && cd build
+        - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. 
+        - make
+        - make install
+        - cd ../../catkin_ws
+        - source devel/setup.bash
+        - catkin_make
+        - catkin_make install
+
+    - dist: xenial
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get install dpkg
+        - sudo apt-get -y install ros-kinetic-rtabmap-ros
+        - sudo apt-get -y remove ros-kinetic-rtabmap
+
+      script: 
+        - source /opt/ros/kinetic/setup.bash
+        - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
+        - cd ..
+        - mkdir -p catkin_ws/src
+        - cd catkin_ws/src
+        - catkin_init_workspace
+        - cd ..
+        - catkin_make
+        - cd ..
+        - mv rtabmap_ros catkin_ws/src/.
+        - git clone https://github.com/introlab/rtabmap.git
+        - cd rtabmap
+        - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
+        - mkdir -p build && cd build
+        - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. 
+        - make
+        - make install
+        - cd ../../catkin_ws
+        - source devel/setup.bash
+        - catkin_make
+        - catkin_make install        
+
+    - dist: bionic
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get install dpkg
+        - sudo apt-get -y install ros-melodic-rtabmap-ros
+        - sudo apt-get -y remove ros-melodic-rtabmap
+
+      script: 
+        - source /opt/ros/melodic/setup.bash
+        - export PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages
+        - cd ..
+        - mkdir -p catkin_ws/src
+        - cd catkin_ws/src
+        - catkin_init_workspace
+        - cd ..
+        - catkin_make
+        - cd ..
+        - mv rtabmap_ros catkin_ws/src/.
+        - git clone https://github.com/introlab/rtabmap.git
+        - cd rtabmap
+        - if [ "$TRAVIS_BRANCH" = "devel" ]; then git checkout devel; fi
+        - mkdir -p build && cd build
+        - cmake -DCMAKE_INSTALL_PREFIX=~/build/introlab/catkin_ws/devel .. 
+        - make
+        - make install
+        - cd ../../catkin_ws
+        - source devel/setup.bash
+        - catkin_make
+        - catkin_make install

 notifications:
  email:
@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/Goal.h"
 #include "rtabmap_ros/GetPlan.h"
 #include "rtabmap_ros/CommonDataSubscriber.h"
+#include "rtabmap_ros/OdomInfo.h"

 #include "MapsManager.h"

@@ -144,6 +145,7 @@ private:
 #endif
 	void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
 	void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
+	void interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2);

 	void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);

@@ -320,8 +322,13 @@ private:
 	std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
 	ros::Subscriber imuSub_;
 	std::map<double, rtabmap::Transform> imus_;
+
 	ros::Subscriber interOdomSub_;
-	std::list<nav_msgs::Odometry> interOdoms_;
+	std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> > interOdoms_;
+	message_filters::Subscriber<nav_msgs::Odometry> interOdomSyncSub_;
+	message_filters::Subscriber<rtabmap_ros::OdomInfo> interOdomInfoSyncSub_;
+	typedef message_filters::sync_policies::ExactTime<nav_msgs::Odometry, rtabmap_ros::OdomInfo> MyExactInterOdomSyncPolicy;
+	message_filters::Synchronizer<MyExactInterOdomSyncPolicy> * interOdomSync_;

 	bool stereoToDepth_;
 	bool odomSensorSync_;
@@ -152,6 +152,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg);
 void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);

+std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);

@@ -112,6 +112,7 @@ CoreWrapper::CoreWrapper() :
 		transformThread_(0),
 		tfThreadRunning_(false),
 		stereoToDepth_(false),
+		interOdomSync_(0),
 		odomSensorSync_(false),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
@@ -518,8 +519,22 @@ void CoreWrapper::onInit()
 			NODELET_INFO("Create intermediate nodes");
 			if(rate_ == 0.0f)
 			{
-				NODELET_INFO("Subscribe to inter odom messges");
-				interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
+				bool interOdomInfo = false;
+				pnh.getParam("subscribe_inter_odom_info", interOdomInfo);
+				if(interOdomInfo)
+				{
+					NODELET_INFO("Subscribe to inter odom + info messages");
+					interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(queueSize_), interOdomSyncSub_, interOdomInfoSyncSub_);
+					interOdomSync_->registerCallback(boost::bind(&CoreWrapper::interOdomInfoCallback, this, _1, _2));
+					interOdomSyncSub_.subscribe(nh, "inter_odom", 1);
+					interOdomInfoSyncSub_.subscribe(nh, "inter_odom_info", 1);
+				}
+				else
+				{
+					NODELET_INFO("Subscribe to inter odom messages");
+					interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
+				}
+
 			}
 		}
 	}
@@ -742,6 +757,7 @@ CoreWrapper::~CoreWrapper()
 	rtabmap_.close();
 	printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));

+	delete interOdomSync_;
 	delete mbClient_;
 }

@@ -1638,24 +1654,24 @@ void CoreWrapper::process(
 	if(rtabmap_.isIDsGenerated() || data.id() > 0)
 	{
 		// Add intermediate nodes?
-		for(std::list<nav_msgs::Odometry>::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
+		for(std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
 		{
-			if(iter->header.stamp < lastPoseStamp_)
+			if(iter->first.header.stamp < lastPoseStamp_)
 			{
-				Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->pose.pose);
+				Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->first.pose.pose);
 				if(!interOdom.isNull())
 				{
 					cv::Mat covariance;
-					double variance = iter->twist.covariance[0];
+					double variance = iter->first.twist.covariance[0];
 					if(variance == BAD_COVARIANCE || variance <= 0.0f)
 					{
 						//use the one of the pose
-						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->pose.covariance.data()).clone();
+						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.pose.covariance.data()).clone();
 						covariance /= 2.0;
 					}
 					else
 					{
-						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->twist.covariance.data()).clone();
+						covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.twist.covariance.data()).clone();
 					}
 					if(!uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
 					{
@@ -1684,18 +1700,40 @@ void CoreWrapper::process(
 							Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 							0,
 							cv::Size(1,2));
-					SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->header.stamp));
+					SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
 					Transform gt;
 					if(!groundTruthFrameId_.empty())
 					{
-						gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
+						gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->first.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
 					}
 					interData.setGroundTruth(gt);
-					rtabmap_.process(interData, interOdom, covariance);
+
+					std::map<std::string, float> externalStats;
+					std::vector<float> odomVelocity;
+					if(iter->second.timeEstimation != 0.0f)
+					{
+						OdometryInfo info = odomInfoFromROS(iter->second);
+						externalStats = rtabmap_ros::odomInfoToStatistics(info);
+
+						if(info.interval>0.0)
+						{
+							odomVelocity.resize(6);
+							float x,y,z,roll,pitch,yaw;
+							info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+							odomVelocity[0] = x/info.interval;
+							odomVelocity[1] = y/info.interval;
+							odomVelocity[2] = z/info.interval;
+							odomVelocity[3] = roll/info.interval;
+							odomVelocity[4] = pitch/info.interval;
+							odomVelocity[5] = yaw/info.interval;
+						}
+					}
+
+					rtabmap_.process(interData, interOdom, covariance, odomVelocity, externalStats);
 				}
 				interOdoms_.erase(iter++);
 			}
-			else if(iter->header.stamp == lastPoseStamp_)
+			else if(iter->first.header.stamp == lastPoseStamp_)
 			{
 				interOdoms_.erase(iter++);
 				break;
@@ -1822,23 +1860,7 @@ void CoreWrapper::process(
 		std::vector<float> odomVelocity;
 		if(odomInfo.timeEstimation != 0.0f)
 		{
-			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
-			externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
-			externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
-			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
-			externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
-			float speed = 0.0f;
-			if(odomInfo.interval>0.0)
-				speed = odomInfo.transform.x()/odomInfo.interval*3.6;
-			externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
-			externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
-			externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
-			externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
-			externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
-			externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
-			externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
-			externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
-			externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", odomInfo.memoryUsage));
+			externalStats = rtabmap_ros::odomInfoToStatistics(odomInfo);

 			if(odomInfo.interval>0.0)
 			{
@@ -2181,7 +2203,15 @@ void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
 {
 	if(!paused_)
 	{
-		interOdoms_.push_back(*msg);
+		interOdoms_.push_back(std::make_pair(*msg, rtabmap_ros::OdomInfo()));
+	}
+}
+
+void CoreWrapper::interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2)
+{
+	if(!paused_)
+	{
+		interOdoms_.push_back(std::make_pair(*msg1, *msg2));
 	}
 }

@@ -1110,6 +1110,48 @@ void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	transformToPoseMsg(signature.getGroundTruthPose(), msg.groundTruthPose);
 }

+std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info)
+{
+	std::map<std::string, float> stats;
+
+	stats.insert(std::make_pair("Odometry/TimeRegistration/ms", info.reg.totalTime*1000.0f));
+	stats.insert(std::make_pair("Odometry/RAM_usage/MB", info.memoryUsage));
+
+	// Based on rtabmap/MainWindow.cpp
+	stats.insert(std::make_pair("Odometry/Features/", info.features));
+	stats.insert(std::make_pair("Odometry/Matches/", info.reg.matches));
+	stats.insert(std::make_pair("Odometry/MatchesRatio/", info.features<=0?0.0f:float(info.reg.inliers)/float(info.features)));
+	stats.insert(std::make_pair("Odometry/Inliers/", info.reg.inliers));
+	stats.insert(std::make_pair("Odometry/InliersMeanDistance/m", info.reg.inliersMeanDistance));
+	stats.insert(std::make_pair("Odometry/InliersDistribution/", info.reg.inliersDistribution));
+	stats.insert(std::make_pair("Odometry/InliersRatio/", info.reg.inliers));
+	stats.insert(std::make_pair("Odometry/ICPInliersRatio/", info.reg.icpInliersRatio));
+	stats.insert(std::make_pair("Odometry/ICPRotation/rad", info.reg.icpRotation));
+	stats.insert(std::make_pair("Odometry/ICPTranslation/m", info.reg.icpTranslation));
+	stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", info.reg.icpStructuralComplexity));
+	stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
+	stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
+	stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
+	stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at<double>(5,5)));
+	stats.insert(std::make_pair("Odometry/TimeEstimation/ms", info.timeEstimation*1000.0f));
+	stats.insert(std::make_pair("Odometry/TimeFiltering/ms", info.timeParticleFiltering*1000.0f));
+	stats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize));
+	stats.insert(std::make_pair("Odometry/LocalScanMapSize/", info.localScanMapSize));
+	stats.insert(std::make_pair("Odometry/LocalKeyFrames/", info.localKeyFrames));
+	stats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers));
+	stats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints));
+	stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", info.localBundleTime*1000.0f));
+	stats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded?1.0f:0.0f));
+	stats.insert(std::make_pair("Odometry/Interval/ms", (float)info.interval));
+	float speed = 0.0f;
+	if(info.interval>0.0)
+		speed = info.transform.x()/info.interval*3.6;
+	stats.insert(std::make_pair("Odometry/Speed/kph", speed));
+	stats.insert(std::make_pair("Odometry/Distance/m", info.distanceTravelled));
+
+	return stats;
+}
+
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 {
 	rtabmap::OdometryInfo info;
@@ -52,7 +52,8 @@ public:
 	ObstaclesDetection() :
 		frameId_("base_link"),
 		waitForTransform_(false),
-		mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection())
+		mapFrameProjection_(rtabmap::Parameters::defaultGridMapFrameProjection()),
+		warned_(false)
 	{}

 	virtual ~ObstaclesDetection()
@@ -109,6 +110,9 @@ private:
 		ros::NodeHandle & nh = getNodeHandle();
 		ros::NodeHandle & pnh = getPrivateNodeHandle();

+		ULogger::setType(ULogger::kTypeConsole);
+		ULogger::setLevel(ULogger::kWarning);
+
 		int queueSize = 10;
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("frame_id", frameId_, frameId_);
@@ -295,6 +299,17 @@ private:
 			std::vector<int> indices;
 			pcl::removeNaNFromPointCloud(*inputCloud, *inputCloud, indices);
 		}
+		else if(!inputCloud->is_dense && inputCloud->height == 1)
+		{
+			if(!warned_)
+			{
+				NODELET_WARN("Detected possible wrong format of point cloud \"%s\", it is "
+						"indicated that it is not dense, but there is only one row. "
+						"Assuming it is dense... This message will only appear once.", cloudSub_.getTopic().c_str());
+				warned_ = true;
+			}
+			inputCloud->is_dense = true;
+		}

 		//Common variables for all strategies
 		pcl::IndicesPtr ground, obstacles;
@@ -425,6 +440,7 @@ private:

 	rtabmap::OccupancyGrid grid_;
 	bool mapFrameProjection_;
+	bool warned_;

 	tf::TransformListener tfListener_;

@@ -107,7 +107,7 @@ private:
 		pnh.param("range_min", rangeMin_, rangeMin_);
 		pnh.param("range_max", rangeMax_, rangeMax_);
 		pnh.param("voxel_size", voxelSize_, voxelSize_);
-		ROS_ASSERT(maxClouds_>=0 && assemblingTime_ >=0);
+		ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);

 		cloudsSkipped_ = skipClouds_;

@@ -168,9 +168,9 @@ private:
 				*cpy = *cloudMsg;
 				clouds_.push_back(cpy);

-				if(  (int)clouds_.size() >= maxClouds_ && maxClouds_ != 0 
-					|| 
-					 (double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ && assemblingTime_ != 0.0 )
+				if(  (int)clouds_.size() >= maxClouds_ && maxClouds_ > 0
+					||
+					 (double)(*cpy).header.stamp.toSec() >= (double)clouds_[0]->header.stamp.toSec() + assemblingTime_ && assemblingTime_ > 0.0 )
 				{
 					pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
 					pcl_conversions::toPCL(*clouds_.back(), *assembled);
@@ -205,7 +205,7 @@ private:
 							pcl::concatenatePointCloud(*assembled, *rtabmap::util3d::laserScanToPointCloud2(scan, t), *assembledTmp);
 						}
 						else
-						{	
+						{
 							sensor_msgs::PointCloud2 output;
 							pcl_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output);
 							pcl::PCLPointCloud2 output2;
@@ -200,13 +200,22 @@ private:
 			pcl_conversions::toPCL(*pointCloud2Msg, *cloud);

 			cv_bridge::CvImage depthImage;
-			depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());

-			if(fillHolesSize_ > 0 && fillIterations_ > 0)
+			if(cloud->data.empty())
 			{
-				for(int i=0; i<fillIterations_;++i)
+				ROS_WARN("Received an empty cloud on topic \"%s\"! A depth image with all zeros is returned.", pointCloudSub_.getTopic().c_str());
+				depthImage.image = cv::Mat::zeros(model.imageSize(), CV_32FC1);
+			}
+			else
+			{
+				depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
+
+				if(fillHolesSize_ > 0 && fillIterations_ > 0)
 				{
-					depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
+					for(int i=0; i<fillIterations_;++i)
+					{
+						depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
+					}
 				}
 			}