m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
Files
530a53641e2a000ad5575a751d62a879040adae8
agv_pro_ros2/rtabmap_ros/rtabmap_util/src/MapsManager.cpp
T
X-lanni 943ce5b06f feat(slam): add rtabmap_ros
2025-07-14 11:34:38 +08:00

1414 lines
52 KiB
C++
Raw Blame History

/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap_util/MapsManager.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/Memory.h>
#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
#include <octomap_msgs/conversions.h>
#endif
#include <octomap/ColorOcTree.h>
#include <rtabmap/core/OctoMap.h>
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <rtabmap/core/global_map/GridMap.h>
#endif
using namespace rtabmap;
namespace rtabmap_util {
MapsManager::MapsManager() :
cloudOutputVoxelized_(true),
cloudSubtractFiltering_(false),
cloudSubtractFilteringMinNeighbors_(2),
mapFilterRadius_(0.0),
mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true),
alwaysUpdateMap_(false),
scanEmptyRayTracing_(true),
assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
occupancyGrid_(new OccupancyGrid(&localMaps_)),
localMapMaker_(new LocalGridMaker),
gridUpdated_(true),
#ifdef RTABMAP_OCTOMAP
octomap_(new OctoMap(&localMaps_)),
#endif
octomapTreeDepth_(16),
octomapUpdated_(true),
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMap_(new GridMap(&localMaps_)),
#endif
elevationMapUpdated_(true),
latching_(true)
{
}
void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
{
// common map stuff
mapFilterRadius_ = node.declare_parameter("map_filter_radius", rclcpp::ParameterValue(mapFilterRadius_)).get<double>();
mapFilterAngle_ = node.declare_parameter("map_filter_angle", rclcpp::ParameterValue(mapFilterAngle_)).get<double>();
mapCacheCleanup_ = node.declare_parameter("map_cleanup", rclcpp::ParameterValue(mapCacheCleanup_)).get<bool>();
alwaysUpdateMap_ = node.declare_parameter("map_always_update", rclcpp::ParameterValue(alwaysUpdateMap_)).get<bool>();
scanEmptyRayTracing_ = node.declare_parameter("map_empty_ray_tracing", rclcpp::ParameterValue(scanEmptyRayTracing_)).get<bool>();
cloudOutputVoxelized_ = node.declare_parameter("cloud_output_voxelized", rclcpp::ParameterValue(cloudOutputVoxelized_)).get<bool>();
cloudSubtractFiltering_ = node.declare_parameter("cloud_subtract_filtering", rclcpp::ParameterValue(cloudSubtractFiltering_)).get<bool>();
cloudSubtractFilteringMinNeighbors_ = node.declare_parameter("cloud_subtract_filtering_min_neighbors", rclcpp::ParameterValue(cloudSubtractFilteringMinNeighbors_)).get<int>();
// If true, the last message published on
// the map topics will be saved and sent to new subscribers when they
// connect
latching_ = node.declare_parameter("latch", rclcpp::ParameterValue(latching_)).get<bool>();
RCLCPP_INFO(node.get_logger(), "%s(maps): latch = %s", name.c_str(), latching_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_radius = %f", name.c_str(), mapFilterRadius_);
RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_angle = %f", name.c_str(), mapFilterAngle_);
RCLCPP_INFO(node.get_logger(), "%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): map_always_update = %s", name.c_str(), alwaysUpdateMap_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): map_empty_ray_tracing = %s", name.c_str(), scanEmptyRayTracing_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_output_voxelized = %s", name.c_str(), cloudOutputVoxelized_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_);
#ifdef RTABMAP_OCTOMAP
octomapTreeDepth_ = node.declare_parameter("octomap_tree_depth", rclcpp::ParameterValue(octomapTreeDepth_)).get<int>();
if(octomapTreeDepth_ > 16)
{
RCLCPP_WARN(node.get_logger(), "octomap_tree_depth maximum is 16");
octomapTreeDepth_ = 16;
}
else if(octomapTreeDepth_ < 0)
{
RCLCPP_WARN(node.get_logger(), "octomap_tree_depth cannot be negative, set to 16 instead");
octomapTreeDepth_ = 16;
}
RCLCPP_INFO(node.get_logger(), "%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_);
#endif
// mapping topics
latched_.clear();
gridMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&gridMapPub_, false));
gridProbMapPub_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("grid_prob_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&gridProbMapPub_, false));
cloudMapPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&cloudMapPub_, false));
cloudObstaclesPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&cloudObstaclesPub_, false));
cloudGroundPub_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("cloud_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&cloudGroundPub_, false));
#ifdef RTABMAP_OCTOMAP
#ifdef WITH_OCTOMAP_MSGS
octoMapPubBin_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_binary", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&octoMapPubBin_, false));
octoMapPubFull_ = node.create_publisher<octomap_msgs::msg::Octomap>("octomap_full", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&octoMapPubFull_, false));
#endif
octoMapCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_occupied_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); // FIXME latching option in ROS2?
latched_.insert(std::make_pair((void*)&octoMapCloud_, false));
octoMapFrontierCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_global_frontier_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&octoMapFrontierCloud_, false));
octoMapObstacleCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_obstacles", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&octoMapObstacleCloud_, false));
octoMapGroundCloud_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_ground", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&octoMapGroundCloud_, false));
octoMapEmptySpace_ = node.create_publisher<sensor_msgs::msg::PointCloud2>("octomap_empty_space", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&octoMapEmptySpace_, false));
octoMapProj_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&octoMapProj_, false));
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMapPub_ = node.create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&elevationMapPub_, false));
#endif
}
MapsManager::~MapsManager() {
clear();
delete occupancyGrid_;
delete localMapMaker_;
#ifdef RTABMAP_OCTOMAP
delete octomap_;
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
delete elevationMap_;
#endif
}
void parameterMoved(
rclcpp::Node & node,
const std::string & rosName,
const std::string & parameterName,
ParametersMap & parameters)
{
rclcpp::Parameter p;
if(node.get_parameter(rosName, p))
{
ParametersMap::const_iterator iter = Parameters::getDefaultParameters().find(parameterName);
if(iter != Parameters::getDefaultParameters().end())
{
RCLCPP_WARN(node.get_logger(), "Parameter \"%s\" has moved from "
"rtabmap_ros to rtabmap library. Use "
"parameter \"%s\" instead. The value \"%s\" is still "
"copied to new parameter name.",
rosName.c_str(),
parameterName.c_str(),
p.value_to_string().c_str());
parameters.insert(ParametersPair(parameterName, p.value_to_string()));
}
else
{
RCLCPP_ERROR(node.get_logger(), "Parameter \"%s\" not found in default parameters.", parameterName.c_str());
}
}
}
void MapsManager::backwardCompatibilityParameters(rclcpp::Node & node, ParametersMap & parameters) const
{
// moved
parameterMoved(node, "cloud_decimation", Parameters::kGridDepthDecimation(), parameters);
parameterMoved(node, "cloud_max_depth", Parameters::kGridRangeMax(), parameters);
parameterMoved(node, "cloud_min_depth", Parameters::kGridRangeMin(), parameters);
parameterMoved(node, "cloud_voxel_size", Parameters::kGridCellSize(), parameters);
parameterMoved(node, "cloud_floor_culling_height", Parameters::kGridMaxGroundHeight(), parameters);
parameterMoved(node, "cloud_ceiling_culling_height", Parameters::kGridMaxObstacleHeight(), parameters);
parameterMoved(node, "cloud_noise_filtering_radius", Parameters::kGridNoiseFilteringRadius(), parameters);
parameterMoved(node, "cloud_noise_filtering_min_neighbors", Parameters::kGridNoiseFilteringMinNeighbors(), parameters);
parameterMoved(node, "scan_decimation", Parameters::kGridScanDecimation(), parameters);
parameterMoved(node, "scan_voxel_size", Parameters::kGridCellSize(), parameters);
parameterMoved(node, "proj_max_ground_angle", Parameters::kGridMaxGroundAngle(), parameters);
parameterMoved(node, "proj_min_cluster_size", Parameters::kGridMinClusterSize(), parameters);
parameterMoved(node, "proj_max_height", Parameters::kGridMaxObstacleHeight(), parameters);
parameterMoved(node, "proj_max_obstacles_height", Parameters::kGridMaxObstacleHeight(), parameters);
parameterMoved(node, "proj_max_ground_height", Parameters::kGridMaxGroundHeight(), parameters);
parameterMoved(node, "proj_detect_flat_obstacles", Parameters::kGridFlatObstacleDetected(), parameters);
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_size", Parameters::kGridGlobalMinSize(), parameters);
parameterMoved(node, "grid_eroded", Parameters::kGridGlobalEroded(), parameters);
parameterMoved(node, "grid_footprint_radius", Parameters::kGridGlobalFootprintRadius(), parameters);
#ifdef RTABMAP_OCTOMAP
parameterMoved(node, "octomap_ground_is_obstacle", Parameters::kGridGroundIsObstacle(), parameters);
parameterMoved(node, "octomap_occupancy_thr", Parameters::kGridGlobalOccupancyThr(), parameters);
#endif
}
void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
{
parameters_ = parameters;
delete occupancyGrid_;
occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_);
localMapMaker_->parseParameters(parameters_);
#ifdef 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
}
void MapsManager::set2DMap(
const cv::Mat & map,
float xMin,
float yMin,
float cellSize,
const std::map<int, rtabmap::Transform> & poses,
const rtabmap::Memory * memory)
{
occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses);
//update cache in case the map should be updated
if(memory)
{
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
{
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);
if(data.gridCellSize() == 0.0f)
{
UWARN("Local occupancy grid doesn't exist for node %d", iter->first);
}
else
{
cv::Mat ground, obstacles, emptyCells;
data.uncompressData(
0,
0,
0,
0,
&ground,
&obstacles,
&emptyCells);
localMaps_.add(iter->first, ground, obstacles, emptyCells, data.gridCellSize(), data.gridViewPoint());
}
}
}
}
}
void MapsManager::clear()
{
localMaps_.clear();
assembledGround_->clear();
assembledObstacles_->clear();
assembledGroundPoses_.clear();
assembledObstaclePoses_.clear();
assembledGroundIndex_.release();
assembledObstacleIndex_.release();
groundClouds_.clear();
obstacleClouds_.clear();
occupancyGrid_->clear();
#ifdef RTABMAP_OCTOMAP
octomap_->clear();
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMap_->clear();
#endif
for(std::map<void*, bool>::iterator iter=latched_.begin(); iter!=latched_.end(); ++iter)
{
iter->second = false;
}
}
bool MapsManager::hasSubscribers() const
{
return cloudMapPub_->get_subscription_count() != 0 ||
cloudObstaclesPub_->get_subscription_count() != 0 ||
cloudGroundPub_->get_subscription_count() != 0 ||
gridMapPub_->get_subscription_count() != 0 ||
gridProbMapPub_->get_subscription_count() != 0
#ifdef RTABMAP_OCTOMAP
||
#ifdef WITH_OCTOMAP_MSGS
octoMapPubBin_->get_subscription_count() != 0 ||
octoMapPubFull_->get_subscription_count() != 0 ||
#endif
octoMapCloud_->get_subscription_count() != 0 ||
octoMapFrontierCloud_->get_subscription_count() != 0 ||
octoMapObstacleCloud_->get_subscription_count() != 0 ||
octoMapGroundCloud_->get_subscription_count() != 0 ||
octoMapEmptySpace_->get_subscription_count() != 0 ||
octoMapProj_->get_subscription_count() != 0
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
|| elevationMapPub_->get_subscription_count() != 0
#endif
;
}
bool MapsManager::isMapUpdated() const
{
// We are currently using the check made in OccupancyGrid::update()
// to know if the map/graph changed. If there are no subscribers to
// any grid topics, we won't know it, so we will assume the graph
// changed.
return gridUpdated_ ||
(gridMapPub_->get_subscription_count() == 0 &&
gridProbMapPub_->get_subscription_count() == 0);
}
std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
{
if(mapFilterRadius_ > 0.0)
{
// filter nodes
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
return rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
}
return std::map<int, Transform>();
}
std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
const std::map<int, rtabmap::Transform> & posesIn,
const rtabmap::Memory * memory,
bool updateGrid,
bool updateOctomap,
const std::map<int, rtabmap::Signature> & signatures)
{
bool updateGridCache = updateGrid || updateOctomap;
bool updateElevation = false;
if(!updateGrid && !updateOctomap && !updateOctomap)
{
// all false, update only those where we have subscribers
#ifdef RTABMAP_OCTOMAP
updateOctomap =
#ifdef WITH_OCTOMAP_MSGS
octoMapPubBin_->get_subscription_count() != 0 ||
octoMapPubFull_->get_subscription_count() != 0 ||
#endif
octoMapCloud_->get_subscription_count() != 0 ||
octoMapFrontierCloud_->get_subscription_count() != 0 ||
octoMapObstacleCloud_->get_subscription_count() != 0 ||
octoMapGroundCloud_->get_subscription_count() != 0 ||
octoMapEmptySpace_->get_subscription_count() != 0 ||
octoMapProj_->get_subscription_count() != 0;
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
updateElevation = elevationMapPub_->get_subscription_count() != 0;
#endif
updateGrid = gridMapPub_->get_subscription_count() != 0 ||
gridProbMapPub_->get_subscription_count() != 0;
updateGridCache = updateOctomap || updateGrid || updateElevation ||
cloudMapPub_->get_subscription_count() != 0 ||
cloudObstaclesPub_->get_subscription_count() != 0 ||
cloudGroundPub_->get_subscription_count() != 0;
}
#if not (defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP))
updateOctomap = false;
#endif
#if not (defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP))
updateElevation = false;
#endif
gridUpdated_ = updateGrid;
octomapUpdated_ = updateOctomap;
elevationMapUpdated_ = updateElevation;
UDEBUG("Updating map caches...");
if(!memory && signatures.size() == 0)
{
UERROR("Memory and signatures should not be both null!?");
return std::map<int, rtabmap::Transform>();
}
// process only nodes (exclude landmarks)
std::map<int, rtabmap::Transform> poses;
if(posesIn.begin()->first < 0)
{
poses.insert(posesIn.lower_bound(0), posesIn.end());
}
else
{
poses = posesIn;
}
std::map<int, rtabmap::Transform> filteredPoses;
// update cache
if(updateGridCache)
{
// filter nodes
if(mapFilterRadius_ > 0.0)
{
UDEBUG("Filter nodes...");
double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
if(poses.find(0) != poses.end())
{
// make sure to keep latest data
filteredPoses.insert(*poses.find(0));
}
}
else
{
filteredPoses = poses;
}
if(!alwaysUpdateMap_)
{
filteredPoses.erase(0);
}
bool longUpdate = false;
UTimer longUpdateTimer;
if(filteredPoses.size() > 20)
{
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()-localMaps_.size()));
longUpdate = true;
}
#ifdef RTABMAP_OCTOMAP
if(updateOctomap && octomap_->addedNodes().size() < 5)
{
UWARN("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
}
bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false;
for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
{
if(!iter->second.isNull())
{
rtabmap::SensorData data;
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);
if(findIter != signatures.end())
{
data = findIter->second.sensorData();
}
else if(memory)
{
data = memory->getNodeData(iter->first, localMapMaker_->isGridFromDepth() && !occupancySavedInDB, !localMapMaker_->isGridFromDepth() && !occupancySavedInDB, false, true);
}
UDEBUG("Adding grid map %d to cache...", iter->first);
cv::Point3f viewPoint;
cv::Mat ground, obstacles, emptyCells;
if(iter->first > 0)
{
cv::Mat rgb, depth;
LaserScan scan;
bool generateGrid = data.gridCellSize() == 0.0f;
static bool warningShown = false;
if(occupancySavedInDB && generateGrid && !warningShown)
{
warningShown = true;
UWARN("Occupancy grid for location %d should be added to global map (e..g, a ROS node is subscribed to "
"any occupancy grid output) but it cannot be found "
"in memory. For convenience, the occupancy "
"grid is regenerated. Make sure parameter \"%s\" is true to "
"avoid this warning for the next locations added to map. For older "
"locations already in database without an occupancy grid map, you can use the "
"\"rtabmap-databaseViewer\" to regenerate the missing occupancy grid maps and "
"save them back in the database for next sessions. This warning is only shown once.",
data.id(), Parameters::kRGBDCreateOccupancyGrid().c_str());
}
if(memory && occupancySavedInDB && generateGrid)
{
// 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, localMapMaker_->isGridFromDepth(), !localMapMaker_->isGridFromDepth(), false, false);
}
data.uncompressData(
localMapMaker_->isGridFromDepth() && generateGrid?&rgb:0,
localMapMaker_->isGridFromDepth() && generateGrid?&depth:0,
!localMapMaker_->isGridFromDepth() && generateGrid?&scan:0,
0,
generateGrid?0:&ground,
generateGrid?0:&obstacles,
generateGrid?0:&emptyCells);
if(generateGrid)
{
Signature tmp(data);
tmp.setPose(iter->second);
localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint);
}
else
{
localMaps_.add(iter->first, ground, obstacles, emptyCells, data.gridCellSize(), data.gridViewPoint());
}
}
else
{
// generate tmp occupancy grid for latest id (assuming data is already uncompressed)
// For negative laser scans, fill empty space?
bool unknownSpaceFilled = Parameters::defaultGridScan2dUnknownSpaceFilled();
Parameters::parse(parameters_, Parameters::kGridScan2dUnknownSpaceFilled(), unknownSpaceFilled);
if(unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_)
{
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(scanEmptyRayTracing_)));
localMapMaker_->parseParameters(parameters);
}
cv::Mat rgb, depth;
LaserScan scan;
bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_);
data.uncompressData(
localMapMaker_->isGridFromDepth() && generateGrid?&rgb:0,
localMapMaker_->isGridFromDepth() && generateGrid?&depth:0,
!localMapMaker_->isGridFromDepth() && generateGrid?&scan:0,
0,
generateGrid?0:&ground,
generateGrid?0:&obstacles,
generateGrid?0:&emptyCells);
if(generateGrid)
{
Signature tmp(data);
tmp.setPose(iter->second);
localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint);
}
else
{
localMaps_.add(iter->first, ground, obstacles, emptyCells, data.gridCellSize(), data.gridViewPoint());
}
// put back
if(unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_)
{
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled)));
localMapMaker_->parseParameters(parameters);
}
}
}
}
else
{
UERROR("Pose null for node %d", iter->first);
}
}
if(updateGrid)
{
gridUpdated_ = occupancyGrid_->update(filteredPoses);
}
#ifdef RTABMAP_OCTOMAP
if(updateOctomap)
{
UTimer time;
octomapUpdated_ = octomap_->update(filteredPoses);
UINFO("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);
UINFO("GridMap (elevation map) update time = %fs", time.ticks());
}
#endif
localMaps_.clear(true);
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();
iter!=groundClouds_.end();)
{
if(!uContains(poses, iter->first))
{
groundClouds_.erase(iter++);
}
else
{
++iter;
}
}
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=obstacleClouds_.begin();
iter!=obstacleClouds_.end();)
{
if(!uContains(poses, iter->first))
{
obstacleClouds_.erase(iter++);
}
else
{
++iter;
}
}
if(longUpdate)
{
UWARN("Map(s) updated! (%f s)", longUpdateTimer.ticks());
}
}
return filteredPoses;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const rtabmap::FlannIndex & substractCloudIndex,
float radiusSearch,
int minNeighborsInRadius)
{
UASSERT(minNeighborsInRadius > 0);
UASSERT(substractCloudIndex.indexedFeatures());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
output->resize(cloud->size());
int oi = 0; // output iterator
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<std::vector<size_t> > kIndices;
std::vector<std::vector<float> > kDistances;
cv::Mat pt = (cv::Mat_<float>(1, 3) << cloud->at(i).x, cloud->at(i).y, cloud->at(i).z);
substractCloudIndex.radiusSearch(pt, kIndices, kDistances, radiusSearch, minNeighborsInRadius, 32, 0, false);
if(kIndices.size() == 1 && int(kIndices[0].size()) < minNeighborsInRadius)
{
output->at(oi++) = cloud->at(i);
}
}
output->resize(oi);
return output;
}
void MapsManager::publishMaps(
const std::map<int, rtabmap::Transform> & poses,
const rclcpp::Time & stamp,
const std::string & mapFrameId)
{
UDEBUG("Publishing maps... poses=%d", (int)poses.size());
// publish maps
if(cloudMapPub_->get_subscription_count() ||
cloudObstaclesPub_->get_subscription_count() ||
cloudGroundPub_->get_subscription_count())
{
// generate the assembled cloud!
UTimer time;
// detect if the graph has changed, if so, recreate the clouds
bool graphGroundOptimized = false;
bool graphObstacleOptimized = false;
bool updateGround = cloudMapPub_->get_subscription_count() ||
cloudGroundPub_->get_subscription_count();
bool updateObstacles = cloudMapPub_->get_subscription_count() ||
cloudObstaclesPub_->get_subscription_count();
bool graphGroundChanged = updateGround;
bool graphObstacleChanged = updateObstacles;
float updateErrorSqr = occupancyGrid_->getUpdateError()*occupancyGrid_->getUpdateError();
for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
{
std::map<int, Transform>::const_iterator jter;
if(updateGround)
{
jter = assembledGroundPoses_.find(iter->first);
if(jter != assembledGroundPoses_.end())
{
graphGroundChanged = false;
UASSERT(!iter->second.isNull() && !jter->second.isNull());
if(iter->second.getDistanceSquared(jter->second) > updateErrorSqr)
{
graphGroundOptimized = true;
}
}
}
if(updateObstacles)
{
jter = assembledObstaclePoses_.find(iter->first);
if(jter != assembledObstaclePoses_.end())
{
graphObstacleChanged = false;
UASSERT(!iter->second.isNull() && !jter->second.isNull());
if(iter->second.getDistanceSquared(jter->second) > updateErrorSqr)
{
graphObstacleOptimized = true;
}
}
}
}
int countObstacles = 0;
int countGrounds = 0;
int previousIndexedGroundSize = assembledGroundIndex_.indexedFeatures();
int previousIndexedObstacleSize = assembledObstacleIndex_.indexedFeatures();
if(graphGroundOptimized || graphGroundChanged)
{
int previousSize = assembledGround_->size();
assembledGround_->clear();
assembledGround_->reserve(previousSize);
assembledGroundPoses_.clear();
assembledGroundIndex_.release();
}
if(graphObstacleOptimized || graphObstacleChanged )
{
int previousSize = assembledObstacles_->size();
assembledObstacles_->clear();
assembledObstacles_->reserve(previousSize);
assembledObstaclePoses_.clear();
assembledObstacleIndex_.release();
}
if(graphGroundOptimized || graphObstacleOptimized)
{
UINFO("Graph has changed, updating clouds...");
UTimer t;
cv::Mat tmpGroundPts;
cv::Mat tmpObstaclePts;
for(std::map<int, Transform>::const_iterator iter = poses.lower_bound(1); iter!=poses.end(); ++iter)
{
if(updateGround &&
(graphGroundOptimized || assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end()))
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator kter=groundClouds_.find(iter->first);
if(kter != groundClouds_.end() && kter->second->size())
{
assembledGroundPoses_.insert(*iter);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(kter->second, iter->second);
*assembledGround_+=*transformed;
if(cloudSubtractFiltering_)
{
for(unsigned int i=0; i<transformed->size(); ++i)
{
if(tmpGroundPts.empty())
{
tmpGroundPts = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
tmpGroundPts.reserve(previousIndexedGroundSize>0?previousIndexedGroundSize:100);
}
else
{
cv::Mat pt = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
tmpGroundPts.push_back(pt);
}
}
}
++countGrounds;
}
}
if(updateObstacles &&
(graphObstacleOptimized || assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end()))
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator kter=obstacleClouds_.find(iter->first);
if(kter != obstacleClouds_.end() && kter->second->size())
{
assembledObstaclePoses_.insert(*iter);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(kter->second, iter->second);
*assembledObstacles_+=*transformed;
if(cloudSubtractFiltering_)
{
for(unsigned int i=0; i<transformed->size(); ++i)
{
if(tmpObstaclePts.empty())
{
tmpObstaclePts = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
tmpObstaclePts.reserve(previousIndexedObstacleSize>0?previousIndexedObstacleSize:100);
}
else
{
cv::Mat pt = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
tmpObstaclePts.push_back(pt);
}
}
}
++countObstacles;
}
}
}
double addingPointsTime = t.ticks();
if(graphGroundOptimized && !tmpGroundPts.empty())
{
assembledGroundIndex_.buildKDTreeSingleIndex(tmpGroundPts, 15);
}
if(graphObstacleOptimized && !tmpObstaclePts.empty())
{
assembledObstacleIndex_.buildKDTreeSingleIndex(tmpObstaclePts, 15);
}
double indexingTime = t.ticks();
UINFO("Graph optimized! Time recreating clouds (%d ground, %d obstacles) = %f s (indexing %fs)", countGrounds, countObstacles, addingPointsTime+indexingTime, indexingTime);
}
else if(graphGroundChanged || graphObstacleChanged)
{
UWARN("Graph has changed! The whole cloud is regenerated.");
}
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
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!=localMaps_.end() && jter->second.groundCells.cols)
{
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_)
{
if(assembledGroundIndex_.indexedFeatures())
{
subtractedCloud = subtractFiltering(transformed, assembledGroundIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
}
if(subtractedCloud->size())
{
UDEBUG("Adding ground %d pts=%d/%d (index=%d)", iter->first, subtractedCloud->size(), transformed->size(), assembledGroundIndex_.indexedFeatures());
cv::Mat pts(subtractedCloud->size(), 3, CV_32FC1);
for(unsigned int i=0; i<subtractedCloud->size(); ++i)
{
pts.at<float>(i, 0) = subtractedCloud->at(i).x;
pts.at<float>(i, 1) = subtractedCloud->at(i).y;
pts.at<float>(i, 2) = subtractedCloud->at(i).z;
}
if(!assembledGroundIndex_.isBuilt())
{
assembledGroundIndex_.buildKDTreeSingleIndex(pts, 15);
}
else
{
assembledGroundIndex_.addPoints(pts);
}
}
}
if(iter->first>0)
{
groundClouds_.insert(std::make_pair(iter->first, util3d::transformPointCloud(subtractedCloud, iter->second.inverse())));
}
if(subtractedCloud->size())
{
*assembledGround_+=*subtractedCloud;
}
++countGrounds;
}
}
if(updateObstacles && assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end())
{
if(iter->first > 0)
{
assembledObstaclePoses_.insert(*iter);
}
if(jter!=localMaps_.end() && jter->second.obstacleCells.cols)
{
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_)
{
if(assembledObstacleIndex_.indexedFeatures())
{
subtractedCloud = subtractFiltering(transformed, assembledObstacleIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
}
if(subtractedCloud->size())
{
UDEBUG("Adding obstacle %d pts=%d/%d (index=%d)", iter->first, subtractedCloud->size(), transformed->size(), assembledObstacleIndex_.indexedFeatures());
cv::Mat pts(subtractedCloud->size(), 3, CV_32FC1);
for(unsigned int i=0; i<subtractedCloud->size(); ++i)
{
pts.at<float>(i, 0) = subtractedCloud->at(i).x;
pts.at<float>(i, 1) = subtractedCloud->at(i).y;
pts.at<float>(i, 2) = subtractedCloud->at(i).z;
}
if(!assembledObstacleIndex_.isBuilt())
{
assembledObstacleIndex_.buildKDTreeSingleIndex(pts, 15);
}
else
{
assembledObstacleIndex_.addPoints(pts);
}
}
}
if(iter->first>0)
{
obstacleClouds_.insert(std::make_pair(iter->first, util3d::transformPointCloud(subtractedCloud, iter->second.inverse())));
}
if(subtractedCloud->size())
{
*assembledObstacles_+=*subtractedCloud;
}
++countObstacles;
}
}
}
if(cloudOutputVoxelized_)
{
UASSERT(occupancyGrid_->getCellSize() > 0.0);
if(countGrounds && assembledGround_->size())
{
assembledGround_ = util3d::voxelize(assembledGround_, occupancyGrid_->getCellSize());
}
if(countObstacles && assembledObstacles_->size())
{
assembledObstacles_ = util3d::voxelize(assembledObstacles_, occupancyGrid_->getCellSize());
}
}
UINFO("Assembled %d obstacle and %d ground clouds (%d points, %fs)",
countObstacles, countGrounds, (int)(assembledGround_->size() + assembledObstacles_->size()), time.ticks());
if( countGrounds > 0 ||
countObstacles > 0 ||
!latching_ ||
(assembledGround_->empty() && assembledObstacles_->empty()) ||
(cloudGroundPub_->get_subscription_count() && !latched_.at(&cloudGroundPub_)) ||
(cloudObstaclesPub_->get_subscription_count() && !latched_.at(&cloudObstaclesPub_)) ||
(cloudMapPub_->get_subscription_count() && !latched_.at(&cloudMapPub_)))
{
if(cloudGroundPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*assembledGround_, *cloudMsg);
cloudMsg->header.stamp = stamp;
cloudMsg->header.frame_id = mapFrameId;
cloudGroundPub_->publish(std::move(cloudMsg));
latched_.at(&cloudGroundPub_) = true;
}
if(cloudObstaclesPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(*assembledObstacles_, *cloudMsg);
cloudMsg->header.stamp = stamp;
cloudMsg->header.frame_id = mapFrameId;
cloudObstaclesPub_->publish(std::move(cloudMsg));
latched_.at(&cloudObstaclesPub_) = true;
}
if(cloudMapPub_->get_subscription_count())
{
pcl::PointCloud<pcl::PointXYZRGB> cloud = *assembledObstacles_ + *assembledGround_;
sensor_msgs::msg::PointCloud2::UniquePtr cloudMsg(new sensor_msgs::msg::PointCloud2);
pcl::toROSMsg(cloud, *cloudMsg);
cloudMsg->header.stamp = stamp;
cloudMsg->header.frame_id = mapFrameId;
if(cloudMapPub_->get_subscription_count())
{
cloudMapPub_->publish(std::move(cloudMsg));
latched_.at(&cloudMapPub_) = true;
}
}
}
}
else if(mapCacheCleanup_)
{
if(!groundClouds_.empty() || !obstacleClouds_.empty())
{
size_t totalBytes = 0;
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();iter!=groundClouds_.end();++iter)
{
totalBytes += sizeof(int) + iter->second->points.size()*sizeof(pcl::PointXYZRGB);
}
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=obstacleClouds_.begin();iter!=obstacleClouds_.end();++iter)
{
totalBytes += sizeof(int) + iter->second->points.size()*sizeof(pcl::PointXYZRGB);
}
totalBytes += (assembledGround_->size() + assembledObstacles_->size()) *sizeof(pcl::PointXYZRGB);
totalBytes += (assembledGroundPoses_.size() + assembledObstaclePoses_.size()) * 13*sizeof(float);
totalBytes += assembledGroundIndex_.indexedFeatures()*assembledGroundIndex_.featuresDim() * sizeof(float);
totalBytes += assembledObstacleIndex_.indexedFeatures()*assembledObstacleIndex_.featuresDim() * sizeof(float);
UINFO("MapsManager: cleanup point clouds (%ld points, %ld cached clouds, ~%ld MB)...",
assembledGround_->size()+assembledObstacles_->size(),
groundClouds_.size()+obstacleClouds_.size(),
totalBytes/1048576);
}
assembledGround_->clear();
assembledObstacles_->clear();
assembledGroundPoses_.clear();
assembledObstaclePoses_.clear();
assembledGroundIndex_.release();
assembledObstacleIndex_.release();
groundClouds_.clear();
obstacleClouds_.clear();
}
if(cloudMapPub_->get_subscription_count() == 0)
{
latched_.at(&cloudMapPub_) = false;
}
if(cloudGroundPub_->get_subscription_count() == 0)
{
latched_.at(&cloudGroundPub_) = false;
}
if(cloudObstaclesPub_->get_subscription_count() == 0)
{
latched_.at(&cloudObstaclesPub_) = false;
}
#ifdef RTABMAP_OCTOMAP
if( octomapUpdated_ ||
!latching_ ||
#ifdef WITH_OCTOMAP_MSGS
(octoMapPubBin_->get_subscription_count() && !latched_.at(&octoMapPubBin_)) ||
(octoMapPubFull_->get_subscription_count() && !latched_.at(&octoMapPubFull_)) ||
#endif
(octoMapCloud_->get_subscription_count() && !latched_.at(&octoMapCloud_)) ||
(octoMapFrontierCloud_->get_subscription_count() && !latched_.at(&octoMapFrontierCloud_)) ||
(octoMapObstacleCloud_->get_subscription_count() && !latched_.at(&octoMapObstacleCloud_)) ||
(octoMapGroundCloud_->get_subscription_count() && !latched_.at(&octoMapGroundCloud_)) ||
(octoMapEmptySpace_->get_subscription_count() && !latched_.at(&octoMapEmptySpace_)) ||
(octoMapProj_->get_subscription_count() && !latched_.at(&octoMapProj_)))
{
#ifdef WITH_OCTOMAP_MSGS
if(octoMapPubBin_->get_subscription_count())
{
octomap_msgs::msg::Octomap msg;
octomap_msgs::binaryMapToMsg(*octomap_->octree(), msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapPubBin_->publish(msg);
latched_.at(&octoMapPubBin_) = true;
}
if(octoMapPubFull_->get_subscription_count())
{
octomap_msgs::msg::Octomap msg;
octomap_msgs::fullMapToMsg(*octomap_->octree(), msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapPubFull_->publish(msg);
latched_.at(&octoMapPubFull_) = true;
}
#endif
if(octoMapCloud_->get_subscription_count() ||
octoMapFrontierCloud_->get_subscription_count() ||
octoMapObstacleCloud_->get_subscription_count() ||
octoMapGroundCloud_->get_subscription_count() ||
octoMapEmptySpace_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2 msg;
pcl::IndicesPtr obstacleIndices(new std::vector<int>);
pcl::IndicesPtr frontierIndices(new std::vector<int>);
pcl::IndicesPtr emptyIndices(new std::vector<int>);
pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get(),0);
if(octoMapCloud_->get_subscription_count())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudOccupiedSpace;
pcl::IndicesPtr indices = util3d::concatenate(obstacleIndices, groundIndices);
pcl::copyPointCloud(*cloud, *indices, cloudOccupiedSpace);
pcl::toROSMsg(cloudOccupiedSpace, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapCloud_->publish(msg);
latched_.at(&octoMapCloud_) = true;
}
if(octoMapFrontierCloud_->get_subscription_count())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudFrontier;
pcl::copyPointCloud(*cloud, *frontierIndices, cloudFrontier);
pcl::toROSMsg(cloudFrontier, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapFrontierCloud_->publish(msg);
latched_.at(&octoMapFrontierCloud_) = true;
}
if(octoMapObstacleCloud_->get_subscription_count())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles);
pcl::toROSMsg(cloudObstacles, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapObstacleCloud_->publish(msg);
latched_.at(&octoMapObstacleCloud_) = true;
}
if(octoMapGroundCloud_->get_subscription_count())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudGround;
pcl::copyPointCloud(*cloud, *groundIndices, cloudGround);
pcl::toROSMsg(cloudGround, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapGroundCloud_->publish(msg);
latched_.at(&octoMapGroundCloud_) = true;
}
if(octoMapEmptySpace_->get_subscription_count())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudEmptySpace;
pcl::copyPointCloud(*cloud, *emptyIndices, cloudEmptySpace);
pcl::toROSMsg(cloudEmptySpace, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapEmptySpace_->publish(msg);
latched_.at(&octoMapEmptySpace_) = true;
}
}
if(octoMapProj_->get_subscription_count())
{
// create the projection map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, occupancyGrid_->getMinMapSize(), octomapTreeDepth_);
if(!pixels.empty())
{
//init
nav_msgs::msg::OccupancyGrid map;
map.info.resolution = gridCellSize;
map.info.origin.position.x = 0.0;
map.info.origin.position.y = 0.0;
map.info.origin.position.z = 0.0;
map.info.origin.orientation.x = 0.0;
map.info.origin.orientation.y = 0.0;
map.info.origin.orientation.z = 0.0;
map.info.origin.orientation.w = 1.0;
map.info.width = pixels.cols;
map.info.height = pixels.rows;
map.info.origin.position.x = xMin;
map.info.origin.position.y = yMin;
map.data.resize(map.info.width * map.info.height);
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
map.header.frame_id = mapFrameId;
map.header.stamp = stamp;
octoMapProj_->publish(map);
latched_.at(&octoMapProj_) = true;
}
else if(poses.size())
{
UWARN("Octomap projection map is empty! (poses=%d octomap nodes=%d). "
"Make sure you enabled \"%s\" and set \"%s\"=1. "
"See \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" for more info.",
(int)poses.size(), (int)octomap_->octree()->size(),
Parameters::kGrid3D().c_str(), Parameters::kGridSensor().c_str());
}
}
}
if( mapCacheCleanup_ &&
#ifdef WITH_OCTOMAP_MSGS
octoMapPubBin_->get_subscription_count() == 0 &&
octoMapPubFull_->get_subscription_count() == 0 &&
#endif
octoMapCloud_->get_subscription_count() == 0 &&
octoMapFrontierCloud_->get_subscription_count() == 0 &&
octoMapObstacleCloud_->get_subscription_count() == 0 &&
octoMapGroundCloud_->get_subscription_count() == 0 &&
octoMapEmptySpace_->get_subscription_count() == 0 &&
octoMapProj_->get_subscription_count() == 0)
{
if(octomap_->octree()->getNumLeafNodes()>0)
{
UINFO("MapsManager: cleanup octomap (%ld leaf nodes, ~%ld MB)...",
octomap_->octree()->getNumLeafNodes(),
octomap_->octree()->memoryUsage()/1048576);
}
octomap_->clear();
}
#ifdef WITH_OCTOMAP_MSGS
if(octoMapPubBin_->get_subscription_count() == 0)
{
latched_.at(&octoMapPubBin_) = false;
}
if(octoMapPubFull_->get_subscription_count() == 0)
{
latched_.at(&octoMapPubFull_) = false;
}
#endif
if(octoMapCloud_->get_subscription_count() == 0)
{
latched_.at(&octoMapCloud_) = false;
}
if(octoMapFrontierCloud_->get_subscription_count() == 0)
{
latched_.at(&octoMapFrontierCloud_) = false;
}
if(octoMapObstacleCloud_->get_subscription_count() == 0)
{
latched_.at(&octoMapObstacleCloud_) = false;
}
if(octoMapGroundCloud_->get_subscription_count() == 0)
{
latched_.at(&octoMapGroundCloud_) = false;
}
if(octoMapEmptySpace_->get_subscription_count() == 0)
{
latched_.at(&octoMapEmptySpace_) = false;
}
if(octoMapProj_->get_subscription_count() == 0)
{
latched_.at(&octoMapProj_) = false;
}
#endif
if( gridUpdated_ ||
!latching_ ||
(gridMapPub_->get_subscription_count() && !latched_.at(&gridMapPub_)) ||
(gridProbMapPub_->get_subscription_count() && !latched_.at(&gridProbMapPub_)))
{
if(gridProbMapPub_->get_subscription_count())
{
// create the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = this->getGridProbMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
{
//init
nav_msgs::msg::OccupancyGrid map;
map.info.resolution = gridCellSize;
map.info.origin.position.x = 0.0;
map.info.origin.position.y = 0.0;
map.info.origin.position.z = 0.0;
map.info.origin.orientation.x = 0.0;
map.info.origin.orientation.y = 0.0;
map.info.origin.orientation.z = 0.0;
map.info.origin.orientation.w = 1.0;
map.info.width = pixels.cols;
map.info.height = pixels.rows;
map.info.origin.position.x = xMin;
map.info.origin.position.y = yMin;
map.data.resize(map.info.width * map.info.height);
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
map.header.frame_id = mapFrameId;
map.header.stamp = stamp;
if(gridProbMapPub_->get_subscription_count())
{
gridProbMapPub_->publish(map);
latched_.at(&gridProbMapPub_) = true;
}
}
else if(poses.size())
{
UWARN("Grid map is empty! (local maps=%ld)", localMaps_.size());
}
}
if(gridMapPub_->get_subscription_count())
{
// create the grid map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = this->getGridMap(xMin, yMin, gridCellSize);
if(!pixels.empty())
{
//init
nav_msgs::msg::OccupancyGrid map;
map.info.resolution = gridCellSize;
map.info.origin.position.x = 0.0;
map.info.origin.position.y = 0.0;
map.info.origin.position.z = 0.0;
map.info.origin.orientation.x = 0.0;
map.info.origin.orientation.y = 0.0;
map.info.origin.orientation.z = 0.0;
map.info.origin.orientation.w = 1.0;
map.info.width = pixels.cols;
map.info.height = pixels.rows;
map.info.origin.position.x = xMin;
map.info.origin.position.y = yMin;
map.data.resize(map.info.width * map.info.height);
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
map.header.frame_id = mapFrameId;
map.header.stamp = stamp;
gridMapPub_->publish(map);
latched_.at(&gridMapPub_) = true;
}
else if(poses.size())
{
UWARN("Grid map is empty! (local maps=%ld)", localMaps_.size());
}
}
}
if(gridMapPub_->get_subscription_count() == 0)
{
latched_.at(&gridMapPub_) = false;
}
if(gridProbMapPub_->get_subscription_count() == 0)
{
latched_.at(&gridProbMapPub_) = false;
}
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
if( elevationMapUpdated_ ||
!latching_ ||
(elevationMapPub_->get_subscription_count() && !latched_.at(&elevationMapPub_)))
{
grid_map_msgs::msg::GridMap::UniquePtr msg;
#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>21) || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR==21 && RTABMAP_VERSION_PATCH>=8)
msg = grid_map::GridMapRosConverter::toMessage(*elevationMap_->gridMap());
#else
msg = grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap());
#endif
msg->header.frame_id = mapFrameId;
msg->header.stamp = stamp;
elevationMapPub_->publish(std::move(msg));
}
if(elevationMapPub_->get_subscription_count() == 0)
{
latched_.at(&elevationMapPub_) = false;
}
if( mapCacheCleanup_ &&
elevationMapPub_->get_subscription_count() == 0)
{
elevationMap_->clear();
}
#endif
if(!this->hasSubscribers() && mapCacheCleanup_)
{
if(!localMaps_.empty())
{
size_t totalBytes = localMaps_.getMemoryUsed();
UINFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", localMaps_.size(), totalBytes/1048576);
}
localMaps_.clear();
}
}
cv::Mat MapsManager::getGridMap(
float & xMin,
float & yMin,
float & gridCellSize)
{
gridCellSize = occupancyGrid_->getCellSize();
return occupancyGrid_->getMap(xMin, yMin);
}
cv::Mat MapsManager::getGridProbMap(
float & xMin,
float & yMin,
float & gridCellSize)
{
gridCellSize = occupancyGrid_->getCellSize();
return occupancyGrid_->getProbMap(xMin, yMin);
}
} // namespace rtabmap_ros
Reference in New Issue View Git Blame Copy Permalink