rtabmap: added local grid output topics for convenience. rviz/MapCloud: added cloud_from_scan option. point_cloud_assembler: added option to skip clouds.

src/CoreWrapper.cpp

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <geometry_msgs/PoseArray.h>
 #include <sensor_msgs/image_encodings.h>
 #include <cv_bridge/cv_bridge.h>
+#include <pcl_conversions/pcl_conversions.h>
 #include <pcl/io/io.h>
 
 #include <visualization_msgs/MarkerArray.h>
@@ -217,6 +218,9 @@ void CoreWrapper::onInit()
 	landmarksPub_ = nh.advertise<geometry_msgs::PoseArray>("landmarks", 1);
 	labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
 	mapPathPub_ = nh.advertise<nav_msgs::Path>("mapPath", 1);
+	localGridObstacle_ = nh.advertise<sensor_msgs::PointCloud2>("local_grid_obstacle", 1);
+	localGridEmpty_ = nh.advertise<sensor_msgs::PointCloud2>("local_grid_empty", 1);
+	localGridGround_ = nh.advertise<sensor_msgs::PointCloud2>("local_grid_ground", 1);
 	localizationPosePub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("localization_pose", 1);
 	initialPoseSub_ = nh.subscribe("initialpose", 1, &CoreWrapper::initialPoseCallback, this);
 
@@ -3257,6 +3261,34 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 		mapGraphPub_.publish(msg);
 	}
 
+	if(localGridObstacle_.getNumSubscribers() && !stats.getLastSignatureData().sensorData().gridObstacleCellsRaw().empty())
+	{
+		pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridObstacleCellsRaw()));
+		sensor_msgs::PointCloud2 msg;
+		pcl_conversions::moveFromPCL(*cloud, msg);
+		msg.header.stamp = stamp;
+		msg.header.frame_id = frameId_;
+		localGridObstacle_.publish(msg);
+	}
+	if(localGridEmpty_.getNumSubscribers() && !stats.getLastSignatureData().sensorData().gridEmptyCellsRaw().empty())
+	{
+		pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridEmptyCellsRaw()));
+		sensor_msgs::PointCloud2 msg;
+		pcl_conversions::moveFromPCL(*cloud, msg);
+		msg.header.stamp = stamp;
+		msg.header.frame_id = frameId_;
+		localGridEmpty_.publish(msg);
+	}
+	if(localGridGround_.getNumSubscribers() && !stats.getLastSignatureData().sensorData().gridGroundCellsRaw().empty())
+	{
+		pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridGroundCellsRaw()));
+		sensor_msgs::PointCloud2 msg;
+		pcl_conversions::moveFromPCL(*cloud, msg);
+		msg.header.stamp = stamp;
+		msg.header.frame_id = frameId_;
+		localGridGround_.publish(msg);
+	}
+
 	bool pubLabels = labelsPub_.getNumSubscribers();
 	visualization_msgs::MarkerArray markers;
 	if((landmarksPub_.getNumSubscribers() || pubLabels) && !stats.poses().empty() && stats.poses().begin()->first < 0)

src/nodelets/point_cloud_assembler.cpp

@@ -65,6 +65,8 @@ public:
 		callbackCalled_(false),
 		exactSync_(0),
 		maxClouds_(1),
+		skipClouds_(0),
+		cloudsSkipped_(0),
 		fixedFrameId_("odom")
 	{}
 
@@ -90,8 +92,11 @@ private:
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
 		pnh.param("max_clouds", maxClouds_, maxClouds_);
+		pnh.param("skip_clouds", skipClouds_, skipClouds_);
 		ROS_ASSERT(maxClouds_>0);
 
+		cloudsSkipped_ = skipClouds_;
+
 		std::string subscribedTopicsMsg;
 		if(!fixedFrameId_.empty())
 		{
@@ -141,39 +146,48 @@ private:
 	{
 		if(cloudPub_.getNumSubscribers())
 		{
-			sensor_msgs::PointCloud2Ptr cpy(new sensor_msgs::PointCloud2);
-			*cpy = *cloudMsg;
-			clouds_.push_back(cpy);
-
-			if((int)clouds_.size() >= maxClouds_)
+			if(skipClouds_<=0 || cloudsSkipped_ >= skipClouds_)
 			{
-				pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
-				pcl_conversions::toPCL(*clouds_.back(), *assembled);
+				cloudsSkipped_ = 0;
 
-				for(size_t i=0; i<clouds_.size()-1; ++i)
+				sensor_msgs::PointCloud2Ptr cpy(new sensor_msgs::PointCloud2);
+				*cpy = *cloudMsg;
+				clouds_.push_back(cpy);
+
+				if((int)clouds_.size() >= maxClouds_)
 				{
-					rtabmap::Transform t = rtabmap_ros::getTransform(
-							clouds_[i]->header.frame_id, //sourceTargetFrame
-							fixedFrameId_, //fixedFrame
-							clouds_[i]->header.stamp, //stampSource
-							clouds_.back()->header.stamp, //stampTarget
-							tfListener_,
-							0.1);
+					pcl::PCLPointCloud2Ptr assembled(new pcl::PCLPointCloud2);
+					pcl_conversions::toPCL(*clouds_.back(), *assembled);
 
-					sensor_msgs::PointCloud2 output;
-					pcl_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output);
-					pcl::PCLPointCloud2 output2;
-					pcl_conversions::toPCL(output, output2);
-					pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
-					pcl::concatenatePointCloud(*assembled, output2, *assembledTmp);
-					assembled = assembledTmp;
+					for(size_t i=0; i<clouds_.size()-1; ++i)
+					{
+						rtabmap::Transform t = rtabmap_ros::getTransform(
+								clouds_[i]->header.frame_id, //sourceTargetFrame
+								fixedFrameId_, //fixedFrame
+								clouds_[i]->header.stamp, //stampSource
+								clouds_.back()->header.stamp, //stampTarget
+								tfListener_,
+								0.1);
+
+						sensor_msgs::PointCloud2 output;
+						pcl_ros::transformPointCloud(t.toEigen4f(), *clouds_[i], output);
+						pcl::PCLPointCloud2 output2;
+						pcl_conversions::toPCL(output, output2);
+						pcl::PCLPointCloud2Ptr assembledTmp(new pcl::PCLPointCloud2);
+						pcl::concatenatePointCloud(*assembled, output2, *assembledTmp);
+						assembled = assembledTmp;
+					}
+
+					sensor_msgs::PointCloud2 rosCloud;
+					pcl_conversions::moveFromPCL(*assembled, rosCloud);
+					rosCloud.header = cloudMsg->header;
+					cloudPub_.publish(rosCloud);
+					clouds_.clear();
 				}
-
-				sensor_msgs::PointCloud2 rosCloud;
-				pcl_conversions::moveFromPCL(*assembled, rosCloud);
-				rosCloud.header = cloudMsg->header;
-				cloudPub_.publish(rosCloud);
-				clouds_.clear();
+			}
+			else
+			{
+				++cloudsSkipped_;
 			}
 		}
 	}
@@ -208,6 +222,8 @@ private:
 	message_filters::Subscriber<nav_msgs::Odometry> syncOdomSub_;
 
 	int maxClouds_;
+	int skipClouds_;
+	int cloudsSkipped_;
 	std::string fixedFrameId_;
 	tf::TransformListener tfListener_;
 

src/rviz/MapCloudDisplay.cpp

@@ -131,6 +131,10 @@ MapCloudDisplay::MapCloudDisplay()
 	connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
 			 this, SLOT( setColorTransformerOptions( EnumProperty* )));
 
+	cloud_from_scan_ = new rviz::BoolProperty( "Cloud from scan", false,
+										 "Create the cloud from laser scans instead of the RGB-D/Stereo images.",
+										 this, SLOT( updateCloudParameters() ), this );
+
 	cloud_decimation_ = new rviz::IntProperty( "Cloud decimation", 4,
 										 "Decimation of the input RGB and depth images before creating the cloud.",
 										 this, SLOT( updateCloudParameters() ), this );
@@ -269,24 +273,31 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 	}
 
 	// Add new clouds...
+	bool fromDepth = !cloud_from_scan_->getBool();
 	for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
 	{
 		int id = map.nodes[i].id;
 
 		// Always refresh the cloud if there are data
 		rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
-		if(!s.sensorData().imageCompressed().empty() &&
-		   !s.sensorData().depthOrRightCompressed().empty() &&
-		   (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection()))
+		ROS_WARN("s.sensorData().laserScanCompressed()=%d", s.sensorData().laserScanCompressed().size());
+		if((fromDepth &&
+			!s.sensorData().imageCompressed().empty() &&
+		    !s.sensorData().depthOrRightCompressed().empty() &&
+		    (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValidForProjection())) ||
+		   (!fromDepth && !s.sensorData().laserScanCompressed().isEmpty()))
 		{
 			cv::Mat image, depth;
-			s.sensorData().uncompressData(&image, &depth, 0);
+			rtabmap::LaserScan scan;
 
+			s.sensorData().uncompressData(fromDepth?&image:0, fromDepth?&depth:0, !fromDepth?&scan:0);
 
-			if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
+			sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+			if(fromDepth && !s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
 			{
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 				pcl::IndicesPtr validIndices(new std::vector<int>);
+
 				cloud = rtabmap::util3d::cloudRGBFromSensorData(
 						s.sensorData(),
 						cloud_decimation_->getInt(),
@@ -294,13 +305,13 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 						cloud_min_depth_->getFloat(),
 						validIndices.get());
 
-				if(cloud_voxel_size_->getFloat())
+				if(!cloud->empty())
 				{
-					cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
-				}
+					if(cloud_voxel_size_->getFloat())
+					{
+						cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
+					}
 
-				if(cloud->size())
-				{
 					if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
 					{
 						// convert in /odom frame
@@ -312,23 +323,54 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 						cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
 					}
 
-					sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
-					pcl::toROSMsg(*cloud, *cloudMsg);
-					cloudMsg->header = map.header;
-
-					CloudInfoPtr info(new CloudInfo);
-					info->message_ = cloudMsg;
-					info->pose_ = rtabmap::Transform::getIdentity();
-					info->id_ = id;
-
-					if (transformCloud(info, true))
+					if(!cloud->empty())
 					{
-						boost::mutex::scoped_lock lock(new_clouds_mutex_);
-						new_cloud_infos_.erase(id);
-						new_cloud_infos_.insert(std::make_pair(id, info));
+						pcl::toROSMsg(*cloud, *cloudMsg);
 					}
 				}
 			}
+			else if(!fromDepth && !scan.isEmpty())
+			{
+				scan = rtabmap::util3d::commonFiltering(
+						scan,
+						1,
+						cloud_min_depth_->getFloat(),
+						cloud_max_depth_->getFloat(),
+						cloud_voxel_size_->getFloat());
+				pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
+				cloud = rtabmap::util3d::laserScanToPointCloudI(scan, scan.localTransform());
+				if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
+				{
+					// convert in /odom frame
+					cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose());
+					cloud = rtabmap::util3d::passThrough(cloud, "z",
+							cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
+							cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
+					// convert back in /base_link frame
+					cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
+				}
+
+				if(!cloud->empty())
+				{
+					pcl::toROSMsg(*cloud, *cloudMsg);
+				}
+			}
+
+			if(!cloudMsg->data.empty())
+			{
+				cloudMsg->header = map.header;
+				CloudInfoPtr info(new CloudInfo);
+				info->message_ = cloudMsg;
+				info->pose_ = rtabmap::Transform::getIdentity();
+				info->id_ = id;
+
+				if (transformCloud(info, true))
+				{
+					boost::mutex::scoped_lock lock(new_clouds_mutex_);
+					new_cloud_infos_.erase(id);
+					new_cloud_infos_.insert(std::make_pair(id, info));
+				}
+			}
 		}
 	}
 

src/rviz/MapCloudDisplay.h

@@ -108,6 +108,7 @@ public:
 	rviz::EnumProperty* xyz_transformer_property_;
 	rviz::EnumProperty* color_transformer_property_;
 	rviz::EnumProperty* style_property_;
+	rviz::BoolProperty* cloud_from_scan_;
 	rviz::IntProperty* cloud_decimation_;
 	rviz::FloatProperty* cloud_max_depth_;
 	rviz::FloatProperty* cloud_min_depth_;