Updated Info msg: changed localLoopClosureId for proximityDetectionId. point_cloud_xyzrgb nodelet: handling bgr and rgb encoding. Updated demo_find_object.launch with 0.11. MapsManager: Fixed grids generated with only one point.

src/MapsManager.cpp

@@ -39,6 +39,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 		cloudFrustumCulling_(false),
 		cloudNoiseFilteringRadius_(0.0),
 		cloudNoiseFilteringMinNeighbors_(5),
+		scanDecimation_(0),
 		scanVoxelSize_(0.0),
 		scanOutputVoxelized_(false),
 		projMaxGroundAngle_(45.0), // degrees
@@ -48,6 +49,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 		gridSize_(0), // meters
 		gridEroded_(false),
 		gridUnknownSpaceFilled_(false),
+		gridMaxUnknownSpaceFilledRange_(6.0),
 		mapFilterRadius_(0.5),
 		mapFilterAngle_(30.0), // degrees
 		mapCacheCleanup_(true)
@@ -89,6 +91,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
 	pnh.param("grid_size", gridSize_, gridSize_); // m
 	pnh.param("grid_eroded", gridEroded_, gridEroded_);
 	pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
+	pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_);
 
 	// common map stuff
 	pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
@@ -358,6 +361,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 								{
 									scan = util3d::downsample(scan, scanDecimation_);
 								}
+
 								if(scanRequired || scanVoxelSize_ > 0.0)
 								{
 									pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
@@ -369,16 +373,24 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 											scan = util3d::laserScan2dFromPointCloud(*scanCloud);
 										}
 									}
+
 									if(scanRequired)
 									{
 										uInsert(scans_, std::make_pair(iter->first, scanCloud));
 									}
 								}
 							}
+
 							if(gridRequired && scan.type() == CV_32FC2)
 							{
 								cv::Mat ground, obstacles;
-								util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());
+								util3d::occupancy2DFromLaserScan(
+										scan,
+										ground,
+										obstacles,
+										gridCellSize_,
+										data.id() < 0 || gridUnknownSpaceFilled_,
+										data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange());
 								uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
 							}
 						}

src/MapsManager.h

@@ -85,6 +85,7 @@ private:
 	double gridSize_;
 	bool gridEroded_;
 	bool gridUnknownSpaceFilled_;
+	double gridMaxUnknownSpaceFilledRange_;
 	double mapFilterRadius_;
 	double mapFilterAngle_;
 	bool mapCacheCleanup_;

src/MsgConversion.cpp

@@ -146,7 +146,7 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
 	// rtabmap_ros::Info
 	stat.setRefImageId(info.refId);
 	stat.setLoopClosureId(info.loopClosureId);
-	stat.setLocalLoopClosureId(info.localLoopClosureId);
+	stat.setProximityDetectionId(info.proximityDetectionId);
 
 	stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform));
 
@@ -190,7 +190,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
 {
 	info.refId = stats.refImageId();
 	info.loopClosureId = stats.loopClosureId();
-	info.localLoopClosureId = stats.localLoopClosureId();
+	info.proximityDetectionId = stats.proximityDetectionId();
 
 	rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform);
 

src/nodelets/point_cloud_xyzrgb.cpp

@@ -173,7 +173,21 @@ private:
 
 		if(cloudPub_.getNumSubscribers())
 		{
-			cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
+			cv_bridge::CvImageConstPtr imagePtr;
+			if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+			{
+				imagePtr = cv_bridge::toCvShare(image);
+			}
+			else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+					image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+			{
+				imagePtr = cv_bridge::toCvShare(image, "mono8");
+			}
+			else
+			{
+				imagePtr = cv_bridge::toCvShare(image, "bgr8");
+			}
+
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
 
 			image_geometry::PinholeCameraModel model;

src/rviz/InfoDisplay.cpp

@@ -51,8 +51,8 @@ void InfoDisplay::onInitialize()
 	this->setStatusStd(rviz::StatusProperty::Ok, "Info", "");
 	this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", "");
 	this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
-	this->setStatusStd(rviz::StatusProperty::Ok, "Global", "0");
-	this->setStatusStd(rviz::StatusProperty::Ok, "Local", "0");
+	this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0");
+	this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0");
 
 	spinner_.start();
 }
@@ -63,12 +63,12 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
 		boost::mutex::scoped_lock lock(info_mutex_);
 		if(msg->loopClosureId)
 		{
-			info_ = QString("%1->%2 [Global]").arg(msg->refId).arg(msg->loopClosureId);
+			info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId);
 			globalCount_ += 1;
 		}
-		else if(msg->localLoopClosureId)
+		else if(msg->proximityDetectionId)
 		{
-			info_ = QString("%1->%2 [Local]").arg(msg->refId).arg(msg->localLoopClosureId);
+			info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId);
 			localCount_ += 1;
 		}
 		else
@@ -103,8 +103,8 @@ void InfoDisplay::update( float wall_dt, float ros_dt )
 			this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString());
 			this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString());
 		}
-		this->setStatusStd(rviz::StatusProperty::Ok, "Global", tr("%1").arg(globalCount_).toStdString());
-		this->setStatusStd(rviz::StatusProperty::Ok, "Local", tr("%1").arg(localCount_).toStdString());
+		this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
+		this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString());
 
 		for(std::map<std::string, float>::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter)
 		{