From c8f1f598cd0e09da7ce7914d7647e676d120efdc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 10 Dec 2023 22:49:31 -0800 Subject: [PATCH 1/4] GridMap integration --- 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 | 251 +++++++++++------- .../src/nodelets/obstacles_detection.cpp | 8 +- 6 files changed, 211 insertions(+), 112 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 0c565782e..cd5c2413e 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -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..7331fd123 100644 --- a/rtabmap_util/include/rtabmap_util/MapsManager.h +++ b/rtabmap_util/include/rtabmap_util/MapsManager.h @@ -40,6 +40,8 @@ namespace rtabmap { class OctoMap; class Memory; class OccupancyGrid; +class LocalMapMaker; +class GridMap; } // namespace rtabmap @@ -85,6 +87,7 @@ class MapsManager { const rtabmap::OctoMap * getOctomap() const {return octomap_;} const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;} + const rtabmap::LocalMapMaker * getLocalMapMaker() const {return localMapMaker_;} private: // mapping stuff @@ -112,6 +115,7 @@ class MapsManager { ros::Publisher octoMapObstacleCloud_; ros::Publisher octoMapEmptySpace_; ros::Publisher octoMapProj_; + ros::Publisher elevationMapPub_; std::map assembledGroundPoses_; std::map assembledObstaclePoses_; @@ -122,18 +126,20 @@ 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_; + std::map, cv::Mat> > localMaps_; // < , empty cells > + std::map localMapsViewpoints_; rtabmap::OccupancyGrid * occupancyGrid_; + rtabmap::LocalMapMaker * 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..979ab860a 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -46,12 +47,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #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; @@ -70,14 +74,21 @@ MapsManager::MapsManager() : assembledObstacles_(new pcl::PointCloud), assembledGround_(new pcl::PointCloud), occupancyGrid_(new OccupancyGrid), + localMapMaker_(new LocalMapMaker), gridUpdated_(true), -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) octomap_(new OctoMap), -#endif +#else + octomap_(0), #endif octomapTreeDepth_(16), octomapUpdated_(true), +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + elevationMap_(new GridMap), +#else + elevationMap_(0), +#endif + elevationMapUpdated_(true), latching_(true) { } @@ -135,8 +146,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 +159,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 +194,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 +212,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 +223,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 } @@ -311,21 +322,19 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet 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_); + occupancyGrid_ = new OccupancyGrid(parameters_); + localMapMaker_ = new LocalMapMaker(parameters_); -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) if(octomap_) { delete octomap_; @@ -333,6 +342,14 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) } octomap_ = new OctoMap(parameters_); #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + if(elevationMap_) + { + delete elevationMap_; + elevationMap_ = 0; + } + elevationMap_ = new GridMap(parameters_); #endif } @@ -350,8 +367,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, cv::Mat> >::iterator jter = localMaps_.find(iter->first); + if(!uContains(localMaps_, iter->first)) { rtabmap::SensorData data; data = memory->getNodeData(iter->first, false, false, false, true); @@ -371,8 +388,8 @@ 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())); + uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); + uInsert(localMapsViewpoints_, std::make_pair(iter->first, data.gridViewPoint())); occupancyGrid_->addToCache(iter->first, ground, obstacles, emptyCells); } } @@ -386,8 +403,8 @@ void MapsManager::set2DMap( void MapsManager::clear() { - gridMaps_.clear(); - gridMapsViewpoints_.clear(); + localMaps_.clear(); + localMapsViewpoints_.clear(); assembledGround_->clear(); assembledObstacles_->clear(); assembledGroundPoses_.clear(); @@ -397,10 +414,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 +476,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 +494,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 +564,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 +585,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_, iter->first))) { ROS_DEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); @@ -575,7 +595,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 +624,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 +639,15 @@ 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); + uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); + uInsert(localMapsViewpoints_, std::make_pair(iter->first, 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)); + uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); + uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint)); } } else @@ -641,16 +661,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 +680,15 @@ 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); + uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); + uInsert(localMapsViewpoints_, std::make_pair(iter->first, 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)); + uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); + uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint)); } // put back @@ -676,7 +696,7 @@ std::map MapsManager::updateMapCaches( { ParametersMap parameters; parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled))); - occupancyGrid_->parseParameters(parameters); + localMapMaker_->parseParameters(parameters); } } } @@ -685,8 +705,8 @@ std::map MapsManager::updateMapCaches( (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()) + std::map, cv::Mat> >::iterator mter = localMaps_.find(iter->first); + if(mter != localMaps_.end()) { if(!mter->second.first.first.empty() || !mter->second.first.second.empty() || !mter->second.second.empty()) { @@ -695,15 +715,14 @@ std::map MapsManager::updateMapCaches( } } -#ifdef WITH_OCTOMAP_MSGS -#ifdef RTABMAP_OCTOMAP +#if defined(WITH_OCTOMAP_MSGS) and defined(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()) + std::map, cv::Mat> >::iterator mter = localMaps_.find(iter->first); + std::map::iterator pter = localMapsViewpoints_.find(iter->first); + if(mter != localMaps_.end() && pter!=localMapsViewpoints_.end()) { if((mter->second.first.first.empty() || mter->second.first.first.channels() > 2) && (mter->second.first.second.empty() || mter->second.first.second.channels() > 2) && @@ -721,6 +740,31 @@ std::map MapsManager::updateMapCaches( } } #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + if(updateElevation && + (iter->first == 0 || + elevationMap_->addedNodes().find(iter->first) == elevationMap_->addedNodes().end())) + { + std::map, cv::Mat> >::iterator mter = localMaps_.find(iter->first); + std::map::iterator pter = localMapsViewpoints_.find(iter->first); + if(mter != localMaps_.end() && pter!=localMapsViewpoints_.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)) + { + elevationMap_->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 elevation map with 2D occupancy grids. " + "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " + "all occupancy grid parameters.", + iter->first); + } + } + } #endif } else @@ -734,8 +778,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,14 +786,23 @@ std::map MapsManager::updateMapCaches( ROS_INFO("Octomap update time = %fs", time.ticks()); } #endif + +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + if(updateElevation) + { + UTimer time; + elevationMapUpdated_ = elevationMap_->update(filteredPoses); + ROS_INFO("GridMap (elevation map) update time = %fs", time.ticks()); + } #endif - for(std::map, cv::Mat> >::iterator iter=gridMaps_.begin(); - iter!=gridMaps_.end();) + + for(std::map, cv::Mat> >::iterator iter=localMaps_.begin(); + iter!=localMaps_.end();) { if(!uContains(poses, iter->first)) { - UASSERT(gridMapsViewpoints_.erase(iter->first) != 0); - gridMaps_.erase(iter++); + UASSERT(localMapsViewpoints_.erase(iter->first) != 0); + localMaps_.erase(iter++); } else { @@ -981,7 +1033,7 @@ void MapsManager::publishMaps( } else { - std::map, cv::Mat> >::iterator jter = gridMaps_.find(iter->first); + std::map, cv::Mat> >::iterator jter = localMaps_.find(iter->first); } } } @@ -1005,14 +1057,14 @@ 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, cv::Mat> >::iterator jter = localMaps_.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.first.first.cols) { pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.first), iter->second, 0, 255, 0); pcl::PointCloud::Ptr subtractedCloud = transformed; @@ -1059,7 +1111,7 @@ void MapsManager::publishMaps( { assembledObstaclePoses_.insert(*iter); } - if(jter!=gridMaps_.end() && jter->second.first.second.cols) + if(jter!=localMaps_.end() && jter->second.first.second.cols) { pcl::PointCloud::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.second), iter->second, 255, 0, 0); pcl::PointCloud::Ptr subtractedCloud = transformed; @@ -1214,8 +1266,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 +1456,6 @@ void MapsManager::publishMaps( { latched_.at(&octoMapProj_) = false; } - -#endif #endif if( gridUpdated_ || @@ -1469,7 +1518,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 +1564,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 +1582,43 @@ 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) + for(std::map, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.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); + totalBytes += localMapsViewpoints_.size()*sizeof(int) + localMapsViewpoints_.size() * sizeof(cv::Point3f); + ROS_INFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", localMaps_.size(), totalBytes/1048576); } - gridMaps_.clear(); - gridMapsViewpoints_.clear(); + localMaps_.clear(); + localMapsViewpoints_.clear(); } } diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index 9474bccce..16e7027f8 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "rtabmap/core/OccupancyGrid.h" +#include "rtabmap/core/LocalMapMaker.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::LocalMapMaker localMapMaker_; bool mapFrameProjection_; bool warned_; From 596f637b1bd98a7a408f8d572ad1e251864c7eea Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 16 Dec 2023 17:35:04 -0800 Subject: [PATCH 2/4] Updated with upstream changes --- rtabmap_util/src/MapsManager.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 979ab860a..59f39208f 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -55,7 +55,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) #include -#include +#include #endif using namespace rtabmap; @@ -317,7 +317,6 @@ 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); From be0736c1816cf9032ea79ef1111fe66cf5f1d560 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 17 Dec 2023 19:29:44 -0800 Subject: [PATCH 3/4] updated with upstream changes --- rtabmap_slam/src/CoreWrapper.cpp | 2 +- .../include/rtabmap_util/MapsManager.h | 10 +- rtabmap_util/src/MapsManager.cpp | 151 +++--------------- .../src/nodelets/obstacles_detection.cpp | 4 +- 4 files changed, 32 insertions(+), 135 deletions(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index cd5c2413e..9f09ab599 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -57,10 +57,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include #include #include +#include #include #ifdef WITH_OCTOMAP_MSGS diff --git a/rtabmap_util/include/rtabmap_util/MapsManager.h b/rtabmap_util/include/rtabmap_util/MapsManager.h index 7331fd123..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,7 +41,7 @@ namespace rtabmap { class OctoMap; class Memory; class OccupancyGrid; -class LocalMapMaker; +class LocalGridMaker; class GridMap; } // namespace rtabmap @@ -87,7 +88,7 @@ class MapsManager { const rtabmap::OctoMap * getOctomap() const {return octomap_;} const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;} - const rtabmap::LocalMapMaker * getLocalMapMaker() const {return localMapMaker_;} + const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;} private: // mapping stuff @@ -126,11 +127,10 @@ class MapsManager { std::map::Ptr > groundClouds_; std::map::Ptr > obstacleClouds_; - std::map, cv::Mat> > localMaps_; // < , empty cells > - std::map localMapsViewpoints_; + rtabmap::LocalGridCache localMaps_; rtabmap::OccupancyGrid * occupancyGrid_; - rtabmap::LocalMapMaker * localMapMaker_; + rtabmap::LocalGridMaker * localMapMaker_; bool gridUpdated_; rtabmap::OctoMap * octomap_; diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 59f39208f..a0770928e 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -38,14 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include - #include #include #include #include +#include #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) #include @@ -73,18 +72,18 @@ MapsManager::MapsManager() : scanEmptyRayTracing_(true), assembledObstacles_(new pcl::PointCloud), assembledGround_(new pcl::PointCloud), - occupancyGrid_(new OccupancyGrid), - localMapMaker_(new LocalMapMaker), + occupancyGrid_(new OccupancyGrid(&localMaps_)), + localMapMaker_(new LocalGridMaker), gridUpdated_(true), #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) - octomap_(new 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), + elevationMap_(new GridMap(&localMaps_)), #else elevationMap_(0), #endif @@ -330,8 +329,8 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) { parameters_ = parameters; - occupancyGrid_ = new OccupancyGrid(parameters_); - localMapMaker_ = new LocalMapMaker(parameters_); + occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_); + localMapMaker_ = new LocalGridMaker(parameters_); #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) if(octomap_) @@ -339,7 +338,7 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) delete octomap_; octomap_ = 0; } - octomap_ = new OctoMap(parameters_); + octomap_ = new OctoMap(&localMaps_, parameters_); #endif #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) @@ -348,7 +347,7 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) delete elevationMap_; elevationMap_ = 0; } - elevationMap_ = new GridMap(parameters_); + elevationMap_ = new GridMap(&localMaps_, parameters_); #endif } @@ -366,8 +365,8 @@ void MapsManager::set2DMap( { for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) { - std::map, cv::Mat> >::iterator jter = localMaps_.find(iter->first); - if(!uContains(localMaps_, 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); @@ -387,15 +386,9 @@ void MapsManager::set2DMap( &obstacles, &emptyCells); - uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(localMapsViewpoints_, 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); - } } } } @@ -403,7 +396,6 @@ void MapsManager::set2DMap( void MapsManager::clear() { localMaps_.clear(); - localMapsViewpoints_.clear(); assembledGround_->clear(); assembledObstacles_->clear(); assembledGroundPoses_.clear(); @@ -584,7 +576,7 @@ std::map MapsManager::updateMapCaches( if(!iter->second.isNull()) { rtabmap::SensorData data; - if(updateGridCache && (iter->first == 0 || !uContains(localMaps_, 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); @@ -639,14 +631,12 @@ std::map MapsManager::updateMapCaches( Signature tmp(data); tmp.setPose(iter->second); localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); - uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } else { viewPoint = data.gridViewPoint(); - uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } } else @@ -680,14 +670,12 @@ std::map MapsManager::updateMapCaches( Signature tmp(data); tmp.setPose(iter->second); localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint); - uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } else { viewPoint = data.gridViewPoint(); - uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells))); - uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint)); + localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint); } // put back @@ -699,72 +687,6 @@ std::map MapsManager::updateMapCaches( } } } - - if(updateGrid && - (iter->first == 0 || - occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end())) - { - std::map, cv::Mat> >::iterator mter = localMaps_.find(iter->first); - if(mter != localMaps_.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); - } - } - } - -#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) - if(updateOctomap && - (iter->first == 0 || - octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end())) - { - std::map, cv::Mat> >::iterator mter = localMaps_.find(iter->first); - std::map::iterator pter = localMapsViewpoints_.find(iter->first); - if(mter != localMaps_.end() && pter!=localMapsViewpoints_.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 - -#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) - if(updateElevation && - (iter->first == 0 || - elevationMap_->addedNodes().find(iter->first) == elevationMap_->addedNodes().end())) - { - std::map, cv::Mat> >::iterator mter = localMaps_.find(iter->first); - std::map::iterator pter = localMapsViewpoints_.find(iter->first); - if(mter != localMaps_.end() && pter!=localMapsViewpoints_.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)) - { - elevationMap_->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 elevation map with 2D occupancy grids. " - "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " - "all occupancy grid parameters.", - iter->first); - } - } - } -#endif } else { @@ -795,19 +717,7 @@ std::map MapsManager::updateMapCaches( } #endif - for(std::map, cv::Mat> >::iterator iter=localMaps_.begin(); - iter!=localMaps_.end();) - { - if(!uContains(poses, iter->first)) - { - UASSERT(localMapsViewpoints_.erase(iter->first) != 0); - localMaps_.erase(iter++); - } - else - { - ++iter; - } - } + localMaps_.clear(true); for(std::map::Ptr >::iterator iter=groundClouds_.begin(); iter!=groundClouds_.end();) @@ -1030,10 +940,6 @@ void MapsManager::publishMaps( } ++countObstacles; } - else - { - std::map, cv::Mat> >::iterator jter = localMaps_.find(iter->first); - } } } double addingPointsTime = t.ticks(); @@ -1056,16 +962,16 @@ void MapsManager::publishMaps( for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { - std::map, cv::Mat> >::iterator jter = localMaps_.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!=localMaps_.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_) { @@ -1110,9 +1016,9 @@ void MapsManager::publishMaps( { assembledObstaclePoses_.insert(*iter); } - if(jter!=localMaps_.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_) { @@ -1605,19 +1511,10 @@ void MapsManager::publishMaps( { if(!localMaps_.empty()) { - size_t totalBytes = 0; - for(std::map, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.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 += localMapsViewpoints_.size()*sizeof(int) + localMapsViewpoints_.size() * sizeof(cv::Point3f); + size_t totalBytes = localMaps_.getMemoryUsed(); ROS_INFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", localMaps_.size(), totalBytes/1048576); } localMaps_.clear(); - localMapsViewpoints_.clear(); } } diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index 16e7027f8..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/LocalMapMaker.h" #include "rtabmap/utilite/UStl.h" namespace rtabmap_util @@ -441,7 +441,7 @@ class ObstaclesDetection : public nodelet::Nodelet std::string mapFrameId_; bool waitForTransform_; - rtabmap::LocalMapMaker localMapMaker_; + rtabmap::LocalGridMaker localMapMaker_; bool mapFrameProjection_; bool warned_; From b7bcf35c4d1fae6870b9610a6980fe8bab13f525 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 17 Dec 2023 21:41:18 -0800 Subject: [PATCH 4/4] some fixes --- rtabmap_util/src/MapsManager.cpp | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index a0770928e..3b82b4340 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -329,24 +329,18 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet void MapsManager::setParameters(const rtabmap::ParametersMap & parameters) { parameters_ = parameters; + delete occupancyGrid_; occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_); - localMapMaker_ = new LocalGridMaker(parameters_); + + localMapMaker_->parseParameters(parameters_); #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) - if(octomap_) - { - delete octomap_; - octomap_ = 0; - } + delete octomap_; octomap_ = new OctoMap(&localMaps_, parameters_); #endif #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) - if(elevationMap_) - { - delete elevationMap_; - elevationMap_ = 0; - } + delete elevationMap_; elevationMap_ = new GridMap(&localMaps_, parameters_); #endif }