rtabmap 0.12.15 required, updated Grid parameters (removed grid_cell_size #161)

CMakeLists.txt

@@ -18,7 +18,7 @@ find_package(rviz)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.12.3 REQUIRED)
+find_package(RTABMap 0.12.5 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

include/rtabmap_ros/MapsManager.h

@@ -82,11 +82,6 @@ private:
 	bool cloudOutputVoxelized_;
 	bool cloudSubtractFiltering_;
 	int cloudSubtractFilteringMinNeighbors_;
-	double gridCellSize_;
-	bool gridIncremental_;
-	double gridSize_;
-	bool gridEroded_;
-	double footprintRadius_;
 	double mapFilterRadius_;
 	double mapFilterAngle_;
 	bool mapCacheCleanup_;

src/MapsManager.cpp

@@ -61,11 +61,6 @@ MapsManager::MapsManager() :
 		cloudOutputVoxelized_(true),
 		cloudSubtractFiltering_(false),
 		cloudSubtractFilteringMinNeighbors_(2),
-		gridCellSize_(0.05), // meters
-		gridIncremental_(false),
-		gridSize_(0), // meters
-		gridEroded_(false),
-		footprintRadius_(0.0),
 		mapFilterRadius_(0.0),
 		mapFilterAngle_(30.0), // degrees
 		mapCacheCleanup_(true),
@@ -82,19 +77,6 @@ MapsManager::MapsManager() :
 
 void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace)
 {
-	// common grid map stuff
-	pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
-	if(gridCellSize_ <= 0)
-	{
-		ROS_FATAL("\"grid_cell_size\" (%f) should be greater than 0!", gridCellSize_);
-	}
-	occupancyGrid_->setCellSize(gridCellSize_);
-
-	pnh.param("grid_incremental", gridIncremental_, gridIncremental_); // m
-	pnh.param("grid_size", gridSize_, gridSize_); // m
-	pnh.param("grid_eroded", gridEroded_, gridEroded_);
-	pnh.param("grid_footprint_radius", footprintRadius_, footprintRadius_);
-
 	// common map stuff
 	pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
 	pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
@@ -115,11 +97,6 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
 	pnh.param("cloud_subtract_filtering", cloudSubtractFiltering_, cloudSubtractFiltering_);
 	pnh.param("cloud_subtract_filtering_min_neighbors", cloudSubtractFilteringMinNeighbors_, cloudSubtractFilteringMinNeighbors_);
 
-	ROS_INFO("%s(maps): grid_cell_size             = %f", name.c_str(), gridCellSize_);
-	ROS_INFO("%s(maps): grid_incremental           = %s", name.c_str(), gridIncremental_?"true":"false");
-	ROS_INFO("%s(maps): grid_size                  = %f", name.c_str(), gridSize_);
-	ROS_INFO("%s(maps): grid_eroded                = %s", name.c_str(), gridEroded_?"true":"false");
-	ROS_INFO("%s(maps): grid_footprint_radius      = %f", name.c_str(), footprintRadius_);
 	ROS_INFO("%s(maps): map_filter_radius          = %f", name.c_str(), mapFilterRadius_);
 	ROS_INFO("%s(maps): map_filter_angle           = %f", name.c_str(), mapFilterAngle_);
 	ROS_INFO("%s(maps): map_cleanup                = %s", name.c_str(), mapCacheCleanup_?"true":"false");
@@ -133,7 +110,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
 #ifdef RTABMAP_OCTOMAP
 	pnh.param("octomap_occupancy_thr", octomapOccupancyThr_, octomapOccupancyThr_);
 	UASSERT(octomapOccupancyThr_>=0.0 && octomapOccupancyThr_<=1.0);
-	octomap_ = new OctoMap(gridCellSize_, octomapOccupancyThr_);
+	octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_);
 	pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
 	if(octomapTreeDepth_ > 16)
 	{
@@ -232,6 +209,17 @@ void parameterMoved(
 				nh.getParam(rosName, v);
 				parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
 			}
+			else if(type.compare("bool"))
+			{
+				bool v = uStr2Bool(iter->second);
+				nh.getParam(rosName, v);
+				if(rosName.compare("grid_incremental") == 0)
+				{
+					v = !v; // new parameter is called kGridGlobalFullUpdate(), which is the inverse
+				}
+				parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
+
+			}
 			else
 			{
 				ROS_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str());
@@ -274,6 +262,11 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet
 	parameterMoved(pnh, "proj_map_frame", Parameters::kGridMapFrameProjection(), parameters);
 	parameterMoved(pnh, "grid_unknown_space_filled", Parameters::kGridScan2dUnknownSpaceFilled(), parameters);
 	parameterMoved(pnh, "grid_unknown_space_filled_max_range", Parameters::kGridScan2dMaxFilledRange(), parameters);
+	parameterMoved(pnh, "grid_cell_size", Parameters::kGridCellSize(), parameters);
+	parameterMoved(pnh, "grid_incremental", Parameters::kGridGlobalFullUpdate(), parameters);
+	parameterMoved(pnh, "grid_size", Parameters::kGridGlobalMinSize(), parameters);
+	parameterMoved(pnh, "grid_eroded", Parameters::kGridGlobalEroded(), parameters);
+	parameterMoved(pnh, "grid_footprint_radius", Parameters::kGridGlobalFootprintRadius(), parameters);
 
 #ifdef WITH_OCTOMAP_ROS
 #ifdef RTABMAP_OCTOMAP
@@ -285,11 +278,18 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet
 void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
 {
 	parameters_ = parameters;
-
-	// don't use grid cell size from parameters as we use grid_cell_size ros param
-	uInsert(parameters_, ParametersPair(Parameters::kGridCellSize(), uNumber2Str(gridCellSize_)));
-
 	occupancyGrid_->parseParameters(parameters_);
+
+#ifdef WITH_OCTOMAP_ROS
+#ifdef RTABMAP_OCTOMAP
+	if(octomap_)
+	{
+		delete octomap_;
+		octomap_ = 0;
+	}
+	octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_);
+#endif
+#endif
 }
 
 void MapsManager::clear()
@@ -868,7 +868,7 @@ void MapsManager::publishMaps(
 						{
 							if(assembledGroundIndex_.indexedFeatures())
 							{
-								subtractedCloud = subtractFiltering(transformed, assembledGroundIndex_, gridCellSize_, cloudSubtractFilteringMinNeighbors_);
+								subtractedCloud = subtractFiltering(transformed, assembledGroundIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
 							}
 							if(subtractedCloud->size())
 							{
@@ -909,7 +909,7 @@ void MapsManager::publishMaps(
 						{
 							if(assembledObstacleIndex_.indexedFeatures())
 							{
-								subtractedCloud = subtractFiltering(transformed, assembledObstacleIndex_, gridCellSize_, cloudSubtractFilteringMinNeighbors_);
+								subtractedCloud = subtractFiltering(transformed, assembledObstacleIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
 							}
 							if(subtractedCloud->size())
 							{
@@ -944,14 +944,14 @@ void MapsManager::publishMaps(
 
 		if(cloudOutputVoxelized_)
 		{
-			UASSERT(gridCellSize_ > 0.0);
+			UASSERT(occupancyGrid_->getCellSize() > 0.0);
 			if(countGrounds && assembledGround_->size())
 			{
-				assembledGround_ = util3d::voxelize(assembledGround_, gridCellSize_);
+				assembledGround_ = util3d::voxelize(assembledGround_, occupancyGrid_->getCellSize());
 			}
 			if(countObstacles && assembledObstacles_->size())
 			{
-				assembledObstacles_ = util3d::voxelize(assembledObstacles_, gridCellSize_);
+				assembledObstacles_ = util3d::voxelize(assembledObstacles_, occupancyGrid_->getCellSize());
 			}
 		}
 
@@ -1058,7 +1058,7 @@ void MapsManager::publishMaps(
 		{
 			// create the projection map
 			float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
-			cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, gridSize_);
+			cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, occupancyGrid_->getMinMapSize(), octomapTreeDepth_);
 
 			if(!pixels.empty())
 			{
@@ -1178,24 +1178,8 @@ cv::Mat MapsManager::generateGridMap(
 		float & yMin,
 		float & gridCellSize)
 {
-	gridCellSize = gridCellSize_;
+	gridCellSize = occupancyGrid_->getCellSize();
-	cv::Mat map;
+	occupancyGrid_->update(poses);
-	if(gridIncremental_)
+	return occupancyGrid_->getMap(xMin, yMin);
-	{
-		occupancyGrid_->update(poses, gridSize_, footprintRadius_);
-		map = occupancyGrid_->getMap(xMin, yMin);
-	}
-	else
-	{
-		map = util3d::create2DMapFromOccupancyLocalMaps(
-				poses,
-				gridMaps_,
-				gridCellSize_,
-				xMin, yMin,
-				gridSize_,
-				gridEroded_,
-				footprintRadius_);
-	}
-	return map;
 }