0.17.5: added grid_prob_map topic (ref #269)

CMakeLists.txt

@@ -30,7 +30,7 @@ find_package(find_object_2d)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.17.4 REQUIRED)
+find_package(RTABMap 0.17.5 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

include/rtabmap_ros/CoreWrapper.h

@@ -151,6 +151,7 @@ private:
 	bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
 	bool getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
+	bool getProbMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool getProjMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
@@ -247,6 +248,7 @@ private:
 	ros::ServiceServer getMapDataSrv_;
 	ros::ServiceServer getProjMapSrv_;
 	ros::ServiceServer getMapSrv_;
+	ros::ServiceServer getProbMapSrv_;
 	ros::ServiceServer getGridMapSrv_;
 	ros::ServiceServer publishMapDataSrv_;
 	ros::ServiceServer getPlanSrv_;

include/rtabmap_ros/MapsManager.h

@@ -74,6 +74,11 @@ public:
 			float & yMin,
 			float & gridCellSize);
 
+	cv::Mat getGridProbMap(
+			float & xMin,
+			float & yMin,
+			float & gridCellSize);
+
 	const rtabmap::OctoMap * getOctomap() const {return octomap_;}
 	const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
 
@@ -93,6 +98,7 @@ private:
 	ros::Publisher cloudObstaclesPub_;
 	ros::Publisher projMapPub_;
 	ros::Publisher gridMapPub_;
+	ros::Publisher gridProbMapPub_;
 	ros::Publisher scanMapPub_;
 	ros::Publisher octoMapPubBin_;
 	ros::Publisher octoMapPubFull_;

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.17.4</version>
+  <version>0.17.5</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CoreWrapper.cpp

@@ -523,6 +523,7 @@ void CoreWrapper::onInit()
 	setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
 	getMapDataSrv_ = nh.advertiseService("get_map_data", &CoreWrapper::getMapDataCallback, this);
 	getMapSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
+	getProbMapSrv_ = nh.advertiseService("get_prob_map", &CoreWrapper::getProbMapCallback, this);
 	getGridMapSrv_ = nh.advertiseService("get_grid_map", &CoreWrapper::getGridMapCallback, this);
 	getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
 	publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
@@ -2192,6 +2193,39 @@ bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetM
 	return false;
 }
 
+bool CoreWrapper::getProbMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res)
+{
+	// create the grid map
+	float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
+	cv::Mat pixels = mapsManager_.getGridProbMap(xMin, yMin, gridCellSize);
+
+	if(!pixels.empty())
+	{
+		//init
+		res.map.info.resolution = gridCellSize;
+		res.map.info.origin.position.x = 0.0;
+		res.map.info.origin.position.y = 0.0;
+		res.map.info.origin.position.z = 0.0;
+		res.map.info.origin.orientation.x = 0.0;
+		res.map.info.origin.orientation.y = 0.0;
+		res.map.info.origin.orientation.z = 0.0;
+		res.map.info.origin.orientation.w = 1.0;
+
+		res.map.info.width = pixels.cols;
+		res.map.info.height = pixels.rows;
+		res.map.info.origin.position.x = xMin;
+		res.map.info.origin.position.y = yMin;
+		res.map.data.resize(res.map.info.width * res.map.info.height);
+
+		memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
+
+		res.map.header.frame_id = mapFrameId_;
+		res.map.header.stamp = ros::Time::now();
+		return true;
+	}
+	return false;
+}
+
 bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtabmap_ros::PublishMap::Response& res)
 {
 	NODELET_INFO("rtabmap: Publishing map...");

src/MapsManager.cpp

@@ -139,6 +139,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
 		nht = &pnh;
 	}
 	gridMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
+	gridProbMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("grid_prob_map", 1, latch);
 	cloudMapPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
 	cloudObstaclesPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_obstacles", 1, latch);
 	cloudGroundPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_ground", 1, latch);
@@ -267,7 +268,7 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet
 #ifdef WITH_OCTOMAP_ROS
 #ifdef RTABMAP_OCTOMAP
 	parameterMoved(pnh, "octomap_ground_is_obstacle", Parameters::kGridGroundIsObstacle(), parameters);
-	parameterMoved(pnh, "octomap_occupancy_thr", Parameters::kGridGlobalOctoMapOccupancyThr(), parameters);
+	parameterMoved(pnh, "octomap_occupancy_thr", Parameters::kGridGlobalOccupancyThr(), parameters);
 #endif
 #endif
 }
@@ -364,6 +365,7 @@ bool MapsManager::hasSubscribers() const
 			cloudGroundPub_.getNumSubscribers() != 0 ||
 			projMapPub_.getNumSubscribers() != 0 ||
 			gridMapPub_.getNumSubscribers() != 0 ||
+			gridProbMapPub_.getNumSubscribers() != 0 ||
 			scanMapPub_.getNumSubscribers() != 0 ||
 			octoMapPubBin_.getNumSubscribers() != 0 ||
 			octoMapPubFull_.getNumSubscribers() != 0 ||
@@ -406,7 +408,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 				octoMapProj_.getNumSubscribers() != 0;
 
 		updateGrid = projMapPub_.getNumSubscribers() != 0 ||
-				gridMapPub_.getNumSubscribers() != 0;
+				gridMapPub_.getNumSubscribers() != 0 ||
+				gridProbMapPub_.getNumSubscribers() != 0;
 
 		updateGridCache = updateOctomap || updateGrid ||
 				cloudMapPub_.getNumSubscribers() != 0 ||
@@ -1228,7 +1231,7 @@ void MapsManager::publishMaps(
 #endif
 #endif
 
-	if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers())
+	if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers() || gridProbMapPub_.getNumSubscribers())
 	{
 		if(projMapPub_.getNumSubscribers())
 		{
@@ -1247,46 +1250,88 @@ void MapsManager::publishMaps(
 			}
 		}
 
-		// create the grid map
+		if(gridProbMapPub_.getNumSubscribers())
-		float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
-		cv::Mat pixels = this->getGridMap(xMin, yMin, gridCellSize);
-
-		if(!pixels.empty())
 		{
-			//init
+			// create the grid map
-			nav_msgs::OccupancyGrid map;
+			float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
-			map.info.resolution = gridCellSize;
+			cv::Mat pixels = this->getGridProbMap(xMin, yMin, gridCellSize);
-			map.info.origin.position.x = 0.0;
+			if(!pixels.empty())
-			map.info.origin.position.y = 0.0;
-			map.info.origin.position.z = 0.0;
-			map.info.origin.orientation.x = 0.0;
-			map.info.origin.orientation.y = 0.0;
-			map.info.origin.orientation.z = 0.0;
-			map.info.origin.orientation.w = 1.0;
-
-			map.info.width = pixels.cols;
-			map.info.height = pixels.rows;
-			map.info.origin.position.x = xMin;
-			map.info.origin.position.y = yMin;
-			map.data.resize(map.info.width * map.info.height);
-
-			memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
-
-			map.header.frame_id = mapFrameId;
-			map.header.stamp = stamp;
-
-			if(gridMapPub_.getNumSubscribers())
 			{
-				gridMapPub_.publish(map);
+				//init
+				nav_msgs::OccupancyGrid map;
+				map.info.resolution = gridCellSize;
+				map.info.origin.position.x = 0.0;
+				map.info.origin.position.y = 0.0;
+				map.info.origin.position.z = 0.0;
+				map.info.origin.orientation.x = 0.0;
+				map.info.origin.orientation.y = 0.0;
+				map.info.origin.orientation.z = 0.0;
+				map.info.origin.orientation.w = 1.0;
+
+				map.info.width = pixels.cols;
+				map.info.height = pixels.rows;
+				map.info.origin.position.x = xMin;
+				map.info.origin.position.y = yMin;
+				map.data.resize(map.info.width * map.info.height);
+
+				memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
+
+				map.header.frame_id = mapFrameId;
+				map.header.stamp = stamp;
+
+				if(gridProbMapPub_.getNumSubscribers())
+				{
+					gridProbMapPub_.publish(map);
+				}
 			}
-			if(projMapPub_.getNumSubscribers())
+			else if(poses.size())
 			{
-				projMapPub_.publish(map);
+				ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
 			}
 		}
-		else if(poses.size())
+		if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers())
 		{
-			ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
+			// create the grid map
+			float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
+			cv::Mat pixels = this->getGridMap(xMin, yMin, gridCellSize);
+
+			if(!pixels.empty())
+			{
+				//init
+				nav_msgs::OccupancyGrid map;
+				map.info.resolution = gridCellSize;
+				map.info.origin.position.x = 0.0;
+				map.info.origin.position.y = 0.0;
+				map.info.origin.position.z = 0.0;
+				map.info.origin.orientation.x = 0.0;
+				map.info.origin.orientation.y = 0.0;
+				map.info.origin.orientation.z = 0.0;
+				map.info.origin.orientation.w = 1.0;
+
+				map.info.width = pixels.cols;
+				map.info.height = pixels.rows;
+				map.info.origin.position.x = xMin;
+				map.info.origin.position.y = yMin;
+				map.data.resize(map.info.width * map.info.height);
+
+				memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
+
+				map.header.frame_id = mapFrameId;
+				map.header.stamp = stamp;
+
+				if(gridMapPub_.getNumSubscribers())
+				{
+					gridMapPub_.publish(map);
+				}
+				if(projMapPub_.getNumSubscribers())
+				{
+					projMapPub_.publish(map);
+				}
+			}
+			else if(poses.size())
+			{
+				ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
+			}
 		}
 	}
 
@@ -1306,3 +1351,12 @@ cv::Mat MapsManager::getGridMap(
 	return occupancyGrid_->getMap(xMin, yMin);
 }
 
+cv::Mat MapsManager::getGridProbMap(
+		float & xMin,
+		float & yMin,
+		float & gridCellSize)
+{
+	gridCellSize = occupancyGrid_->getCellSize();
+	return occupancyGrid_->getProbMap(xMin, yMin);
+}
+