Obstacles detection: added output "proj_obstacles" for a cloud of projected obstacles on xy plane. Fixed not published /scan_map when scan_voxel_size=0.0.

src/MapsManager.cpp

@@ -421,7 +421,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 
 						if(scanRequired || gridRequired)
 						{
-							if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0))
+							if(scan.cols && (scanRequired || scanVoxelSize_ > 0.0 || scanDecimation_ > 1))
 							{
 								if(scanDecimation_ > 1)
 								{
@@ -491,6 +491,18 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 				++iter;
 			}
 		}
+		for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=scans_.begin();
+			iter!=scans_.end();)
+		{
+			if(!uContains(poses, iter->first))
+			{
+				scans_.erase(iter++);
+			}
+			else
+			{
+				++iter;
+			}
+		}
 		for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=projMaps_.begin();
 			iter!=projMaps_.end();)
 		{

src/nodelets/obstacles_detection.cpp

@@ -75,7 +75,8 @@ public:
 		maxGroundHeight_(0.0),    // if<=0.0 -> disabled, used only if detect_flat_obstacles is true
 		segmentFlatObstacles_(false),
 		waitForTransform_(false),
-		optimizeForCloseObjects_(false)
+		optimizeForCloseObjects_(false),
+		projVoxelSize_(0.01)
 	{}
 
 	virtual ~ObstaclesDetection()
@@ -110,11 +111,13 @@ private:
 		pnh.param("detect_flat_obstacles", segmentFlatObstacles_, segmentFlatObstacles_);
 		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 		pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
+		pnh.param("proj_voxel_size", projVoxelSize_, projVoxelSize_);
 
 		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
 
 		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
 		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
+		projObstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("proj_obstacles", 1);
 	}
 
 
@@ -123,7 +126,7 @@ private:
 	{
 		ros::WallTime time = ros::WallTime::now();
 
-		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
+		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0 && projObstaclesPub_.getNumSubscribers() == 0)
 		{
 			// no one wants the results
 			return;
@@ -187,7 +190,7 @@ private:
 						pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
 					}
 
-					if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
 					{
 						pcl::copyPointCloud(*originalCloud, *obstacles, *obstaclesCloud);
 					}
@@ -223,7 +226,7 @@ private:
 						ground->clear();
 					}
 
-					if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
 					{
 						pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud);
 						obstacles->clear();
@@ -249,7 +252,7 @@ private:
 					}
 
 
-					if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
+					if((obstaclesPub_.getNumSubscribers() || projObstaclesPub_.getNumSubscribers()) && obstacles.get() && obstacles->size())
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles2(new pcl::PointCloud<pcl::PointXYZ>);
 						pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2);
@@ -281,6 +284,27 @@ private:
 			obstaclesPub_.publish(rosCloud);
 		}
 
+		if(projObstaclesPub_.getNumSubscribers())
+		{
+			for(unsigned int i=0; i<obstaclesCloud->size(); ++i)
+			{
+				obstaclesCloud->at(i).z = 0;
+			}
+
+			if(obstaclesCloud->size() && projVoxelSize_ > 0.0)
+			{
+				obstaclesCloud = rtabmap::util3d::voxelize(obstaclesCloud, projVoxelSize_);
+			}
+
+			sensor_msgs::PointCloud2 rosCloud;
+			pcl::toROSMsg(*obstaclesCloud, rosCloud);
+			rosCloud.header.stamp = cloudMsg->header.stamp;
+			rosCloud.header.frame_id = frameId_;
+
+			//publish the message
+			projObstaclesPub_.publish(rosCloud);
+		}
+
 		//NODELET_INFO("Obstacles segmentation time = %f s", (ros::WallTime::now() - time).toSec());
 	}
 
@@ -295,11 +319,13 @@ private:
 	bool segmentFlatObstacles_;
 	bool waitForTransform_;
 	bool optimizeForCloseObjects_;
+	double projVoxelSize_;
 
 	tf::TransformListener tfListener_;
 
 	ros::Publisher groundPub_;
 	ros::Publisher obstaclesPub_;
+	ros::Publisher projObstaclesPub_;
 
 	ros::Subscriber cloudSub_;
 };