MapsManager.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  documentation and/or other materials provided with the distribution.
11  * Neither the name of the Universite de Sherbrooke nor the
12  names of its contributors may be used to endorse or promote products
13  derived from this software without specific prior written permission.
14 
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
19 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 
27 #include "rtabmap_util/MapsManager.h"
28 
29 #include <rtabmap/utilite/ULogger.h>
30 #include <rtabmap/utilite/UTimer.h>
31 #include <rtabmap/utilite/UStl.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <rtabmap/core/util3d_mapping.h>
34 #include <rtabmap/core/util3d_filtering.h>
35 #include <rtabmap/core/util3d_transforms.h>
36 #include <rtabmap/core/util2d.h>
37 #include <rtabmap/core/Memory.h>
38 #include <rtabmap/core/Graph.h>
39 #include <rtabmap/core/Version.h>
40 #include <rtabmap/core/OccupancyGrid.h>
41 #include <pcl/search/kdtree.h>
42 
43 #include <nav_msgs/OccupancyGrid.h>
44 #include <ros/ros.h>
45 
46 #include <pcl_conversions/pcl_conversions.h>
47 #include <rtabmap/core/LocalGridMaker.h>
48 
49 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
50 #include <octomap_msgs/conversions.h>
51 #include <octomap/ColorOcTree.h>
52 #include <rtabmap/core/OctoMap.h>
53 #endif
54 
55 #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
56 #include <grid_map_ros/GridMapRosConverter.hpp>
57 #include <rtabmap/core/global_map/GridMap.h>
58 #endif
59 
60 using namespace rtabmap;
61 
62 namespace rtabmap_util {
63 
64 MapsManager::MapsManager() :
65  cloudOutputVoxelized_(true),
66  cloudSubtractFiltering_(false),
67  cloudSubtractFilteringMinNeighbors_(2),
68  mapFilterRadius_(0.0),
69  mapFilterAngle_(30.0), // degrees
70  mapCacheCleanup_(true),
71  alwaysUpdateMap_(false),
72  scanEmptyRayTracing_(true),
73  assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
74  assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
75  occupancyGrid_(new OccupancyGrid(&localMaps_)),
76  localMapMaker_(new LocalGridMaker),
77  gridUpdated_(true),
78 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
79  octomap_(new OctoMap(&localMaps_)),
80 #else
81  octomap_(0),
82 #endif
83  octomapTreeDepth_(16),
84  octomapUpdated_(true),
85 #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
86  elevationMap_(new GridMap(&localMaps_)),
87 #else
88  elevationMap_(0),
89 #endif
90  elevationMapUpdated_(true),
91  latching_(true)
92 {
93 }
94 
95 void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace)
96 {
97  // common map stuff
98  pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
99  pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
100  pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
101 
102  if(pnh.hasParam("map_negative_poses_ignored"))
103  {
104  ROS_WARN("Parameter \"map_negative_poses_ignored\" has been "
105  "removed. Use \"map_always_update\" instead.");
106  if(!pnh.hasParam("map_always_update"))
107  {
108  bool negPosesIgnored;
109  pnh.getParam("map_negative_poses_ignored", negPosesIgnored);
110  alwaysUpdateMap_ = !negPosesIgnored;
111  }
112  }
113  pnh.param("map_always_update", alwaysUpdateMap_, alwaysUpdateMap_);
114 
115  if(pnh.hasParam("map_negative_scan_empty_ray_tracing"))
116  {
117  ROS_WARN("Parameter \"map_negative_scan_empty_ray_tracing\" has been "
118  "removed. Use \"map_empty_ray_tracing\" instead.");
119  if(!pnh.hasParam("map_empty_ray_tracing"))
120  {
121  pnh.getParam("map_negative_scan_empty_ray_tracing", scanEmptyRayTracing_);
122  }
123  }
124  pnh.param("map_empty_ray_tracing", scanEmptyRayTracing_, scanEmptyRayTracing_);
125 
126  if(pnh.hasParam("scan_output_voxelized"))
127  {
128  ROS_WARN("Parameter \"scan_output_voxelized\" has been "
129  "removed. Use \"cloud_output_voxelized\" instead.");
130  if(!pnh.hasParam("cloud_output_voxelized"))
131  {
132  pnh.getParam("scan_output_voxelized", cloudOutputVoxelized_);
133  }
134  }
135  pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
136  pnh.param("cloud_subtract_filtering", cloudSubtractFiltering_, cloudSubtractFiltering_);
137  pnh.param("cloud_subtract_filtering_min_neighbors", cloudSubtractFilteringMinNeighbors_, cloudSubtractFilteringMinNeighbors_);
138 
139  ROS_INFO("%s(maps): map_filter_radius = %f", name.c_str(), mapFilterRadius_);
140  ROS_INFO("%s(maps): map_filter_angle = %f", name.c_str(), mapFilterAngle_);
141  ROS_INFO("%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false");
142  ROS_INFO("%s(maps): map_always_update = %s", name.c_str(), alwaysUpdateMap_?"true":"false");
143  ROS_INFO("%s(maps): map_empty_ray_tracing = %s", name.c_str(), scanEmptyRayTracing_?"true":"false");
144  ROS_INFO("%s(maps): cloud_output_voxelized = %s", name.c_str(), cloudOutputVoxelized_?"true":"false");
145  ROS_INFO("%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false");
146  ROS_INFO("%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_);
147 
148 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
149  pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
150  if(octomapTreeDepth_ > 16)
151  {
152  ROS_WARN("octomap_tree_depth maximum is 16");
153  octomapTreeDepth_ = 16;
154  }
155  else if(octomapTreeDepth_ < 0)
156  {
157  ROS_WARN("octomap_tree_depth cannot be negative, set to 16 instead");
158  octomapTreeDepth_ = 16;
159  }
160  ROS_INFO("%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_);
161 #endif
162 
163  // If true, the last message published on
164  // the map topics will be saved and sent to new subscribers when they
165  // connect
166  pnh.param("latch", latching_, latching_);
167 
168  // mapping topics
169  ros::NodeHandle * nht;
170  if(usePublicNamespace)
171  {
172  nht = &nh;
173  }
174  else
175  {
176  nht = &pnh;
177  }
178  latched_.clear();
179  gridMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latching_);
180  latched_.insert(std::make_pair((void*)&gridMapPub_, false));
181  gridProbMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("grid_prob_map", 1, latching_);
182  latched_.insert(std::make_pair((void*)&gridProbMapPub_, false));
183  cloudMapPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latching_);
184  latched_.insert(std::make_pair((void*)&cloudMapPub_, false));
185  cloudObstaclesPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_obstacles", 1, latching_);
186  latched_.insert(std::make_pair((void*)&cloudObstaclesPub_, false));
187  cloudGroundPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_ground", 1, latching_);
188  latched_.insert(std::make_pair((void*)&cloudGroundPub_, false));
189 
190  // deprecated
191  projMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latching_);
192  latched_.insert(std::make_pair((void*)&projMapPub_, false));
193  scanMapPub_ = nht->advertise<sensor_msgs::PointCloud2>("scan_map", 1, latching_);
194  latched_.insert(std::make_pair((void*)&scanMapPub_, false));
195 
196 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
197  octoMapPubBin_ = nht->advertise<octomap_msgs::Octomap>("octomap_binary", 1, latching_);
198  latched_.insert(std::make_pair((void*)&octoMapPubBin_, false));
199  octoMapPubFull_ = nht->advertise<octomap_msgs::Octomap>("octomap_full", 1, latching_);
200  latched_.insert(std::make_pair((void*)&octoMapPubFull_, false));
201  octoMapCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_occupied_space", 1, latching_);
202  latched_.insert(std::make_pair((void*)&octoMapCloud_, false));
203  octoMapFrontierCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_global_frontier_space", 1, latching_);
204  latched_.insert(std::make_pair((void*)&octoMapFrontierCloud_, false));
205  octoMapObstacleCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_obstacles", 1, latching_);
206  latched_.insert(std::make_pair((void*)&octoMapObstacleCloud_, false));
207  octoMapGroundCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_ground", 1, latching_);
208  latched_.insert(std::make_pair((void*)&octoMapGroundCloud_, false));
209  octoMapEmptySpace_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_empty_space", 1, latching_);
210  latched_.insert(std::make_pair((void*)&octoMapEmptySpace_, false));
211  octoMapProj_ = nht->advertise<nav_msgs::OccupancyGrid>("octomap_grid", 1, latching_);
212  latched_.insert(std::make_pair((void*)&octoMapProj_, false));
213 #endif
214 
215 #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
216  elevationMapPub_ = nht->advertise<grid_map_msgs::GridMap>("elevation_map", 1, latching_);
217  latched_.insert(std::make_pair((void*)&elevationMapPub_, false));
218 #endif
219 }
220 
221 MapsManager::~MapsManager() {
222  clear();
223 
224  delete occupancyGrid_;
225  delete localMapMaker_;
226 
227 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
228  delete octomap_;
229 #endif
230 
231 #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
232  delete elevationMap_;
233 #endif
234 }
235 
236 void parameterMoved(
237  ros::NodeHandle & nh,
238  const std::string & rosName,
239  const std::string & parameterName,
240  ParametersMap & parameters)
241 {
242  if(nh.hasParam(rosName))
243  {
244  ParametersMap::const_iterator iter = Parameters::getDefaultParameters().find(parameterName);
245  if(iter != Parameters::getDefaultParameters().end())
246  {
247  ROS_WARN("Parameter \"%s\" has moved from "
248  "rtabmap_ros to rtabmap library. Use "
249  "parameter \"%s\" instead. The value is still "
250  "copied to new parameter name.",
251  rosName.c_str(),
252  parameterName.c_str());
253  std::string type = Parameters::getType(parameterName);
254  if(type.compare("float") || type.compare("double"))
255  {
256  double v = uStr2Double(iter->second);
257  nh.getParam(rosName, v);
258  parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
259  }
260  else if(type.compare("int") || type.compare("unsigned int"))
261  {
262  int v = uStr2Int(iter->second);
263  nh.getParam(rosName, v);
264  parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
265  }
266  else if(type.compare("bool"))
267  {
268  bool v = uStr2Bool(iter->second);
269  nh.getParam(rosName, v);
270  if(rosName.compare("grid_incremental") == 0)
271  {
272  v = !v; // new parameter is called kGridGlobalFullUpdate(), which is the inverse
273  }
274  parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
275 
276  }
277  else
278  {
279  ROS_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str());
280  }
281  }
282  else
283  {
284  ROS_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str());
285  }
286  }
287 }
288 
289 void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, ParametersMap & parameters) const
290 {
291  // removed
292  if(pnh.hasParam("cloud_frustum_culling"))
293  {
294  ROS_WARN("Parameter \"cloud_frustum_culling\" has been removed. OctoMap topics "
295  "already do it. You can remove it from your launch file.");
296  }
297 
298  // moved
299  parameterMoved(pnh, "cloud_decimation", Parameters::kGridDepthDecimation(), parameters);
300  parameterMoved(pnh, "cloud_max_depth", Parameters::kGridRangeMax(), parameters);
301  parameterMoved(pnh, "cloud_min_depth", Parameters::kGridRangeMin(), parameters);
302  parameterMoved(pnh, "cloud_voxel_size", Parameters::kGridCellSize(), parameters);
303  parameterMoved(pnh, "cloud_floor_culling_height", Parameters::kGridMaxGroundHeight(), parameters);
304  parameterMoved(pnh, "cloud_ceiling_culling_height", Parameters::kGridMaxObstacleHeight(), parameters);
305  parameterMoved(pnh, "cloud_noise_filtering_radius", Parameters::kGridNoiseFilteringRadius(), parameters);
306  parameterMoved(pnh, "cloud_noise_filtering_min_neighbors", Parameters::kGridNoiseFilteringMinNeighbors(), parameters);
307  parameterMoved(pnh, "scan_decimation", Parameters::kGridScanDecimation(), parameters);
308  parameterMoved(pnh, "scan_voxel_size", Parameters::kGridCellSize(), parameters);
309  parameterMoved(pnh, "proj_max_ground_angle", Parameters::kGridMaxGroundAngle(), parameters);
310  parameterMoved(pnh, "proj_min_cluster_size", Parameters::kGridMinClusterSize(), parameters);
311  parameterMoved(pnh, "proj_max_height", Parameters::kGridMaxObstacleHeight(), parameters);
312  parameterMoved(pnh, "proj_max_obstacles_height", Parameters::kGridMaxObstacleHeight(), parameters);
313  parameterMoved(pnh, "proj_max_ground_height", Parameters::kGridMaxGroundHeight(), parameters);
314 
315  parameterMoved(pnh, "proj_detect_flat_obstacles", Parameters::kGridFlatObstacleDetected(), parameters);
316  parameterMoved(pnh, "proj_map_frame", Parameters::kGridMapFrameProjection(), parameters);
317  parameterMoved(pnh, "grid_unknown_space_filled", Parameters::kGridScan2dUnknownSpaceFilled(), parameters);
318  parameterMoved(pnh, "grid_cell_size", Parameters::kGridCellSize(), parameters);
319  parameterMoved(pnh, "grid_size", Parameters::kGridGlobalMinSize(), parameters);
320  parameterMoved(pnh, "grid_eroded", Parameters::kGridGlobalEroded(), parameters);
321  parameterMoved(pnh, "grid_footprint_radius", Parameters::kGridGlobalFootprintRadius(), parameters);
322 
323 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
324  parameterMoved(pnh, "octomap_ground_is_obstacle", Parameters::kGridGroundIsObstacle(), parameters);
325  parameterMoved(pnh, "octomap_occupancy_thr", Parameters::kGridGlobalOccupancyThr(), parameters);
326 #endif
327 }
328 
329 void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
330 {
331  parameters_ = parameters;
332  delete occupancyGrid_;
333  occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_);
334 
335  localMapMaker_->parseParameters(parameters_);
336 
337 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
338  delete octomap_;
339  octomap_ = new OctoMap(&localMaps_, parameters_);
340 #endif
341 
342 #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
343  delete elevationMap_;
344  elevationMap_ = new GridMap(&localMaps_, parameters_);
345 #endif
346 }
347 
348 void MapsManager::set2DMap(
349  const cv::Mat & map,
350  float xMin,
351  float yMin,
352  float cellSize,
353  const std::map<int, rtabmap::Transform> & poses,
354  const rtabmap::Memory * memory)
355 {
356  occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses);
357  //update cache in case the map should be updated
358  if(memory)
359  {
360  for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
361  {
362  std::map<int, LocalGrid>::const_iterator jter = localMaps_.find(iter->first);
363  if(!uContains(localMaps_.localGrids(), iter->first))
364  {
365  rtabmap::SensorData data;
366  data = memory->getNodeData(iter->first, false, false, false, true);
367  if(data.gridCellSize() == 0.0f)
368  {
369  ROS_WARN("Local occupancy grid doesn't exist for node %d", iter->first);
370  }
371  else
372  {
373  cv::Mat ground, obstacles, emptyCells;
374  data.uncompressData(
375  0,
376  0,
377  0,
378  0,
379  &ground,
380  &obstacles,
381  &emptyCells);
382 
383  localMaps_.add(iter->first, ground, obstacles, emptyCells, data.gridCellSize(), data.gridViewPoint());
384  }
385  }
386  }
387  }
388 }
389 
390 void MapsManager::clear()
391 {
392  localMaps_.clear();
393  assembledGround_->clear();
394  assembledObstacles_->clear();
395  assembledGroundPoses_.clear();
396  assembledObstaclePoses_.clear();
397  assembledGroundIndex_.release();
398  assembledObstacleIndex_.release();
399  groundClouds_.clear();
400  obstacleClouds_.clear();
401  occupancyGrid_->clear();
402 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
403  octomap_->clear();
404 #endif
405 #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
406  elevationMap_->clear();
407 #endif
408  for(std::map<void*, bool>::iterator iter=latched_.begin(); iter!=latched_.end(); ++iter)
409  {
410  iter->second = false;
411  }
412 }
413 
414 bool MapsManager::hasSubscribers() const
415 {
416  return cloudMapPub_.getNumSubscribers() != 0 ||
417  cloudObstaclesPub_.getNumSubscribers() != 0 ||
418  cloudGroundPub_.getNumSubscribers() != 0 ||
419  projMapPub_.getNumSubscribers() != 0 ||
420  gridMapPub_.getNumSubscribers() != 0 ||
421  gridProbMapPub_.getNumSubscribers() != 0 ||
422  scanMapPub_.getNumSubscribers() != 0 ||
423  octoMapPubBin_.getNumSubscribers() != 0 ||
424  octoMapPubFull_.getNumSubscribers() != 0 ||
425  octoMapCloud_.getNumSubscribers() != 0 ||
426  octoMapFrontierCloud_.getNumSubscribers() != 0 ||
427  octoMapObstacleCloud_.getNumSubscribers() != 0 ||
428  octoMapGroundCloud_.getNumSubscribers() != 0 ||
429  octoMapEmptySpace_.getNumSubscribers() != 0 ||
430  octoMapProj_.getNumSubscribers() != 0 ||
431  elevationMapPub_.getNumSubscribers() != 0;
432 }
433 
434 bool MapsManager::isMapUpdated() const
435 {
436  // We are currently using the check made in OccupancyGrid::update()
437  // to know if the map/graph changed. If there are no subscribers to
438  // any grid topics, we won't know it, so we will assume the graph
439  // changed.
440  return gridUpdated_ ||
441  (projMapPub_.getNumSubscribers() == 0 &&
442  gridMapPub_.getNumSubscribers() == 0 &&
443  gridProbMapPub_.getNumSubscribers() == 0);
444 }
445 
446 std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
447 {
448  if(mapFilterRadius_ > 0.0)
449  {
450  // filter nodes
451  double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
452  return rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
453  }
454  return std::map<int, Transform>();
455 }
456 
457 std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
458  const std::map<int, rtabmap::Transform> & posesIn,
459  const rtabmap::Memory * memory,
460  bool updateGrid,
461  bool updateOctomap,
462  const std::map<int, rtabmap::Signature> & signatures)
463 {
464  bool updateGridCache = updateGrid || updateOctomap;
465  bool updateElevation = false;
466  if(!updateGrid && !updateOctomap && !updateOctomap)
467  {
468  // all false, update only those where we have subscribers
469  updateOctomap =
470  octoMapPubBin_.getNumSubscribers() != 0 ||
471  octoMapPubFull_.getNumSubscribers() != 0 ||
472  octoMapCloud_.getNumSubscribers() != 0 ||
473  octoMapFrontierCloud_.getNumSubscribers() != 0 ||
474  octoMapObstacleCloud_.getNumSubscribers() != 0 ||
475  octoMapGroundCloud_.getNumSubscribers() != 0 ||
476  octoMapEmptySpace_.getNumSubscribers() != 0 ||
477  octoMapProj_.getNumSubscribers() != 0;
478 
479  updateGrid = projMapPub_.getNumSubscribers() != 0 ||
480  gridMapPub_.getNumSubscribers() != 0 ||
481  gridProbMapPub_.getNumSubscribers() != 0;
482 
483  updateElevation = elevationMapPub_.getNumSubscribers() != 0;
484 
485  updateGridCache = updateOctomap || updateGrid || updateElevation ||
486  cloudMapPub_.getNumSubscribers() != 0 ||
487  cloudObstaclesPub_.getNumSubscribers() != 0 ||
488  cloudGroundPub_.getNumSubscribers() != 0 ||
489  scanMapPub_.getNumSubscribers() != 0;
490  }
491 
492 #if not (defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP))
493  updateOctomap = false;
494 #endif
495 #if not (defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP))
496  updateElevation = false;
497 #endif
498 
499  gridUpdated_ = updateGrid;
500  octomapUpdated_ = updateOctomap;
501  elevationMapUpdated_ = updateElevation;
502 
503 
504  UDEBUG("Updating map caches...");
505 
506  if(!memory && signatures.size() == 0)
507  {
508  ROS_ERROR("Memory and signatures should not be both null!?");
509  return std::map<int, rtabmap::Transform>();
510  }
511 
512  // process only nodes (exclude landmarks)
513  std::map<int, rtabmap::Transform> poses;
514  if(posesIn.begin()->first < 0)
515  {
516  poses.insert(posesIn.lower_bound(0), posesIn.end());
517  }
518  else
519  {
520  poses = posesIn;
521  }
522  std::map<int, rtabmap::Transform> filteredPoses;
523 
524  // update cache
525  if(updateGridCache)
526  {
527  // filter nodes
528  if(mapFilterRadius_ > 0.0)
529  {
530  UDEBUG("Filter nodes...");
531  double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
532  filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
533  if(poses.find(0) != poses.end())
534  {
535  // make sure to keep latest data
536  filteredPoses.insert(*poses.find(0));
537  }
538  }
539  else
540  {
541  filteredPoses = poses;
542  }
543 
544  if(!alwaysUpdateMap_)
545  {
546  filteredPoses.erase(0);
547  }
548 
549  bool longUpdate = false;
550  UTimer longUpdateTimer;
551  if(filteredPoses.size() > 20)
552  {
553  if(updateGridCache && localMaps_.size() < 5)
554  {
555  ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size()));
556  longUpdate = true;
557  }
558 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
559  if(updateOctomap && octomap_->addedNodes().size() < 5)
560  {
561  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()));
562  longUpdate = true;
563  }
564 #endif
565  }
566 
567  bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false;
568 
569  for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
570  {
571  if(!iter->second.isNull())
572  {
573  rtabmap::SensorData data;
574  if(updateGridCache && (iter->first == 0 || !uContains(localMaps_.localGrids(), iter->first)))
575  {
576  ROS_DEBUG("Data required for %d", iter->first);
577  std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
578  if(findIter != signatures.end())
579  {
580  data = findIter->second.sensorData();
581  }
582  else if(memory)
583  {
584  data = memory->getNodeData(iter->first, localMapMaker_->isGridFromDepth() && !occupancySavedInDB, !localMapMaker_->isGridFromDepth() && !occupancySavedInDB, false, true);
585  }
586 
587  ROS_DEBUG("Adding grid map %d to cache...", iter->first);
588  cv::Point3f viewPoint;
589  cv::Mat ground, obstacles, emptyCells;
590  if(iter->first > 0)
591  {
592  cv::Mat rgb, depth;
593  LaserScan scan;
594  bool generateGrid = data.gridCellSize() == 0.0f;
595  static bool warningShown = false;
596  if(occupancySavedInDB && generateGrid && !warningShown)
597  {
598  warningShown = true;
599  UWARN("Occupancy grid for location %d should be added to global map (e..g, a ROS node is subscribed to "
600  "any occupancy grid output) but it cannot be found "
601  "in memory. For convenience, the occupancy "
602  "grid is regenerated. Make sure parameter \"%s\" is true to "
603  "avoid this warning for the next locations added to map. For older "
604  "locations already in database without an occupancy grid map, you can use the "
605  "\"rtabmap-databaseViewer\" to regenerate the missing occupancy grid maps and "
606  "save them back in the database for next sessions. This warning is only shown once.",
607  data.id(), Parameters::kRGBDCreateOccupancyGrid().c_str());
608  }
609  if(memory && occupancySavedInDB && generateGrid)
610  {
611  // if we are here, it is because we loaded a database with old nodes not having occupancy grid set
612  // try reload again
613  data = memory->getNodeData(iter->first, localMapMaker_->isGridFromDepth(), !localMapMaker_->isGridFromDepth(), false, false);
614  }
615  data.uncompressData(
616  localMapMaker_->isGridFromDepth() && generateGrid?&rgb:0,
617  localMapMaker_->isGridFromDepth() && generateGrid?&depth:0,
618  !localMapMaker_->isGridFromDepth() && generateGrid?&scan:0,
619  0,
620  generateGrid?0:&ground,
621  generateGrid?0:&obstacles,
622  generateGrid?0:&emptyCells);
623 
624  if(generateGrid)
625  {
626  Signature tmp(data);
627  tmp.setPose(iter->second);
628  localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
629  localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint);
630  }
631  else
632  {
633  localMaps_.add(iter->first, ground, obstacles, emptyCells, data.gridCellSize(), data.gridViewPoint());
634  }
635  }
636  else
637  {
638  // generate tmp occupancy grid for latest id (assuming data is already uncompressed)
639  // For negative laser scans, fill empty space?
640  bool unknownSpaceFilled = Parameters::defaultGridScan2dUnknownSpaceFilled();
641  Parameters::parse(parameters_, Parameters::kGridScan2dUnknownSpaceFilled(), unknownSpaceFilled);
642 
643  if(unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_)
644  {
645  ParametersMap parameters;
646  parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(scanEmptyRayTracing_)));
647  localMapMaker_->parseParameters(parameters);
648  }
649 
650  cv::Mat rgb, depth;
651  LaserScan scan;
652  bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_);
653  data.uncompressData(
654  localMapMaker_->isGridFromDepth() && generateGrid?&rgb:0,
655  localMapMaker_->isGridFromDepth() && generateGrid?&depth:0,
656  !localMapMaker_->isGridFromDepth() && generateGrid?&scan:0,
657  0,
658  generateGrid?0:&ground,
659  generateGrid?0:&obstacles,
660  generateGrid?0:&emptyCells);
661 
662  if(generateGrid)
663  {
664  Signature tmp(data);
665  tmp.setPose(iter->second);
666  localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
667  localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint);
668  }
669  else
670  {
671  localMaps_.add(iter->first, ground, obstacles, emptyCells, data.gridCellSize(), data.gridViewPoint());
672  }
673 
674  // put back
675  if(unknownSpaceFilled != scanEmptyRayTracing_ && scanEmptyRayTracing_)
676  {
677  ParametersMap parameters;
678  parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled)));
679  localMapMaker_->parseParameters(parameters);
680  }
681  }
682  }
683  }
684  else
685  {
686  ROS_ERROR("Pose null for node %d", iter->first);
687  }
688  }
689 
690  if(updateGrid)
691  {
692  gridUpdated_ = occupancyGrid_->update(filteredPoses);
693  }
694 
695 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
696  if(updateOctomap)
697  {
698  UTimer time;
699  octomapUpdated_ = octomap_->update(filteredPoses);
700  ROS_INFO("Octomap update time = %fs", time.ticks());
701  }
702 #endif
703 
704 #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
705  if(updateElevation)
706  {
707  UTimer time;
708  elevationMapUpdated_ = elevationMap_->update(filteredPoses);
709  ROS_INFO("GridMap (elevation map) update time = %fs", time.ticks());
710  }
711 #endif
712 
713  localMaps_.clear(true);
714 
715  for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();
716  iter!=groundClouds_.end();)
717  {
718  if(!uContains(poses, iter->first))
719  {
720  groundClouds_.erase(iter++);
721  }
722  else
723  {
724  ++iter;
725  }
726  }
727 
728  for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=obstacleClouds_.begin();
729  iter!=obstacleClouds_.end();)
730  {
731  if(!uContains(poses, iter->first))
732  {
733  obstacleClouds_.erase(iter++);
734  }
735  else
736  {
737  ++iter;
738  }
739  }
740 
741  if(longUpdate)
742  {
743  ROS_WARN("Map(s) updated! (%f s)", longUpdateTimer.ticks());
744  }
745  }
746 
747  return filteredPoses;
748 }
749 
750 pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
751  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
752  const rtabmap::FlannIndex & substractCloudIndex,
753  float radiusSearch,
754  int minNeighborsInRadius)
755 {
756  UASSERT(minNeighborsInRadius > 0);
757  UASSERT(substractCloudIndex.indexedFeatures());
758 
759  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
760  output->resize(cloud->size());
761  int oi = 0; // output iterator
762  for(unsigned int i=0; i<cloud->size(); ++i)
763  {
764  std::vector<std::vector<size_t> > kIndices;
765  std::vector<std::vector<float> > kDistances;
766  cv::Mat pt = (cv::Mat_<float>(1, 3) << cloud->at(i).x, cloud->at(i).y, cloud->at(i).z);
767  substractCloudIndex.radiusSearch(pt, kIndices, kDistances, radiusSearch, minNeighborsInRadius, 32, 0, false);
768  if(kIndices.size() == 1 && kIndices[0].size() < minNeighborsInRadius)
769  {
770  output->at(oi++) = cloud->at(i);
771  }
772  }
773  output->resize(oi);
774  return output;
775 }
776 
777 void MapsManager::publishMaps(
778  const std::map<int, rtabmap::Transform> & poses,
779  const ros::Time & stamp,
780  const std::string & mapFrameId)
781 {
782  ROS_DEBUG("Publishing maps... poses=%d", (int)poses.size());
783 
784  // publish maps
785  if(cloudMapPub_.getNumSubscribers() ||
786  scanMapPub_.getNumSubscribers() ||
787  cloudObstaclesPub_.getNumSubscribers() ||
788  cloudGroundPub_.getNumSubscribers())
789  {
790  // generate the assembled cloud!
791  UTimer time;
792 
793  if(scanMapPub_.getNumSubscribers())
794  {
795  if(parameters_.find(Parameters::kGridSensor()) != parameters_.end() &&
796  uStr2Int(parameters_.at(Parameters::kGridSensor()))==1)
797  {
798  ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic "
799  "instead with <param name=\"%s\" type=\"string\" value=\"0\"/>. "
800  "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
801  "all occupancy grid parameters.",
802  Parameters::kGridSensor().c_str());
803  }
804  else
805  {
806  ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic instead.");
807  }
808  }
809 
810  // detect if the graph has changed, if so, recreate the clouds
811  bool graphGroundOptimized = false;
812  bool graphObstacleOptimized = false;
813  bool updateGround = cloudMapPub_.getNumSubscribers() ||
814  scanMapPub_.getNumSubscribers() ||
815  cloudGroundPub_.getNumSubscribers();
816  bool updateObstacles = cloudMapPub_.getNumSubscribers() ||
817  scanMapPub_.getNumSubscribers() ||
818  cloudObstaclesPub_.getNumSubscribers();
819  bool graphGroundChanged = updateGround;
820  bool graphObstacleChanged = updateObstacles;
821  float updateErrorSqr = occupancyGrid_->getUpdateError()*occupancyGrid_->getUpdateError();
822  for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
823  {
824  std::map<int, Transform>::const_iterator jter;
825  if(updateGround)
826  {
827  jter = assembledGroundPoses_.find(iter->first);
828  if(jter != assembledGroundPoses_.end())
829  {
830  graphGroundChanged = false;
831  UASSERT(!iter->second.isNull() && !jter->second.isNull());
832  if(iter->second.getDistanceSquared(jter->second) > updateErrorSqr)
833  {
834  graphGroundOptimized = true;
835  }
836  }
837  }
838  if(updateObstacles)
839  {
840  jter = assembledObstaclePoses_.find(iter->first);
841  if(jter != assembledObstaclePoses_.end())
842  {
843  graphObstacleChanged = false;
844  UASSERT(!iter->second.isNull() && !jter->second.isNull());
845  if(iter->second.getDistanceSquared(jter->second) > updateErrorSqr)
846  {
847  graphObstacleOptimized = true;
848  }
849  }
850  }
851  }
852  int countObstacles = 0;
853  int countGrounds = 0;
854  int previousIndexedGroundSize = assembledGroundIndex_.indexedFeatures();
855  int previousIndexedObstacleSize = assembledObstacleIndex_.indexedFeatures();
856  if(graphGroundOptimized || graphGroundChanged)
857  {
858  int previousSize = assembledGround_->size();
859  assembledGround_->clear();
860  assembledGround_->reserve(previousSize);
861  assembledGroundPoses_.clear();
862  assembledGroundIndex_.release();
863  }
864  if(graphObstacleOptimized || graphObstacleChanged )
865  {
866  int previousSize = assembledObstacles_->size();
867  assembledObstacles_->clear();
868  assembledObstacles_->reserve(previousSize);
869  assembledObstaclePoses_.clear();
870  assembledObstacleIndex_.release();
871  }
872 
873  if(graphGroundOptimized || graphObstacleOptimized)
874  {
875  ROS_INFO("Graph has changed, updating clouds...");
876  UTimer t;
877  cv::Mat tmpGroundPts;
878  cv::Mat tmpObstaclePts;
879  for(std::map<int, Transform>::const_iterator iter = poses.lower_bound(1); iter!=poses.end(); ++iter)
880  {
881  if(updateGround &&
882  (graphGroundOptimized || assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end()))
883  {
884  std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator kter=groundClouds_.find(iter->first);
885  if(kter != groundClouds_.end() && kter->second->size())
886  {
887  assembledGroundPoses_.insert(*iter);
888  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(kter->second, iter->second);
889  *assembledGround_+=*transformed;
890  if(cloudSubtractFiltering_)
891  {
892  for(unsigned int i=0; i<transformed->size(); ++i)
893  {
894  if(tmpGroundPts.empty())
895  {
896  tmpGroundPts = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
897  tmpGroundPts.reserve(previousIndexedGroundSize>0?previousIndexedGroundSize:100);
898  }
899  else
900  {
901  cv::Mat pt = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
902  tmpGroundPts.push_back(pt);
903  }
904  }
905  }
906  ++countGrounds;
907  }
908  }
909  if(updateObstacles &&
910  (graphObstacleOptimized || assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end()))
911  {
912  std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator kter=obstacleClouds_.find(iter->first);
913  if(kter != obstacleClouds_.end() && kter->second->size())
914  {
915  assembledObstaclePoses_.insert(*iter);
916  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(kter->second, iter->second);
917  *assembledObstacles_+=*transformed;
918  if(cloudSubtractFiltering_)
919  {
920  for(unsigned int i=0; i<transformed->size(); ++i)
921  {
922  if(tmpObstaclePts.empty())
923  {
924  tmpObstaclePts = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
925  tmpObstaclePts.reserve(previousIndexedObstacleSize>0?previousIndexedObstacleSize:100);
926  }
927  else
928  {
929  cv::Mat pt = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
930  tmpObstaclePts.push_back(pt);
931  }
932  }
933  }
934  ++countObstacles;
935  }
936  }
937  }
938  double addingPointsTime = t.ticks();
939 
940  if(graphGroundOptimized && !tmpGroundPts.empty())
941  {
942  assembledGroundIndex_.buildKDTreeSingleIndex(tmpGroundPts, 15);
943  }
944  if(graphObstacleOptimized && !tmpObstaclePts.empty())
945  {
946  assembledObstacleIndex_.buildKDTreeSingleIndex(tmpObstaclePts, 15);
947  }
948  double indexingTime = t.ticks();
949  ROS_INFO("Graph optimized! Time recreating clouds (%d ground, %d obstacles) = %f s (indexing %fs)", countGrounds, countObstacles, addingPointsTime+indexingTime, indexingTime);
950  }
951  else if(graphGroundChanged || graphObstacleChanged)
952  {
953  ROS_WARN("Graph has changed! The whole cloud is regenerated.");
954  }
955 
956  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
957  {
958  std::map<int, LocalGrid>::const_iterator jter = localMaps_.localGrids().find(iter->first);
959  if(updateGround && assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end())
960  {
961  if(iter->first > 0)
962  {
963  assembledGroundPoses_.insert(*iter);
964  }
965  if(jter!=localMaps_.end() && jter->second.groundCells.cols)
966  {
967  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.groundCells), iter->second, 0, 255, 0);
968  pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
969  if(cloudSubtractFiltering_)
970  {
971  if(assembledGroundIndex_.indexedFeatures())
972  {
973  subtractedCloud = subtractFiltering(transformed, assembledGroundIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
974  }
975  if(subtractedCloud->size())
976  {
977  UDEBUG("Adding ground %d pts=%d/%d (index=%d)", iter->first, subtractedCloud->size(), transformed->size(), assembledGroundIndex_.indexedFeatures());
978  cv::Mat pts(subtractedCloud->size(), 3, CV_32FC1);
979  for(unsigned int i=0; i<subtractedCloud->size(); ++i)
980  {
981  pts.at<float>(i, 0) = subtractedCloud->at(i).x;
982  pts.at<float>(i, 1) = subtractedCloud->at(i).y;
983  pts.at<float>(i, 2) = subtractedCloud->at(i).z;
984  }
985  if(!assembledGroundIndex_.isBuilt())
986  {
987  assembledGroundIndex_.buildKDTreeSingleIndex(pts, 15);
988  }
989  else
990  {
991  assembledGroundIndex_.addPoints(pts);
992  }
993  }
994  }
995  if(iter->first>0)
996  {
997  groundClouds_.insert(std::make_pair(iter->first, util3d::transformPointCloud(subtractedCloud, iter->second.inverse())));
998  }
999  if(subtractedCloud->size())
1000  {
1001  *assembledGround_+=*subtractedCloud;
1002  }
1003  ++countGrounds;
1004  }
1005  }
1006  if(updateObstacles && assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end())
1007  {
1008  if(iter->first > 0)
1009  {
1010  assembledObstaclePoses_.insert(*iter);
1011  }
1012  if(jter!=localMaps_.end() && jter->second.obstacleCells.cols)
1013  {
1014  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.obstacleCells), iter->second, 255, 0, 0);
1015  pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
1016  if(cloudSubtractFiltering_)
1017  {
1018  if(assembledObstacleIndex_.indexedFeatures())
1019  {
1020  subtractedCloud = subtractFiltering(transformed, assembledObstacleIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
1021  }
1022  if(subtractedCloud->size())
1023  {
1024  UDEBUG("Adding obstacle %d pts=%d/%d (index=%d)", iter->first, subtractedCloud->size(), transformed->size(), assembledObstacleIndex_.indexedFeatures());
1025  cv::Mat pts(subtractedCloud->size(), 3, CV_32FC1);
1026  for(unsigned int i=0; i<subtractedCloud->size(); ++i)
1027  {
1028  pts.at<float>(i, 0) = subtractedCloud->at(i).x;
1029  pts.at<float>(i, 1) = subtractedCloud->at(i).y;
1030  pts.at<float>(i, 2) = subtractedCloud->at(i).z;
1031  }
1032  if(!assembledObstacleIndex_.isBuilt())
1033  {
1034  assembledObstacleIndex_.buildKDTreeSingleIndex(pts, 15);
1035  }
1036  else
1037  {
1038  assembledObstacleIndex_.addPoints(pts);
1039  }
1040  }
1041  }
1042  if(iter->first>0)
1043  {
1044  obstacleClouds_.insert(std::make_pair(iter->first, util3d::transformPointCloud(subtractedCloud, iter->second.inverse())));
1045  }
1046  if(subtractedCloud->size())
1047  {
1048  *assembledObstacles_+=*subtractedCloud;
1049  }
1050  ++countObstacles;
1051  }
1052  }
1053  }
1054 
1055  if(cloudOutputVoxelized_)
1056  {
1057  UASSERT(occupancyGrid_->getCellSize() > 0.0);
1058  if(countGrounds && assembledGround_->size())
1059  {
1060  assembledGround_ = util3d::voxelize(assembledGround_, occupancyGrid_->getCellSize());
1061  }
1062  if(countObstacles && assembledObstacles_->size())
1063  {
1064  assembledObstacles_ = util3d::voxelize(assembledObstacles_, occupancyGrid_->getCellSize());
1065  }
1066  }
1067 
1068  ROS_INFO("Assembled %d obstacle and %d ground clouds (%d points, %fs)",
1069  countObstacles, countGrounds, (int)(assembledGround_->size() + assembledObstacles_->size()), time.ticks());
1070 
1071  if( countGrounds > 0 ||
1072  countObstacles > 0 ||
1073  !latching_ ||
1074  (assembledGround_->empty() && assembledObstacles_->empty()) ||
1075  (cloudGroundPub_.getNumSubscribers() && !latched_.at(&cloudGroundPub_)) ||
1076  (cloudObstaclesPub_.getNumSubscribers() && !latched_.at(&cloudObstaclesPub_)) ||
1077  (cloudMapPub_.getNumSubscribers() && !latched_.at(&cloudMapPub_)) ||
1078  (scanMapPub_.getNumSubscribers() && !latched_.at(&scanMapPub_)))
1079  {
1080  if(cloudGroundPub_.getNumSubscribers())
1081  {
1082  sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
1083  pcl::toROSMsg(*assembledGround_, *cloudMsg);
1084  cloudMsg->header.stamp = stamp;
1085  cloudMsg->header.frame_id = mapFrameId;
1086  cloudGroundPub_.publish(cloudMsg);
1087  latched_.at(&cloudGroundPub_) = true;
1088  }
1089  if(cloudObstaclesPub_.getNumSubscribers())
1090  {
1091  sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
1092  pcl::toROSMsg(*assembledObstacles_, *cloudMsg);
1093  cloudMsg->header.stamp = stamp;
1094  cloudMsg->header.frame_id = mapFrameId;
1095  cloudObstaclesPub_.publish(cloudMsg);
1096  latched_.at(&cloudObstaclesPub_) = true;
1097  }
1098  if(cloudMapPub_.getNumSubscribers() || scanMapPub_.getNumSubscribers())
1099  {
1100  pcl::PointCloud<pcl::PointXYZRGB> cloud = *assembledObstacles_ + *assembledGround_;
1101  sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
1102  pcl::toROSMsg(cloud, *cloudMsg);
1103  cloudMsg->header.stamp = stamp;
1104  cloudMsg->header.frame_id = mapFrameId;
1105 
1106  if(cloudMapPub_.getNumSubscribers())
1107  {
1108  cloudMapPub_.publish(cloudMsg);
1109  latched_.at(&cloudMapPub_) = true;
1110  }
1111  if(scanMapPub_.getNumSubscribers())
1112  {
1113  scanMapPub_.publish(cloudMsg);
1114  latched_.at(&scanMapPub_) = true;
1115  }
1116  }
1117  }
1118  }
1119  else if(mapCacheCleanup_)
1120  {
1121  if(!groundClouds_.empty() || !obstacleClouds_.empty())
1122  {
1123  size_t totalBytes = 0;
1124  for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();iter!=groundClouds_.end();++iter)
1125  {
1126  totalBytes += sizeof(int) + iter->second->points.size()*sizeof(pcl::PointXYZRGB);
1127  }
1128  for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=obstacleClouds_.begin();iter!=obstacleClouds_.end();++iter)
1129  {
1130  totalBytes += sizeof(int) + iter->second->points.size()*sizeof(pcl::PointXYZRGB);
1131  }
1132  totalBytes += (assembledGround_->size() + assembledObstacles_->size()) *sizeof(pcl::PointXYZRGB);
1133  totalBytes += (assembledGroundPoses_.size() + assembledObstaclePoses_.size()) * 13*sizeof(float);
1134  totalBytes += assembledGroundIndex_.indexedFeatures()*assembledGroundIndex_.featuresDim() * sizeof(float);
1135  totalBytes += assembledObstacleIndex_.indexedFeatures()*assembledObstacleIndex_.featuresDim() * sizeof(float);
1136  ROS_INFO("MapsManager: cleanup point clouds (%ld points, %ld cached clouds, ~%ld MB)...",
1137  assembledGround_->size()+assembledObstacles_->size(),
1138  groundClouds_.size()+obstacleClouds_.size(),
1139  totalBytes/1048576);
1140  }
1141  assembledGround_->clear();
1142  assembledObstacles_->clear();
1143  assembledGroundPoses_.clear();
1144  assembledObstaclePoses_.clear();
1145  assembledGroundIndex_.release();
1146  assembledObstacleIndex_.release();
1147  groundClouds_.clear();
1148  obstacleClouds_.clear();
1149  }
1150  if(cloudMapPub_.getNumSubscribers() == 0)
1151  {
1152  latched_.at(&cloudMapPub_) = false;
1153  }
1154  if(scanMapPub_.getNumSubscribers() == 0)
1155  {
1156  latched_.at(&scanMapPub_) = false;
1157  }
1158  if(cloudGroundPub_.getNumSubscribers() == 0)
1159  {
1160  latched_.at(&cloudGroundPub_) = false;
1161  }
1162  if(cloudObstaclesPub_.getNumSubscribers() == 0)
1163  {
1164  latched_.at(&cloudObstaclesPub_) = false;
1165  }
1166 
1167 #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
1168  if( octomapUpdated_ ||
1169  !latching_ ||
1170  (octoMapPubBin_.getNumSubscribers() && !latched_.at(&octoMapPubBin_)) ||
1171  (octoMapPubFull_.getNumSubscribers() && !latched_.at(&octoMapPubFull_)) ||
1172  (octoMapCloud_.getNumSubscribers() && !latched_.at(&octoMapCloud_)) ||
1173  (octoMapFrontierCloud_.getNumSubscribers() && !latched_.at(&octoMapFrontierCloud_)) ||
1174  (octoMapObstacleCloud_.getNumSubscribers() && !latched_.at(&octoMapObstacleCloud_)) ||
1175  (octoMapGroundCloud_.getNumSubscribers() && !latched_.at(&octoMapGroundCloud_)) ||
1176  (octoMapEmptySpace_.getNumSubscribers() && !latched_.at(&octoMapEmptySpace_)) ||
1177  (octoMapProj_.getNumSubscribers() && !latched_.at(&octoMapProj_)))
1178  {
1179  if(octoMapPubBin_.getNumSubscribers())
1180  {
1181  octomap_msgs::Octomap msg;
1182  octomap_msgs::binaryMapToMsg(*octomap_->octree(), msg);
1183  msg.header.frame_id = mapFrameId;
1184  msg.header.stamp = stamp;
1185  octoMapPubBin_.publish(msg);
1186  latched_.at(&octoMapPubBin_) = true;
1187  }
1188  if(octoMapPubFull_.getNumSubscribers())
1189  {
1190  octomap_msgs::Octomap msg;
1191  octomap_msgs::fullMapToMsg(*octomap_->octree(), msg);
1192  msg.header.frame_id = mapFrameId;
1193  msg.header.stamp = stamp;
1194  octoMapPubFull_.publish(msg);
1195  latched_.at(&octoMapPubFull_) = true;
1196  }
1197  if(octoMapCloud_.getNumSubscribers() ||
1198  octoMapFrontierCloud_.getNumSubscribers() ||
1199  octoMapObstacleCloud_.getNumSubscribers() ||
1200  octoMapGroundCloud_.getNumSubscribers() ||
1201  octoMapEmptySpace_.getNumSubscribers())
1202  {
1203  sensor_msgs::PointCloud2 msg;
1204  pcl::IndicesPtr obstacleIndices(new std::vector<int>);
1205  pcl::IndicesPtr frontierIndices(new std::vector<int>);
1206  pcl::IndicesPtr emptyIndices(new std::vector<int>);
1207  pcl::IndicesPtr groundIndices(new std::vector<int>);
1208  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get(), true, frontierIndices.get(),0);
1209 
1210  if(octoMapCloud_.getNumSubscribers())
1211  {
1212  pcl::PointCloud<pcl::PointXYZRGB> cloudOccupiedSpace;
1213  pcl::IndicesPtr indices = util3d::concatenate(obstacleIndices, groundIndices);
1214  pcl::copyPointCloud(*cloud, *indices, cloudOccupiedSpace);
1215  pcl::toROSMsg(cloudOccupiedSpace, msg);
1216  msg.header.frame_id = mapFrameId;
1217  msg.header.stamp = stamp;
1218  octoMapCloud_.publish(msg);
1219  latched_.at(&octoMapCloud_) = true;
1220  }
1221  if(octoMapFrontierCloud_.getNumSubscribers())
1222  {
1223  pcl::PointCloud<pcl::PointXYZRGB> cloudFrontier;
1224  pcl::copyPointCloud(*cloud, *frontierIndices, cloudFrontier);
1225  pcl::toROSMsg(cloudFrontier, msg);
1226  msg.header.frame_id = mapFrameId;
1227  msg.header.stamp = stamp;
1228  octoMapFrontierCloud_.publish(msg);
1229  latched_.at(&octoMapFrontierCloud_) = true;
1230  }
1231  if(octoMapObstacleCloud_.getNumSubscribers())
1232  {
1233  pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
1234  pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles);
1235  pcl::toROSMsg(cloudObstacles, msg);
1236  msg.header.frame_id = mapFrameId;
1237  msg.header.stamp = stamp;
1238  octoMapObstacleCloud_.publish(msg);
1239  latched_.at(&octoMapObstacleCloud_) = true;
1240  }
1241  if(octoMapGroundCloud_.getNumSubscribers())
1242  {
1243  pcl::PointCloud<pcl::PointXYZRGB> cloudGround;
1244  pcl::copyPointCloud(*cloud, *groundIndices, cloudGround);
1245  pcl::toROSMsg(cloudGround, msg);
1246  msg.header.frame_id = mapFrameId;
1247  msg.header.stamp = stamp;
1248  octoMapGroundCloud_.publish(msg);
1249  latched_.at(&octoMapGroundCloud_) = true;
1250  }
1251  if(octoMapEmptySpace_.getNumSubscribers())
1252  {
1253  pcl::PointCloud<pcl::PointXYZRGB> cloudEmptySpace;
1254  pcl::copyPointCloud(*cloud, *emptyIndices, cloudEmptySpace);
1255  pcl::toROSMsg(cloudEmptySpace, msg);
1256  msg.header.frame_id = mapFrameId;
1257  msg.header.stamp = stamp;
1258  octoMapEmptySpace_.publish(msg);
1259  latched_.at(&octoMapEmptySpace_) = true;
1260  }
1261  }
1262  if(octoMapProj_.getNumSubscribers())
1263  {
1264  // create the projection map
1265  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
1266  cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, occupancyGrid_->getMinMapSize(), octomapTreeDepth_);
1267 
1268  if(!pixels.empty())
1269  {
1270  //init
1271  nav_msgs::OccupancyGrid map;
1272  map.info.resolution = gridCellSize;
1273  map.info.origin.position.x = 0.0;
1274  map.info.origin.position.y = 0.0;
1275  map.info.origin.position.z = 0.0;
1276  map.info.origin.orientation.x = 0.0;
1277  map.info.origin.orientation.y = 0.0;
1278  map.info.origin.orientation.z = 0.0;
1279  map.info.origin.orientation.w = 1.0;
1280 
1281  map.info.width = pixels.cols;
1282  map.info.height = pixels.rows;
1283  map.info.origin.position.x = xMin;
1284  map.info.origin.position.y = yMin;
1285  map.data.resize(map.info.width * map.info.height);
1286 
1287  memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
1288 
1289  map.header.frame_id = mapFrameId;
1290  map.header.stamp = stamp;
1291 
1292  octoMapProj_.publish(map);
1293  latched_.at(&octoMapProj_) = true;
1294  }
1295  else if(poses.size())
1296  {
1297  ROS_WARN("Octomap projection map is empty! (poses=%d octomap nodes=%d). "
1298  "Make sure you enabled \"%s\" and set \"%s\"=1. "
1299  "See \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" for more info.",
1300  (int)poses.size(), (int)octomap_->octree()->size(),
1301  Parameters::kGrid3D().c_str(), Parameters::kGridSensor().c_str());
1302  }
1303  }
1304  }
1305 
1306  if( mapCacheCleanup_ &&
1307  octoMapPubBin_.getNumSubscribers() == 0 &&
1308  octoMapPubFull_.getNumSubscribers() == 0 &&
1309  octoMapCloud_.getNumSubscribers() == 0 &&
1310  octoMapFrontierCloud_.getNumSubscribers() == 0 &&
1311  octoMapObstacleCloud_.getNumSubscribers() == 0 &&
1312  octoMapGroundCloud_.getNumSubscribers() == 0 &&
1313  octoMapEmptySpace_.getNumSubscribers() == 0 &&
1314  octoMapProj_.getNumSubscribers() == 0)
1315  {
1316  if(octomap_->octree()->getNumLeafNodes()>0)
1317  {
1318  ROS_INFO("MapsManager: cleanup octomap (%ld leaf nodes, ~%ld MB)...",
1319  octomap_->octree()->getNumLeafNodes(),
1320  octomap_->octree()->memoryUsage()/1048576);
1321  }
1322  octomap_->clear();
1323  }
1324 
1325  if(octoMapPubBin_.getNumSubscribers() == 0)
1326  {
1327  latched_.at(&octoMapPubBin_) = false;
1328  }
1329  if(octoMapPubFull_.getNumSubscribers() == 0)
1330  {
1331  latched_.at(&octoMapPubFull_) = false;
1332  }
1333  if(octoMapCloud_.getNumSubscribers() == 0)
1334  {
1335  latched_.at(&octoMapCloud_) = false;
1336  }
1337  if(octoMapFrontierCloud_.getNumSubscribers() == 0)
1338  {
1339  latched_.at(&octoMapFrontierCloud_) = false;
1340  }
1341  if(octoMapObstacleCloud_.getNumSubscribers() == 0)
1342  {
1343  latched_.at(&octoMapObstacleCloud_) = false;
1344  }
1345  if(octoMapGroundCloud_.getNumSubscribers() == 0)
1346  {
1347  latched_.at(&octoMapGroundCloud_) = false;
1348  }
1349  if(octoMapEmptySpace_.getNumSubscribers() == 0)
1350  {
1351  latched_.at(&octoMapEmptySpace_) = false;
1352  }
1353  if(octoMapProj_.getNumSubscribers() == 0)
1354  {
1355  latched_.at(&octoMapProj_) = false;
1356  }
1357 #endif
1358 
1359  if( gridUpdated_ ||
1360  !latching_ ||
1361  (gridMapPub_.getNumSubscribers() && !latched_.at(&gridMapPub_)) ||
1362  (projMapPub_.getNumSubscribers() && !latched_.at(&projMapPub_)) ||
1363  (gridProbMapPub_.getNumSubscribers() && !latched_.at(&gridProbMapPub_)))
1364  {
1365  if(projMapPub_.getNumSubscribers())
1366  {
1367  if(parameters_.find(Parameters::kGridSensor()) != parameters_.end() &&
1368  uStr2Int(parameters_.at(Parameters::kGridSensor()))==0)
1369  {
1370  ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic "
1371  "instead with <param name=\"%s\" type=\"string\" value=\"1\"/>. "
1372  "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
1373  "all occupancy grid parameters.",
1374  Parameters::kGridSensor().c_str());
1375  }
1376  else
1377  {
1378  ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic instead.");
1379  }
1380  }
1381 
1382  if(gridProbMapPub_.getNumSubscribers())
1383  {
1384  // create the grid map
1385  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
1386  cv::Mat pixels = this->getGridProbMap(xMin, yMin, gridCellSize);
1387  if(!pixels.empty())
1388  {
1389  //init
1390  nav_msgs::OccupancyGrid map;
1391  map.info.resolution = gridCellSize;
1392  map.info.origin.position.x = 0.0;
1393  map.info.origin.position.y = 0.0;
1394  map.info.origin.position.z = 0.0;
1395  map.info.origin.orientation.x = 0.0;
1396  map.info.origin.orientation.y = 0.0;
1397  map.info.origin.orientation.z = 0.0;
1398  map.info.origin.orientation.w = 1.0;
1399 
1400  map.info.width = pixels.cols;
1401  map.info.height = pixels.rows;
1402  map.info.origin.position.x = xMin;
1403  map.info.origin.position.y = yMin;
1404  map.data.resize(map.info.width * map.info.height);
1405 
1406  memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
1407 
1408  map.header.frame_id = mapFrameId;
1409  map.header.stamp = stamp;
1410 
1411  if(gridProbMapPub_.getNumSubscribers())
1412  {
1413  gridProbMapPub_.publish(map);
1414  latched_.at(&gridProbMapPub_) = true;
1415  }
1416  }
1417  else if(poses.size())
1418  {
1419  ROS_WARN("Grid map is empty! (local maps=%d)", (int)localMaps_.size());
1420  }
1421  }
1422  if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers())
1423  {
1424  // create the grid map
1425  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
1426  cv::Mat pixels = this->getGridMap(xMin, yMin, gridCellSize);
1427 
1428  if(!pixels.empty())
1429  {
1430  //init
1431  nav_msgs::OccupancyGrid map;
1432  map.info.resolution = gridCellSize;
1433  map.info.origin.position.x = 0.0;
1434  map.info.origin.position.y = 0.0;
1435  map.info.origin.position.z = 0.0;
1436  map.info.origin.orientation.x = 0.0;
1437  map.info.origin.orientation.y = 0.0;
1438  map.info.origin.orientation.z = 0.0;
1439  map.info.origin.orientation.w = 1.0;
1440 
1441  map.info.width = pixels.cols;
1442  map.info.height = pixels.rows;
1443  map.info.origin.position.x = xMin;
1444  map.info.origin.position.y = yMin;
1445  map.data.resize(map.info.width * map.info.height);
1446 
1447  memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
1448 
1449  map.header.frame_id = mapFrameId;
1450  map.header.stamp = stamp;
1451 
1452  if(gridMapPub_.getNumSubscribers())
1453  {
1454  gridMapPub_.publish(map);
1455  latched_.at(&gridMapPub_) = true;
1456  }
1457  if(projMapPub_.getNumSubscribers())
1458  {
1459  projMapPub_.publish(map);
1460  latched_.at(&projMapPub_) = true;
1461  }
1462  }
1463  else if(poses.size())
1464  {
1465  ROS_WARN("Grid map is empty! (local maps=%d)", (int)localMaps_.size());
1466  }
1467  }
1468  }
1469 
1470  if(gridMapPub_.getNumSubscribers() == 0)
1471  {
1472  latched_.at(&gridMapPub_) = false;
1473  }
1474  if(projMapPub_.getNumSubscribers() == 0)
1475  {
1476  latched_.at(&projMapPub_) = false;
1477  }
1478  if(gridProbMapPub_.getNumSubscribers() == 0)
1479  {
1480  latched_.at(&gridProbMapPub_) = false;
1481  }
1482 #if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
1483  if( elevationMapUpdated_ ||
1484  !latching_ ||
1485  (elevationMapPub_.getNumSubscribers() && !latched_.at(&elevationMapPub_)))
1486  {
1487  grid_map_msgs::GridMap msg;
1488 #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)
1489  grid_map::GridMapRosConverter::toMessage(*elevationMap_->gridMap(), msg);
1490 #else
1491  grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap(), msg);
1492 #endif
1493  msg.info.header.frame_id = mapFrameId;
1494  msg.info.header.stamp = stamp;
1495  elevationMapPub_.publish(msg);
1496  }
1497  if(elevationMapPub_.getNumSubscribers() == 0)
1498  {
1499  latched_.at(&elevationMapPub_) = false;
1500  }
1501  if( mapCacheCleanup_ &&
1502  elevationMapPub_.getNumSubscribers() == 0)
1503  {
1504  elevationMap_->clear();
1505  }
1506 #endif
1507  if(!this->hasSubscribers() && mapCacheCleanup_)
1508  {
1509  if(!localMaps_.empty())
1510  {
1511  size_t totalBytes = localMaps_.getMemoryUsed();
1512  ROS_INFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", localMaps_.size(), totalBytes/1048576);
1513  }
1514  localMaps_.clear();
1515  }
1516 }
1517 
1518 cv::Mat MapsManager::getGridMap(
1519  float & xMin,
1520  float & yMin,
1521  float & gridCellSize)
1522 {
1523  gridCellSize = occupancyGrid_->getCellSize();
1524  return occupancyGrid_->getMap(xMin, yMin);
1525 }
1526 
1527 cv::Mat MapsManager::getGridProbMap(
1528  float & xMin,
1529  float & yMin,
1530  float & gridCellSize)
1531 {
1532  gridCellSize = occupancyGrid_->getCellSize();
1533  return occupancyGrid_->getProbMap(xMin, yMin);
1534 }
1535 
1536 } // namespace rtabmap_ros
1537 
rtabmap::SensorData
rtabmap_util::MapsManager::parameters_
rtabmap::ParametersMap parameters_
Definition: MapsManager.h:143
ParametersPair
std::pair< std::string, std::string > ParametersPair
int
int
rtabmap_util::MapsManager::octoMapGroundCloud_
ros::Publisher octoMapGroundCloud_
Definition: MapsManager.h:115
rtabmap::LocalGridCache::find
std::map< int, LocalGrid >::const_iterator find(int nodeId) const
rtabmap_util::MapsManager::getGridProbMap
cv::Mat getGridProbMap(float &xMin, float &yMin, float &gridCellSize)
Definition: MapsManager.cpp:1527
pcl
rtabmap_util::MapsManager::localMapMaker_
rtabmap::LocalGridMaker * localMapMaker_
Definition: MapsManager.h:133
rtabmap_util::MapsManager::cloudObstaclesPub_
ros::Publisher cloudObstaclesPub_
Definition: MapsManager.h:106
OctoMap.h
name
octomap_msgs::fullMapToMsg
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
rtabmap_util::MapsManager::octoMapEmptySpace_
ros::Publisher octoMapEmptySpace_
Definition: MapsManager.h:117
rtabmap_util::parameterMoved
void parameterMoved(ros::NodeHandle &nh, const std::string &rosName, const std::string &parameterName, ParametersMap &parameters)
Definition: MapsManager.cpp:236
rtabmap::LocalGridCache::getMemoryUsed
unsigned long getMemoryUsed() const
rtabmap_util::MapsManager::isMapUpdated
bool isMapUpdated() const
Definition: MapsManager.cpp:434
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ros.h
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
rtabmap::FlannIndex::buildKDTreeSingleIndex
void buildKDTreeSingleIndex(const cv::Mat &features, int leafMaxSize=10, bool reorder=true, bool useDistanceL1=false, float rebalancingFactor=2.0f)
end
end
rtabmap_util::MapsManager::assembledGroundPoses_
std::map< int, rtabmap::Transform > assembledGroundPoses_
Definition: MapsManager.h:121
if
if(ret >=0) *info
type
rtabmap::GridMap::gridMap
const grid_map::GridMap * gridMap() const
rtabmap::FlannIndex::addPoints
std::vector< unsigned int > addPoints(const cv::Mat &features)
UStl.h
PointCloud
sensor_msgs::PointCloud2 PointCloud
rtabmap_util::MapsManager::cloudMapPub_
ros::Publisher cloudMapPub_
Definition: MapsManager.h:104
rtabmap_util::MapsManager::cloudGroundPub_
ros::Publisher cloudGroundPub_
Definition: MapsManager.h:105
rtabmap::OctoMap::octree
const RtabmapColorOcTree * octree() const
rtabmap_util::MapsManager::scanMapPub_
ros::Publisher scanMapPub_
Definition: MapsManager.h:110
true
#define true
iterator
rtabmap::LocalGridCache::localGrids
const std::map< int, LocalGrid > & localGrids() const
MapsManager.h
rtabmap_util::MapsManager::alwaysUpdateMap_
bool alwaysUpdateMap_
Definition: MapsManager.h:101
rtabmap::LaserScan
rtabmap::OctoMap::createCloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createCloud(unsigned int treeDepth=0, std::vector< int > *obstacleIndices=0, std::vector< int > *emptyIndices=0, std::vector< int > *groundIndices=0, bool originalRefPoints=true, std::vector< int > *frontierIndices=0, std::vector< double > *cloudProb=0) const
ParametersMap
std::map< std::string, std::string > ParametersMap
uNumber2Str
std::string uNumber2Str(double number, int precision, bool fixed)
rtabmap_util
Definition: MapsManager.h:49
rtabmap_util::MapsManager::occupancyGrid_
rtabmap::OccupancyGrid * occupancyGrid_
Definition: MapsManager.h:132
rtabmap::GridMap
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rtabmap::OccupancyGrid
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
rtabmap_util::MapsManager::octoMapCloud_
ros::Publisher octoMapCloud_
Definition: MapsManager.h:113
rtabmap_util::MapsManager::clear
void clear()
Definition: MapsManager.cpp:390
rtabmap_util::MapsManager::groundClouds_
std::map< int, pcl::PointCloud< pcl::PointXYZRGB >::Ptr > groundClouds_
Definition: MapsManager.h:127
indices
indices
rtabmap_util::MapsManager::octomap_
rtabmap::OctoMap * octomap_
Definition: MapsManager.h:136
rtabmap_util::MapsManager::cloudSubtractFilteringMinNeighbors_
int cloudSubtractFilteringMinNeighbors_
Definition: MapsManager.h:97
rtabmap::FlannIndex::indexedFeatures
size_t indexedFeatures() const
rtabmap_util::MapsManager::assembledObstacles_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr assembledObstacles_
Definition: MapsManager.h:123
rtabmap_util::MapsManager::octoMapPubBin_
ros::Publisher octoMapPubBin_
Definition: MapsManager.h:111
rtabmap::LocalGridCache::empty
bool empty() const
rtabmap::GlobalMap::addedNodes
const std::map< int, Transform > & addedNodes() const
uContains
bool uContains(const std::list< V > &list, const V &value)
rtabmap_util::MapsManager::gridUpdated_
bool gridUpdated_
Definition: MapsManager.h:134
octomap_msgs::binaryMapToMsg
static bool binaryMapToMsg(const OctomapT &octomap, Octomap &msg)
rtabmap_util::MapsManager::mapFilterAngle_
double mapFilterAngle_
Definition: MapsManager.h:99
rtabmap::LocalGridMaker::getCellSize
float getCellSize() const
uBool2Str
std::string uBool2Str(bool boolean)
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
rtabmap_util::MapsManager::~MapsManager
virtual ~MapsManager()
Definition: MapsManager.cpp:221
data
int data[]
rtabmap_util::MapsManager::localMaps_
rtabmap::LocalGridCache localMaps_
Definition: MapsManager.h:130
rtabmap_util::MapsManager::getFilteredPoses
std::map< int, rtabmap::Transform > getFilteredPoses(const std::map< int, rtabmap::Transform > &poses)
Definition: MapsManager.cpp:446
rtabmap::OctoMap::clear
virtual void clear()
rtabmap::LocalGridMaker
rtabmap::LocalGridCache::clear
void clear(bool temporaryOnly=false)
rtabmap::GlobalMap::update
bool update(const std::map< int, Transform > &poses)
rtabmap_util::MapsManager::mapCacheCleanup_
bool mapCacheCleanup_
Definition: MapsManager.h:100
ROS_DEBUG
#define ROS_DEBUG(...)
rtabmap_util::MapsManager::elevationMapPub_
ros::Publisher elevationMapPub_
Definition: MapsManager.h:119
rtabmap::Signature::setPose
void setPose(const Transform &pose)
rtabmap_util::MapsManager::gridProbMapPub_
ros::Publisher gridProbMapPub_
Definition: MapsManager.h:109
rtabmap_util::MapsManager::octoMapProj_
ros::Publisher octoMapProj_
Definition: MapsManager.h:118
rtabmap::OccupancyGrid::clear
virtual void clear()
rtabmap_util::MapsManager::assembledGround_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr assembledGround_
Definition: MapsManager.h:124
rtabmap_util::MapsManager::mapFilterRadius_
double mapFilterRadius_
Definition: MapsManager.h:98
rtabmap::OccupancyGrid::getMinMapSize
float getMinMapSize() const
rtabmap_util::subtractFiltering
pcl::PointCloud< pcl::PointXYZRGB >::Ptr subtractFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const rtabmap::FlannIndex &substractCloudIndex, float radiusSearch, int minNeighborsInRadius)
Definition: MapsManager.cpp:750
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
UASSERT
#define UASSERT(condition)
ROS_WARN
#define ROS_WARN(...)
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
rtabmap::Memory::getNodeData
SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const
angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::FlannIndex
rtabmap_util::MapsManager::set2DMap
void set2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize, const std::map< int, rtabmap::Transform > &poses, const rtabmap::Memory *memory=0)
Definition: MapsManager.cpp:348
rtabmap_util::MapsManager::scanEmptyRayTracing_
bool scanEmptyRayTracing_
Definition: MapsManager.h:102
time
#define time
LocalGridMaker.h
rtabmap_util::MapsManager::getGridMap
cv::Mat getGridMap(float &xMin, float &yMin, float &gridCellSize)
Definition: MapsManager.cpp:1518
rtabmap_util::MapsManager::assembledObstaclePoses_
std::map< int, rtabmap::Transform > assembledObstaclePoses_
Definition: MapsManager.h:122
rtabmap_util::MapsManager::elevationMap_
rtabmap::GridMap * elevationMap_
Definition: MapsManager.h:140
rtabmap_util::MapsManager::assembledGroundIndex_
rtabmap::FlannIndex assembledGroundIndex_
Definition: MapsManager.h:125
rtabmap::LocalGridCache::add
void add(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint=cv::Point3f(0, 0, 0))
UWARN
#define UWARN(...)
rtabmap_util::MapsManager::latching_
bool latching_
Definition: MapsManager.h:145
uStr2Int
int uStr2Int(const std::string &str)
util3d_filtering.h
rtabmap_util::MapsManager::obstacleClouds_
std::map< int, pcl::PointCloud< pcl::PointXYZRGB >::Ptr > obstacleClouds_
Definition: MapsManager.h:128
rtabmap_util::MapsManager::hasSubscribers
bool hasSubscribers() const
Definition: MapsManager.cpp:414
rtabmap::LocalGridMaker::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
rtabmap_util::MapsManager::init
void init(ros::NodeHandle &nh, ros::NodeHandle &pnh, const std::string &name, bool usePublicNamespace)
Definition: MapsManager.cpp:95
rtabmap::FlannIndex::release
void release()
rtabmap_util::MapsManager::octoMapObstacleCloud_
ros::Publisher octoMapObstacleCloud_
Definition: MapsManager.h:116
UTimer::ticks
double ticks()
rtabmap_util::MapsManager::cloudOutputVoxelized_
bool cloudOutputVoxelized_
Definition: MapsManager.h:95
ros::Time
rtabmap_util::MapsManager::octomapTreeDepth_
int octomapTreeDepth_
Definition: MapsManager.h:137
iter
iterator iter(handle obj)
rtabmap::FlannIndex::radiusSearch
void radiusSearch(const cv::Mat &query, std::vector< std::vector< size_t > > &indices, std::vector< std::vector< float > > &dists, float radius, int maxNeighbors=0, int checks=32, float eps=0.0, bool sorted=true) const
rtabmap::GlobalMap::getUpdateError
float getUpdateError() const
OccupancyGrid.h
rtabmap::LocalGridCache::end
std::map< int, LocalGrid >::const_iterator end() const
republish_tf_static.msg
msg
Definition: republish_tf_static.py:5
c_str
const char * c_str(Args &&...args)
rtabmap::LocalGridMaker::createLocalMap
void createLocalMap(const LaserScan &cloud, const Transform &pose, cv::Mat &groundCells, cv::Mat &obstacleCells, cv::Mat &emptyCells, cv::Point3f &viewPointInOut) const
rtabmap_util::MapsManager::elevationMapUpdated_
bool elevationMapUpdated_
Definition: MapsManager.h:141
uStr2Bool
bool uStr2Bool(const char *str)
ROS_ERROR
#define ROS_ERROR(...)
GridMap.h
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
else
else
rtabmap::FlannIndex::featuresDim
int featuresDim() const
rtabmap::FlannIndex::isBuilt
bool isBuilt()
UTimer
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
rtabmap::OccupancyGrid::getProbMap
cv::Mat getProbMap(float &xMin, float &yMin) const
rtabmap::Memory
pcl::toROSMsg
void toROSMsg(const pcl::PointCloud< T > &cloud, sensor_msgs::Image &msg)
float
float
ULogger.h
conversions.h
rtabmap_util::MapsManager::latched_
std::map< void *, bool > latched_
Definition: MapsManager.h:146
rtabmap::GlobalMap::getCellSize
float getCellSize() const
rtabmap::Memory::getDatabaseVersion
std::string getDatabaseVersion() const
rtabmap::OctoMap
rtabmap_util::MapsManager::assembledObstacleIndex_
rtabmap::FlannIndex assembledObstacleIndex_
Definition: MapsManager.h:126
false
#define false
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
rtabmap::LocalGridMaker::isGridFromDepth
bool isGridFromDepth() const
rtabmap::GridMap::clear
virtual void clear()
UTimer.h
Memory.h
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_CORE_EXPORT radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
t
Point2 t(10, 10)
ROS_INFO
#define ROS_INFO(...)
rtabmap_util::MapsManager::projMapPub_
ros::Publisher projMapPub_
Definition: MapsManager.h:107
UConversion.h
rtabmap_util::MapsManager::cloudSubtractFiltering_
bool cloudSubtractFiltering_
Definition: MapsManager.h:96
rtabmap
rtabmap::LocalGridCache::size
size_t size() const
rtabmap_util::MapsManager::setParameters
void setParameters(const rtabmap::ParametersMap &parameters)
Definition: MapsManager.cpp:329
util3d_transforms.h
uStr2Double
double uStr2Double(const std::string &str)
rtabmap::OccupancyGrid::setMap
void setMap(const cv::Mat &map, float xMin, float yMin, float cellSize, const std::map< int, Transform > &poses)
Graph.h
i
int i
rtabmap_util::MapsManager::updateMapCaches
std::map< int, rtabmap::Transform > updateMapCaches(const std::map< int, rtabmap::Transform > &poses, const rtabmap::Memory *memory, bool updateGrid, bool updateOctomap, const std::map< int, rtabmap::Signature > &signatures=std::map< int, rtabmap::Signature >())
Definition: MapsManager.cpp:457
rtabmap_util::MapsManager::octoMapPubFull_
ros::Publisher octoMapPubFull_
Definition: MapsManager.h:112
ros::NodeHandle
rtabmap::Signature
rtabmap_util::MapsManager::backwardCompatibilityParameters
void backwardCompatibilityParameters(ros::NodeHandle &pnh, rtabmap::ParametersMap &parameters) const
Definition: MapsManager.cpp:289
rtabmap_util::MapsManager::octomapUpdated_
bool octomapUpdated_
Definition: MapsManager.h:138
rtabmap_util::MapsManager::publishMaps
void publishMaps(const std::map< int, rtabmap::Transform > &poses, const ros::Time &stamp, const std::string &mapFrameId)
Definition: MapsManager.cpp:777
util2d.h
rtabmap_util::MapsManager::gridMapPub_
ros::Publisher gridMapPub_
Definition: MapsManager.h:108
rtabmap_util::MapsManager::octoMapFrontierCloud_
ros::Publisher octoMapFrontierCloud_
Definition: MapsManager.h:114
pcl_conversions.h
util3d_mapping.h

rtabmap_util
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:40:50