From 752e7524f5a43717ef012a7b21594644f178c29e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 17 Dec 2023 22:44:40 -0800 Subject: [PATCH] GridMap integration (#1082) * GridMap integration * Updated with upstream changes * updated with upstream changes * some fixes --- rtabmap_slam/src/CoreWrapper.cpp | 3 +- rtabmap_util/CMakeLists.txt | 46 ++- .../include/rtabmap_util/MapsManager.h | 14 +- rtabmap_util/package.xml | 1 + rtabmap_util/src/MapsManager.cpp | 281 ++++++++---------- .../src/nodelets/obstacles_detection.cpp | 8 +- 6 files changed, 171 insertions(+), 182 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 0c565782e..9f09ab599 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -60,6 +60,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #ifdef WITH_OCTOMAP_MSGS @@ -2140,7 +2141,7 @@ void CoreWrapper::process( rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first || rtabmap_.getMemory()->getLastWorkingSignature() == 0 || rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 || - (!mapsManager_.getOccupancyGrid()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data + (!mapsManager_.getLocalMapMaker()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data { SensorData tmpData = data; tmpData.setId(0); diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 5a17b9887..f7aca0e1b 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -9,11 +9,15 @@ find_package(catkin REQUIRED COMPONENTS # Optional components find_package(octomap_msgs) +find_package(grid_map_ros) SET(optional_dependencies "") IF(octomap_msgs_FOUND) SET(optional_dependencies ${optional_dependencies} octomap_msgs) ENDIF(octomap_msgs_FOUND) +IF(grid_map_ros_FOUND) + SET(optional_dependencies ${optional_dependencies} grid_map_ros) +ENDIF(grid_map_ros_FOUND) catkin_package( INCLUDE_DIRS include @@ -32,6 +36,11 @@ include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/include ${catkin_INCLUDE_DIRS} ) + +# libraries +SET(Libraries + ${catkin_LIBRARIES} +) SET(rtabmap_util_plugins_lib_src src/MapsManager.cpp @@ -65,6 +74,19 @@ SET(Libraries ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS") ENDIF(octomap_msgs_FOUND) +# If grid_map is found, add definition +IF(grid_map_ros_FOUND) +MESSAGE(STATUS "WITH grid_map_ros") +include_directories( + ${grid_map_ros_INCLUDE_DIRS} +) +SET(Libraries + ${grid_map_ros_LIBRARIES} + ${Libraries} +) +ADD_DEFINITIONS("-DWITH_GRID_MAP_ROS") +ENDIF(grid_map_ros_FOUND) + ############################ ## Declare a cpp library ############################ @@ -72,51 +94,51 @@ add_library(rtabmap_util_plugins ${rtabmap_util_plugins_lib_src} ) target_link_libraries(rtabmap_util_plugins - ${catkin_LIBRARIES} + ${Libraries} ) add_executable(rtabmap_rgbd_relay src/RGBDRelayNode.cpp) -target_link_libraries(rtabmap_rgbd_relay ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_rgbd_relay ${Libraries}) set_target_properties(rtabmap_rgbd_relay PROPERTIES OUTPUT_NAME "rgbd_relay") add_executable(rtabmap_rgbd_split src/RGBDSplitNode.cpp) -target_link_libraries(rtabmap_rgbd_split ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_rgbd_split ${Libraries}) set_target_properties(rtabmap_rgbd_split PROPERTIES OUTPUT_NAME "rgbd_split") add_executable(rtabmap_map_optimizer src/MapOptimizerNode.cpp) -target_link_libraries(rtabmap_map_optimizer ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_map_optimizer ${Libraries}) set_target_properties(rtabmap_map_optimizer PROPERTIES OUTPUT_NAME "map_optimizer") add_executable(rtabmap_map_assembler src/MapAssemblerNode.cpp) -target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_map_assembler rtabmap_util_plugins ${Libraries}) set_target_properties(rtabmap_map_assembler PROPERTIES OUTPUT_NAME "map_assembler") add_executable(rtabmap_imu_to_tf src/ImuToTFNode.cpp) -target_link_libraries(rtabmap_imu_to_tf ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_imu_to_tf ${Libraries}) set_target_properties(rtabmap_imu_to_tf PROPERTIES OUTPUT_NAME "imu_to_tf") add_executable(rtabmap_lidar_deskewing src/LidarDeskewingNode.cpp) -target_link_libraries(rtabmap_lidar_deskewing ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_lidar_deskewing ${Libraries}) set_target_properties(rtabmap_lidar_deskewing PROPERTIES OUTPUT_NAME "lidar_deskewing") add_executable(rtabmap_data_player src/DbPlayerNode.cpp) -target_link_libraries(rtabmap_data_player ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_data_player ${Libraries}) set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player") add_executable(rtabmap_odom_msg_to_tf src/OdomMsgToTFNode.cpp) -target_link_libraries(rtabmap_odom_msg_to_tf ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_odom_msg_to_tf ${Libraries}) set_target_properties(rtabmap_odom_msg_to_tf PROPERTIES OUTPUT_NAME "odom_msg_to_tf") add_executable(rtabmap_pointcloud_to_depthimage src/PointCloudToDepthImageNode.cpp) -target_link_libraries(rtabmap_pointcloud_to_depthimage ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_pointcloud_to_depthimage ${Libraries}) set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "pointcloud_to_depthimage") add_executable(rtabmap_point_cloud_aggregator src/PointCloudAggregatorNode.cpp) -target_link_libraries(rtabmap_point_cloud_aggregator ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_point_cloud_aggregator ${Libraries}) set_target_properties(rtabmap_point_cloud_aggregator PROPERTIES OUTPUT_NAME "point_cloud_aggregator") add_executable(rtabmap_point_cloud_assembler src/PointCloudAssemblerNode.cpp) -target_link_libraries(rtabmap_point_cloud_assembler ${catkin_LIBRARIES}) +target_link_libraries(rtabmap_point_cloud_assembler ${Libraries}) set_target_properties(rtabmap_point_cloud_assembler PROPERTIES OUTPUT_NAME "point_cloud_assembler") ############# diff --git a/rtabmap_util/include/rtabmap_util/MapsManager.h b/rtabmap_util/include/rtabmap_util/MapsManager.h index c7150e1ad..311ca24c0 100644 --- a/rtabmap_util/include/rtabmap_util/MapsManager.h +++ b/rtabmap_util/include/rtabmap_util/MapsManager.h @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -40,6 +41,8 @@ namespace rtabmap { class OctoMap; class Memory; class OccupancyGrid; +class LocalGridMaker; +class GridMap; } // namespace rtabmap @@ -85,6 +88,7 @@ class MapsManager { const rtabmap::OctoMap * getOctomap() const {return octomap_;} const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;} + const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;} private: // mapping stuff @@ -112,6 +116,7 @@ class MapsManager { ros::Publisher octoMapObstacleCloud_; ros::Publisher octoMapEmptySpace_; ros::Publisher octoMapProj_; + ros::Publisher elevationMapPub_; std::map assembledGroundPoses_; std::map assembledObstaclePoses_; @@ -122,18 +127,19 @@ class MapsManager { std::map::Ptr > groundClouds_; std::map::Ptr > obstacleClouds_; - std::map gridPoses_; - cv::Mat gridMap_; - std::map, cv::Mat> > gridMaps_; // < , empty cells > - std::map gridMapsViewpoints_; + rtabmap::LocalGridCache localMaps_; rtabmap::OccupancyGrid * occupancyGrid_; + rtabmap::LocalGridMaker * localMapMaker_; bool gridUpdated_; rtabmap::OctoMap * octomap_; int octomapTreeDepth_; bool octomapUpdated_; + rtabmap::GridMap * elevationMap_; + bool elevationMapUpdated_; + rtabmap::ParametersMap parameters_; bool latching_; diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index cf55452b8..4eed22a82 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -28,6 +28,7 @@ pluginlib rtabmap_msgs rtabmap_conversions + grid_map_ros diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 1e9e63d4f..3b82b4340 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -38,20 +38,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include - #include #include #include #include +#include -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) #include #include #include #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) +#include +#include #endif using namespace rtabmap; @@ -69,15 +72,22 @@ MapsManager::MapsManager() : scanEmptyRayTracing_(true), assembledObstacles_(new pcl::PointCloud), assembledGround_(new pcl::PointCloud), - occupancyGrid_(new OccupancyGrid), + occupancyGrid_(new OccupancyGrid(&localMaps_)), + localMapMaker_(new LocalGridMaker), gridUpdated_(true), -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - octomap_(new OctoMap), -#endif +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + octomap_(new OctoMap(&localMaps_)), +#else + octomap_(0), #endif octomapTreeDepth_(16), octomapUpdated_(true), +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + elevationMap_(new GridMap(&localMaps_)), +#else + elevationMap_(0), +#endif + elevationMapUpdated_(true), latching_(true) { } @@ -135,8 +145,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s ROS_INFO("%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false"); ROS_INFO("%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_); if(octomapTreeDepth_ > 16) { @@ -149,7 +158,6 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s octomapTreeDepth_ = 16; } ROS_INFO("%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_); -#endif #endif // If true, the last message published on @@ -185,8 +193,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s scanMapPub_ = nht->advertise("scan_map", 1, latching_); latched_.insert(std::make_pair((void*)&scanMapPub_, false)); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) octoMapPubBin_ = nht->advertise("octomap_binary", 1, latching_); latched_.insert(std::make_pair((void*)&octoMapPubBin_, false)); octoMapPubFull_ = nht->advertise("octomap_full", 1, latching_); @@ -204,6 +211,10 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s octoMapProj_ = nht->advertise("octomap_grid", 1, latching_); latched_.insert(std::make_pair((void*)&octoMapProj_, false)); #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + elevationMapPub_ = nht->advertise("elevation_map", 1, latching_); + latched_.insert(std::make_pair((void*)&elevationMapPub_, false)); #endif } @@ -211,15 +222,14 @@ MapsManager::~MapsManager() { clear(); delete occupancyGrid_; + delete localMapMaker_; -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - if(octomap_) - { - delete octomap_; - octomap_ = 0; - } +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + delete octomap_; #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + delete elevationMap_; #endif } @@ -306,33 +316,32 @@ 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_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_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) parameterMoved(pnh, "octomap_ground_is_obstacle", Parameters::kGridGroundIsObstacle(), parameters); parameterMoved(pnh, "octomap_occupancy_thr", Parameters::kGridGlobalOccupancyThr(), parameters); #endif -#endif } void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) { parameters_ = parameters; - occupancyGrid_->parseParameters(parameters_); + delete occupancyGrid_; + occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_); + + localMapMaker_->parseParameters(parameters_); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - if(octomap_) - { - delete octomap_; - octomap_ = 0; - } - octomap_ = new OctoMap(parameters_); +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + delete octomap_; + octomap_ = new OctoMap(&localMaps_, parameters_); #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + delete elevationMap_; + elevationMap_ = new GridMap(&localMaps_, parameters_); #endif } @@ -350,8 +359,8 @@ void MapsManager::set2DMap( { for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) { - std::map, cv::Mat> >::iterator jter = gridMaps_.find(iter->first); - if(!uContains(gridMaps_, iter->first)) + std::map::const_iterator jter = localMaps_.find(iter->first); + if(!uContains(localMaps_.localGrids(), iter->first)) { rtabmap::SensorData data; data = memory->getNodeData(iter->first, false, false, false, true); @@ -371,23 +380,16 @@ void MapsManager::set2DMap( &obstacles, &emptyCells); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, data.gridViewPoint())); - occupancyGrid_->addToCache(iter->first, ground, obstacles, emptyCells); + localMaps_.add(iter->first, ground, obstacles, emptyCells, data.gridCellSize(), data.gridViewPoint()); } } - else - { - occupancyGrid_->addToCache(iter->first, jter->second.first.first, jter->second.first.second, jter->second.second); - } } } } void MapsManager::clear() { - gridMaps_.clear(); - gridMapsViewpoints_.clear(); + localMaps_.clear(); assembledGround_->clear(); assembledObstacles_->clear(); assembledGroundPoses_.clear(); @@ -397,10 +399,11 @@ void MapsManager::clear() groundClouds_.clear(); obstacleClouds_.clear(); occupancyGrid_->clear(); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) octomap_->clear(); #endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + elevationMap_->clear(); #endif for(std::map::iterator iter=latched_.begin(); iter!=latched_.end(); ++iter) { @@ -458,7 +461,8 @@ std::map MapsManager::updateMapCaches( const std::map & signatures) { bool updateGridCache = updateGrid || updateOctomap; - if(!updateGrid && !updateOctomap) + bool updateElevation = false; + if(!updateGrid && !updateOctomap && !updateOctomap) { // all false, update only those where we have subscribers updateOctomap = @@ -475,22 +479,25 @@ std::map MapsManager::updateMapCaches( gridMapPub_.getNumSubscribers() != 0 || gridProbMapPub_.getNumSubscribers() != 0; - updateGridCache = updateOctomap || updateGrid || + updateElevation = elevationMapPub_.getNumSubscribers() != 0; + + updateGridCache = updateOctomap || updateGrid || updateElevation || cloudMapPub_.getNumSubscribers() != 0 || cloudObstaclesPub_.getNumSubscribers() != 0 || cloudGroundPub_.getNumSubscribers() != 0 || scanMapPub_.getNumSubscribers() != 0; } -#ifndef WITH_OCTOMAP_MSGS +#if not (defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)) updateOctomap = false; #endif -#ifndef RTABMAP_OCTOMAP - updateOctomap = false; +#if not (defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)) + updateElevation = false; #endif gridUpdated_ = updateGrid; octomapUpdated_ = updateOctomap; + elevationMapUpdated_ = updateElevation; UDEBUG("Updating map caches..."); @@ -542,19 +549,17 @@ std::map MapsManager::updateMapCaches( UTimer longUpdateTimer; if(filteredPoses.size() > 20) { - if(updateGridCache && gridMaps_.size() < 5) + if(updateGridCache && localMaps_.size() < 5) { - ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size())); + ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); longUpdate = true; } -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) if(updateOctomap && octomap_->addedNodes().size() < 5) { ROS_WARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size())); longUpdate = true; } -#endif #endif } @@ -565,7 +570,7 @@ std::map MapsManager::updateMapCaches( if(!iter->second.isNull()) { rtabmap::SensorData data; - if(updateGridCache && (iter->first == 0 || !uContains(gridMaps_, iter->first))) + if(updateGridCache && (iter->first == 0 || !uContains(localMaps_.localGrids(), iter->first))) { ROS_DEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); @@ -575,7 +580,7 @@ std::map MapsManager::updateMapCaches( } else if(memory) { - data = memory->getNodeData(iter->first, occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, !occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, false, true); + data = memory->getNodeData(iter->first, localMapMaker_->isGridFromDepth() && !occupancySavedInDB, !localMapMaker_->isGridFromDepth() && !occupancySavedInDB, false, true); } ROS_DEBUG("Adding grid map %d to cache...", iter->first); @@ -604,12 +609,12 @@ std::map MapsManager::updateMapCaches( { // if we are here, it is because we loaded a database with old nodes not having occupancy grid set // try reload again - data = memory->getNodeData(iter->first, occupancyGrid_->isGridFromDepth(), !occupancyGrid_->isGridFromDepth(), false, false); + data = memory->getNodeData(iter->first, localMapMaker_->isGridFromDepth(), !localMapMaker_->isGridFromDepth(), false, false); } data.uncompressData( - occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0, - occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0, - !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0, + localMapMaker_->isGridFromDepth() && generateGrid?&rgb:0, + localMapMaker_->isGridFromDepth() && generateGrid?&depth:0, + !localMapMaker_->isGridFromDepth() && generateGrid?&scan:0, 0, generateGrid?0:&ground, generateGrid?0:&obstacles, @@ -619,15 +624,13 @@ std::map MapsManager::updateMapCaches( { Signature tmp(data); tmp.setPose(iter->second); - occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } else { viewPoint = data.gridViewPoint(); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } } else @@ -641,16 +644,16 @@ std::map MapsManager::updateMapCaches( { ParametersMap parameters; parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(scanEmptyRayTracing_))); - occupancyGrid_->parseParameters(parameters); + localMapMaker_->parseParameters(parameters); } cv::Mat rgb, depth; LaserScan scan; bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_); data.uncompressData( - occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0, - occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0, - !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0, + localMapMaker_->isGridFromDepth() && generateGrid?&rgb:0, + localMapMaker_->isGridFromDepth() && generateGrid?&depth:0, + !localMapMaker_->isGridFromDepth() && generateGrid?&scan:0, 0, generateGrid?0:&ground, generateGrid?0:&obstacles, @@ -660,15 +663,13 @@ std::map MapsManager::updateMapCaches( { Signature tmp(data); tmp.setPose(iter->second); - occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } else { viewPoint = data.gridViewPoint(); - uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } // put back @@ -676,52 +677,10 @@ std::map MapsManager::updateMapCaches( { ParametersMap parameters; parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled))); - occupancyGrid_->parseParameters(parameters); + localMapMaker_->parseParameters(parameters); } } } - - if(updateGrid && - (iter->first == 0 || - occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end())) - { - std::map, cv::Mat> >::iterator mter = gridMaps_.find(iter->first); - if(mter != gridMaps_.end()) - { - if(!mter->second.first.first.empty() || !mter->second.first.second.empty() || !mter->second.second.empty()) - { - occupancyGrid_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second); - } - } - } - -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP - if(updateOctomap && - (iter->first == 0 || - octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end())) - { - std::map, cv::Mat> >::iterator mter = gridMaps_.find(iter->first); - std::map::iterator pter = gridMapsViewpoints_.find(iter->first); - if(mter != gridMaps_.end() && pter!=gridMapsViewpoints_.end()) - { - if((mter->second.first.first.empty() || mter->second.first.first.channels() > 2) && - (mter->second.first.second.empty() || mter->second.first.second.channels() > 2) && - (mter->second.second.empty() || mter->second.second.channels() > 2)) - { - octomap_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second, pter->second); - } - else if(!mter->second.first.first.empty() && !mter->second.first.second.empty() && !mter->second.second.empty()) - { - ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. " - "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " - "all occupancy grid parameters.", - iter->first); - } - } - } -#endif -#endif } else { @@ -734,8 +693,7 @@ std::map MapsManager::updateMapCaches( gridUpdated_ = occupancyGrid_->update(filteredPoses); } -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) if(updateOctomap) { UTimer time; @@ -743,20 +701,17 @@ std::map MapsManager::updateMapCaches( ROS_INFO("Octomap update time = %fs", time.ticks()); } #endif -#endif - for(std::map, cv::Mat> >::iterator iter=gridMaps_.begin(); - iter!=gridMaps_.end();) + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + if(updateElevation) { - if(!uContains(poses, iter->first)) - { - UASSERT(gridMapsViewpoints_.erase(iter->first) != 0); - gridMaps_.erase(iter++); - } - else - { - ++iter; - } + UTimer time; + elevationMapUpdated_ = elevationMap_->update(filteredPoses); + ROS_INFO("GridMap (elevation map) update time = %fs", time.ticks()); } +#endif + + localMaps_.clear(true); for(std::map::Ptr >::iterator iter=groundClouds_.begin(); iter!=groundClouds_.end();) @@ -979,10 +934,6 @@ void MapsManager::publishMaps( } ++countObstacles; } - else - { - std::map, cv::Mat> >::iterator jter = gridMaps_.find(iter->first); - } } } double addingPointsTime = t.ticks(); @@ -1005,16 +956,16 @@ void MapsManager::publishMaps( for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { - std::map, cv::Mat> >::iterator jter = gridMaps_.find(iter->first); + std::map::const_iterator jter = localMaps_.localGrids().find(iter->first); if(updateGround && assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end()) { if(iter->first > 0) { assembledGroundPoses_.insert(*iter); } - if(jter!=gridMaps_.end() && jter->second.first.first.cols) + if(jter!=localMaps_.end() && jter->second.groundCells.cols) { - pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.first), iter->second, 0, 255, 0); + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.groundCells), iter->second, 0, 255, 0); pcl::PointCloud::Ptr subtractedCloud = transformed; if(cloudSubtractFiltering_) { @@ -1059,9 +1010,9 @@ void MapsManager::publishMaps( { assembledObstaclePoses_.insert(*iter); } - if(jter!=gridMaps_.end() && jter->second.first.second.cols) + if(jter!=localMaps_.end() && jter->second.obstacleCells.cols) { - pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.second), iter->second, 255, 0, 0); + pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.obstacleCells), iter->second, 255, 0, 0); pcl::PointCloud::Ptr subtractedCloud = transformed; if(cloudSubtractFiltering_) { @@ -1214,8 +1165,7 @@ void MapsManager::publishMaps( latched_.at(&cloudObstaclesPub_) = false; } -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) if( octomapUpdated_ || !latching_ || (octoMapPubBin_.getNumSubscribers() && !latched_.at(&octoMapPubBin_)) || @@ -1405,8 +1355,6 @@ void MapsManager::publishMaps( { latched_.at(&octoMapProj_) = false; } - -#endif #endif if( gridUpdated_ || @@ -1469,7 +1417,7 @@ void MapsManager::publishMaps( } else if(poses.size()) { - ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size()); + ROS_WARN("Grid map is empty! (local maps=%d)", (int)localMaps_.size()); } } if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers()) @@ -1515,7 +1463,7 @@ void MapsManager::publishMaps( } else if(poses.size()) { - ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size()); + ROS_WARN("Grid map is empty! (local maps=%d)", (int)localMaps_.size()); } } } @@ -1533,23 +1481,34 @@ void MapsManager::publishMaps( latched_.at(&gridProbMapPub_) = false; } + if( elevationMapUpdated_ || + !latching_ || + (elevationMapPub_.getNumSubscribers() && !latched_.at(&elevationMapPub_))) + { + grid_map_msgs::GridMap msg; + grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap(), msg); + msg.info.header.frame_id = mapFrameId; + msg.info.header.stamp = stamp; + elevationMapPub_.publish(msg); + } + if(elevationMapPub_.getNumSubscribers() == 0) + { + latched_.at(&elevationMapPub_) = false; + } + if( mapCacheCleanup_ && + elevationMapPub_.getNumSubscribers() == 0) + { + elevationMap_->clear(); + } + if(!this->hasSubscribers() && mapCacheCleanup_) { - if(!gridMaps_.empty()) + if(!localMaps_.empty()) { - size_t totalBytes = 0; - for(std::map, cv::Mat> >::iterator iter=gridMaps_.begin(); iter!=gridMaps_.end(); ++iter) - { - totalBytes+= sizeof(int)+ - iter->second.first.first.total()*iter->second.first.first.elemSize() + - iter->second.first.second.total()*iter->second.first.second.elemSize() + - iter->second.second.total()*iter->second.second.elemSize(); - } - totalBytes += gridMapsViewpoints_.size()*sizeof(int) + gridMapsViewpoints_.size() * sizeof(cv::Point3f); - ROS_INFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", gridMaps_.size(), totalBytes/1048576); + size_t totalBytes = localMaps_.getMemoryUsed(); + ROS_INFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", localMaps_.size(), totalBytes/1048576); } - gridMaps_.clear(); - gridMapsViewpoints_.clear(); + localMaps_.clear(); } } diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index 9474bccce..1fd0038b8 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -40,7 +41,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "rtabmap/core/OccupancyGrid.h" #include "rtabmap/utilite/UStl.h" namespace rtabmap_util @@ -226,7 +226,7 @@ class ObstaclesDetection : public nodelet::Nodelet NODELET_ERROR("obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str()); } - grid_.parseParameters(parameters); + localMapMaker_.parseParameters(parameters); cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this); @@ -325,7 +325,7 @@ class ObstaclesDetection : public nodelet::Nodelet inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform); pcl::IndicesPtr flatObstacles(new std::vector); - pcl::PointCloud::Ptr cloud = grid_.segmentCloud( + pcl::PointCloud::Ptr cloud = localMapMaker_.segmentCloud( inputCloud, pcl::IndicesPtr(new std::vector), pose, @@ -441,7 +441,7 @@ class ObstaclesDetection : public nodelet::Nodelet std::string mapFrameId_; bool waitForTransform_; - rtabmap::OccupancyGrid grid_; + rtabmap::LocalGridMaker localMapMaker_; bool mapFrameProjection_; bool warned_;