diff --git a/CMakeLists.txt b/CMakeLists.txt index 92473315..f419f0f6 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/include/rtabmap_ros/MapsManager.h b/include/rtabmap_ros/MapsManager.h index 4e08108e..1d106707 100644 --- a/include/rtabmap_ros/MapsManager.h +++ b/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_; diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp index e8052cc0..ce87e7fd 100644 --- a/src/MapsManager.cpp +++ b/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_; - cv::Mat map; - if(gridIncremental_) - { - occupancyGrid_->update(poses, gridSize_, footprintRadius_); - map = occupancyGrid_->getMap(xMin, yMin); - } - else - { - map = util3d::create2DMapFromOccupancyLocalMaps( - poses, - gridMaps_, - gridCellSize_, - xMin, yMin, - gridSize_, - gridEroded_, - footprintRadius_); - } - return map; + gridCellSize = occupancyGrid_->getCellSize(); + occupancyGrid_->update(poses); + return occupancyGrid_->getMap(xMin, yMin); }