i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated with upstream changes

Browse Source
This commit is contained in:
matlabbe
2023-12-17 22:46:23 -08:00
parent e2b70a6707
commit 2f365ab7e1
5 changed files with 57 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/include/rtabmap_util/MapsManager.h
+5 -4
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/FlannIndex.h>
#include <rtabmap/core/LocalGrid.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <rclcpp/rclcpp.hpp>
@@ -47,6 +48,7 @@ namespace rtabmap {
class OctoMap;
class Memory;
class OccupancyGrid;
class LocalGridMaker;
} // namespace rtabmap
@@ -94,6 +96,7 @@ public:
const rtabmap::OctoMap * getOctomap() const {return octomap_;}
#endif
const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;}
private:
// mapping stuff
@@ -133,12 +136,10 @@ private:
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > groundClouds_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > obstacleClouds_;
std::map<int, rtabmap::Transform> gridPoses_;
cv::Mat gridMap_;
std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> > gridMaps_; // < <ground, obstacles>, empty cells >
std::map<int, cv::Point3f> gridMapsViewpoints_;
rtabmap::LocalGridCache localMaps_;
rtabmap::OccupancyGrid * occupancyGrid_;
rtabmap::LocalGridMaker * localMapMaker_;
bool gridUpdated_;
#ifdef RTABMAP_OCTOMAP
rtabmap_util/include/rtabmap_util/obstacles_detection.hpp
+2 -2
View File
@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <rtabmap/core/OccupancyGrid.h>
#include <rtabmap/core/LocalGridMaker.h>
namespace rtabmap_util
{
@@ -53,7 +53,7 @@ private:
std::string mapFrameId_;
double waitForTransform_;
rtabmap::OccupancyGrid grid_;
rtabmap::LocalGridMaker localMapMaker_;
bool mapFrameProjection_;
bool warned_;
rtabmap_util/src/MapsManager.cpp
+46 -125
View File
@@ -38,10 +38,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Version.h>
#include <rtabmap/core/OccupancyGrid.h>
#include <pcl/search/kdtree.h>
#include <pcl_conversions/pcl_conversions.h>
#include <rtabmap/core/LocalGridMaker.h>
#ifdef RTABMAP_OCTOMAP
#ifdef WITH_OCTOMAP_MSGS
@@ -66,10 +66,11 @@ MapsManager::MapsManager() :
scanEmptyRayTracing_(true),
assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
occupancyGrid_(new OccupancyGrid),
occupancyGrid_(new OccupancyGrid(&localMaps_)),
localMapMaker_(new LocalGridMaker),
gridUpdated_(true),
#ifdef RTABMAP_OCTOMAP
octomap_(new OctoMap),
octomap_(new OctoMap(&localMaps_)),
#endif
octomapTreeDepth_(16),
octomapUpdated_(true),
@@ -162,13 +163,10 @@ MapsManager::~MapsManager() {
clear();
delete occupancyGrid_;
delete localMapMaker_;
#ifdef RTABMAP_OCTOMAP
if(octomap_)
{
delete octomap_;
octomap_ = 0;
}
delete octomap_;
#endif
}
@@ -223,7 +221,6 @@ void MapsManager::backwardCompatibilityParameters(rclcpp::Node & node, Parameter
parameterMoved(node, "proj_map_frame", Parameters::kGridMapFrameProjection(), parameters);
parameterMoved(node, "grid_unknown_space_filled", Parameters::kGridScan2dUnknownSpaceFilled(), parameters);
parameterMoved(node, "grid_cell_size", Parameters::kGridCellSize(), parameters);
parameterMoved(node, "grid_incremental", Parameters::kGridGlobalFullUpdate(), parameters);
parameterMoved(node, "grid_size", Parameters::kGridGlobalMinSize(), parameters);
parameterMoved(node, "grid_eroded", Parameters::kGridGlobalEroded(), parameters);
parameterMoved(node, "grid_footprint_radius", Parameters::kGridGlobalFootprintRadius(), parameters);
@@ -237,15 +234,14 @@ void MapsManager::backwardCompatibilityParameters(rclcpp::Node & node, Parameter
void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
{
parameters_ = parameters;
occupancyGrid_->parseParameters(parameters_);
delete occupancyGrid_;
occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_);
localMapMaker_->parseParameters(parameters_);
#ifdef RTABMAP_OCTOMAP
if(octomap_)
{
delete octomap_;
octomap_ = 0;
}
octomap_ = new OctoMap(parameters_);
delete octomap_;
octomap_ = new OctoMap(&localMaps_, parameters_);
#endif
}
@@ -263,8 +259,8 @@ void MapsManager::set2DMap(
{
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
{
std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
if(!uContains(gridMaps_, iter->first))
std::map<int, LocalGrid>::const_iterator jter = localMaps_.find(iter->first);
if(jter == localMaps_.end())
{
rtabmap::SensorData data;
data = memory->getNodeData(iter->first, false, false, false, true);
@@ -284,23 +280,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();
@@ -455,9 +444,9 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
UTimer longUpdateTimer;
if(filteredPoses.size() > 20)
{
if(updateGridCache && gridMaps_.size() < 5)
if(updateGridCache && localMaps_.size() < 5)
{
UWARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size()));
UWARN("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 RTABMAP_OCTOMAP
@@ -476,7 +465,7 @@ std::map<int, rtabmap::Transform> 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)))
{
UDEBUG("Data required for %d", iter->first);
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
@@ -486,7 +475,7 @@ std::map<int, rtabmap::Transform> 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);
}
UDEBUG("Adding grid map %d to cache...", iter->first);
@@ -515,12 +504,12 @@ std::map<int, rtabmap::Transform> 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,
@@ -530,15 +519,12 @@ std::map<int, rtabmap::Transform> 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, data.gridCellSize(), data.gridViewPoint());
}
}
else
@@ -552,16 +538,16 @@ std::map<int, rtabmap::Transform> 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,
@@ -571,15 +557,12 @@ std::map<int, rtabmap::Transform> 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, data.gridCellSize(), data.gridViewPoint());
}
// put back
@@ -587,50 +570,10 @@ std::map<int, rtabmap::Transform> 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<int, std::pair<std::pair<cv::Mat, cv::Mat>, 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 RTABMAP_OCTOMAP
if(updateOctomap &&
(iter->first == 0 ||
octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end()))
{
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
std::map<int, cv::Point3f>::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())
{
UWARN("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
}
else
{
@@ -651,19 +594,6 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
UINFO("Octomap update time = %fs", time.ticks());
}
#endif
for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=gridMaps_.begin();
iter!=gridMaps_.end();)
{
if(!uContains(poses, iter->first))
{
UASSERT(gridMapsViewpoints_.erase(iter->first) != 0);
gridMaps_.erase(iter++);
}
else
{
++iter;
}
}
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();
iter!=groundClouds_.end();)
@@ -892,16 +822,16 @@ void MapsManager::publishMaps(
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
std::map<int, LocalGrid>::const_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.groundCells.cols)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.first), iter->second, 0, 255, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.groundCells), iter->second, 0, 255, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
if(cloudSubtractFiltering_)
{
@@ -946,9 +876,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<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.second), iter->second, 255, 0, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.obstacleCells), iter->second, 255, 0, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
if(cloudSubtractFiltering_)
{
@@ -1333,7 +1263,7 @@ void MapsManager::publishMaps(
}
else if(poses.size())
{
UWARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
UWARN("Grid map is empty! (local maps=%ld)", localMaps_.size());
}
}
if(gridMapPub_->get_subscription_count())
@@ -1371,7 +1301,7 @@ void MapsManager::publishMaps(
}
else if(poses.size())
{
UWARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
UWARN("Grid map is empty! (local maps=%ld)", localMaps_.size());
}
}
}
@@ -1387,21 +1317,12 @@ void MapsManager::publishMaps(
if(!this->hasSubscribers() && mapCacheCleanup_)
{
if(!gridMaps_.empty())
if(!localMaps_.empty())
{
size_t totalBytes = 0;
for(std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, 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);
UINFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", gridMaps_.size(), totalBytes/1048576);
size_t totalBytes = localMaps_.getMemoryUsed();
UINFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", localMaps_.size(), totalBytes/1048576);
}
gridMaps_.clear();
gridMapsViewpoints_.clear();
localMaps_.clear();
}
}
rtabmap_util/src/nodelets/obstacles_detection.cpp
+2 -2
View File
@@ -73,7 +73,7 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
RCLCPP_ERROR(this->get_logger(), "obstacles_detection: Parameter \"%s\" is true but map_frame_id is not set!", rtabmap::Parameters::kGridMapFrameProjection().c_str());
}
grid_.parseParameters(gridParameters);
localMapMaker_.parseParameters(gridParameters);
tfBuffer_ = std::make_shared< tf2_ros::Buffer >(this->get_clock());
tfListener_ = std::make_shared< tf2_ros::TransformListener >(*tfBuffer_);
@@ -149,7 +149,7 @@ void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstShar
inputCloud = rtabmap::util3d::transformPointCloud(inputCloud, localTransform);
pcl::IndicesPtr flatObstacles(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = grid_.segmentCloud<pcl::PointXYZ>(
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = localMapMaker_.segmentCloud<pcl::PointXYZ>(
inputCloud,
pcl::IndicesPtr(new std::vector<int>),
pose,