CoreWrapper.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  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap_ros/CoreWrapper.h"
29 
30 #include <stdio.h>
31 #include <ros/ros.h>
32 #include "pluginlib/class_list_macros.h"
33 
34 #include <nav_msgs/Path.h>
35 #include <std_msgs/Int32MultiArray.h>
36 #include <std_msgs/Bool.h>
37 #include <sensor_msgs/image_encodings.h>
38 #include <cv_bridge/cv_bridge.h>
39 #include <pcl/io/io.h>
40 
41 #include <visualization_msgs/MarkerArray.h>
42 
43 #include <rtabmap/utilite/UTimer.h>
44 #include <rtabmap/utilite/UDirectory.h>
45 #include <rtabmap/utilite/UFile.h>
46 #include <rtabmap/utilite/UConversion.h>
47 #include <rtabmap/utilite/UStl.h>
48 #include <rtabmap/utilite/UMath.h>
49 
50 #include <rtabmap/core/util2d.h>
51 #include <rtabmap/core/util3d.h>
52 #include <rtabmap/core/util3d_transforms.h>
53 #include <rtabmap/core/util3d_surface.h>
54 #include <rtabmap/core/Memory.h>
55 #include <rtabmap/core/OdometryEvent.h>
56 #include <rtabmap/core/Version.h>
57 #include <rtabmap/core/OccupancyGrid.h>
58 #include <rtabmap/core/DBDriver.h>
59 #include <rtabmap/core/Registration.h>
60 #include <rtabmap/core/Graph.h>
61 
62 #ifdef WITH_OCTOMAP_MSGS
63 #ifdef RTABMAP_OCTOMAP
64 #include <octomap_msgs/conversions.h>
65 #include <rtabmap/core/OctoMap.h>
66 #endif
67 #endif
68 
69 #define BAD_COVARIANCE 9999
70 
71 //msgs
72 #include "rtabmap_ros/Info.h"
73 #include "rtabmap_ros/MapData.h"
74 #include "rtabmap_ros/MapGraph.h"
75 #include "rtabmap_ros/GetMap.h"
76 #include "rtabmap_ros/PublishMap.h"
77 
78 #include "rtabmap_ros/MsgConversion.h"
79 
80 using namespace rtabmap;
81 
82 namespace rtabmap_ros {
83 
84 CoreWrapper::CoreWrapper() :
85  CommonDataSubscriber(false),
86  paused_(false),
87  lastPose_(Transform::getIdentity()),
88  lastPoseIntermediate_(false),
89  latestNodeWasReached_(false),
90  frameId_("base_link"),
91  odomFrameId_(""),
92  mapFrameId_("map"),
93  groundTruthFrameId_(""), // e.g., "world"
94  groundTruthBaseFrameId_(""), // e.g., "base_link_gt"
95  configPath_(""),
96  databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
97  odomDefaultAngVariance_(1.0),
98  odomDefaultLinVariance_(1.0),
99  waitForTransform_(true),
100  waitForTransformDuration_(0.2), // 200 ms
101  useActionForGoal_(false),
102  useSavedMap_(true),
103  genScan_(false),
104  genScanMaxDepth_(4.0),
105  genScanMinDepth_(0.0),
106  scanCloudMaxPoints_(0),
107  mapToOdom_(rtabmap::Transform::getIdentity()),
108  transformThread_(0),
109  tfThreadRunning_(false),
110  SYNC_INIT(rgb),
111  SYNC_INIT(rgbOdom),
112  stereoToDepth_(false),
113  odomSensorSync_(false),
114  rate_(Parameters::defaultRtabmapDetectionRate()),
115  createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
116  maxMappingNodes_(Parameters::defaultGridGlobalMaxNodes()),
117  time_(ros::Time::now()),
118  previousStamp_(0),
119  mbClient_("move_base", true)
120 {
121  globalPose_.header.stamp = ros::Time(0);
122 }
123 
124 void CoreWrapper::onInit()
125 {
126  ros::NodeHandle & nh = getNodeHandle();
127  ros::NodeHandle & pnh = getPrivateNodeHandle();
128 
129  mapsManager_.init(nh, pnh, getName(), true);
130 
131  bool publishTf = true;
132  double tfDelay = 0.05; // 20 Hz
133  double tfTolerance = 0.1; // 100 ms
134 
135  pnh.param("config_path", configPath_, configPath_);
136  pnh.param("database_path", databasePath_, databasePath_);
137 
138  pnh.param("frame_id", frameId_, frameId_);
139  pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
140  pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
141  pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
142  pnh.param("ground_truth_base_frame_id", groundTruthBaseFrameId_, frameId_);
143  if(pnh.hasParam("depth_cameras") && !pnh.hasParam("depth_cameras"))
144  {
145  NODELET_ERROR("\"depth_cameras\" parameter doesn't exist "
146  "anymore! It is replaced by \"rgbd_cameras\" parameter "
147  "used when \"subscribe_rgbd\" is true");
148  }
149 
150  pnh.param("publish_tf", publishTf, publishTf);
151  pnh.param("tf_delay", tfDelay, tfDelay);
152  if(pnh.hasParam("tf_prefix"))
153  {
154  ROS_ERROR("tf_prefix parameter has been removed, use directly map_frame_id, odom_frame_id and frame_id parameters.");
155  }
156  pnh.param("tf_tolerance", tfTolerance, tfTolerance);
157  pnh.param("odom_tf_angular_variance", odomDefaultAngVariance_, odomDefaultAngVariance_);
158  pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_);
159  pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
160  pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
161  pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
162  pnh.param("use_saved_map", useSavedMap_, useSavedMap_);
163  pnh.param("gen_scan", genScan_, genScan_);
164  pnh.param("gen_scan_max_depth", genScanMaxDepth_, genScanMaxDepth_);
165  pnh.param("gen_scan_min_depth", genScanMinDepth_, genScanMinDepth_);
166  pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
167  if(pnh.hasParam("scan_cloud_normal_k"))
168  {
169  ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been removed. RTAB-Map's parameter \"%s\" should be used instead. "
170  "The value is copied. Use \"%s\" to avoid this warning.",
171  Parameters::kMemLaserScanNormalK().c_str(),
172  Parameters::kMemLaserScanNormalK().c_str());
173  double value;
174  pnh.getParam("scan_cloud_normal_k", value);
175  uInsert(parameters_, ParametersPair(Parameters::kMemLaserScanNormalK(), uNumber2Str(value)));
176  }
177  pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_);
178  pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
179  if(pnh.hasParam("flip_scan"))
180  {
181  NODELET_WARN("Parameter \"flip_scan\" doesn't exist anymore. Rtabmap now "
182  "detects automatically if the laser is upside down with /tf, then if so, it "
183  "switches scan values.");
184  }
185 
186  NODELET_INFO("rtabmap: frame_id = %s", frameId_.c_str());
187  if(!odomFrameId_.empty())
188  {
189  NODELET_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str());
190  }
191  if(!groundTruthFrameId_.empty())
192  {
193  NODELET_INFO("rtabmap: ground_truth_frame_id = %s -> ground_truth_base_frame_id = %s",
194  groundTruthFrameId_.c_str(),
195  groundTruthBaseFrameId_.c_str());
196  }
197  NODELET_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
198  NODELET_INFO("rtabmap: tf_delay = %f", tfDelay);
199  NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
200  NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false");
201  bool subscribeStereo = false;
202  pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
203  if(subscribeStereo)
204  {
205  NODELET_INFO("rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false");
206  }
207 
208  infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
209  mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
210  mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>("mapGraph", 1);
211  labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
212  mapPathPub_ = nh.advertise<nav_msgs::Path>("mapPath", 1);
213  localizationPosePub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("localization_pose", 1);
214  initialPoseSub_ = nh.subscribe("initialpose", 1, &CoreWrapper::initialPoseCallback, this);
215 
216  // planning topics
217  goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
218  goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
219  nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_out", 1);
220  goalReachedPub_ = nh.advertise<std_msgs::Bool>("goal_reached", 1);
221  globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
222  localPathPub_ = nh.advertise<nav_msgs::Path>("local_path", 1);
223 
224  ros::Publisher nextMetricGoal_;
225  ros::Publisher goalReached_;
226  ros::Publisher path_;
227 
228  configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
229  databasePath_ = uReplaceChar(databasePath_, '~', UDirectory::homeDir());
230  if(configPath_.size() && configPath_.at(0) != '/')
231  {
232  configPath_ = UDirectory::currentDir(true) + configPath_;
233  }
234  if(databasePath_.size() && databasePath_.at(0) != '/')
235  {
236  databasePath_ = UDirectory::currentDir(true) + databasePath_;
237  }
238 
239  ParametersMap allParameters = Parameters::getDefaultParameters();
240  uInsert(allParameters, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS
241  uInsert(allParameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), UDirectory::homeDir()+"/.ros")); // change default to ~/.ros
242 
243  // load parameters
244  loadParameters(configPath_, parameters_);
245 
246  // update parameters with user input parameters (private)
247  for(ParametersMap::iterator iter=allParameters.begin(); iter!=allParameters.end(); ++iter)
248  {
249  std::string vStr;
250  bool vBool;
251  int vInt;
252  double vDouble;
253  if(pnh.getParam(iter->first, vStr))
254  {
255  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
256 
257  if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
258  {
259  vStr = uReplaceChar(vStr, '~', UDirectory::homeDir());
260  }
261  else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0)
262  {
263  vStr = uReplaceChar(vStr, '~', UDirectory::homeDir());
264  }
265  uInsert(parameters_, ParametersPair(iter->first, vStr));
266  }
267  else if(pnh.getParam(iter->first, vBool))
268  {
269  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
270  uInsert(parameters_, ParametersPair(iter->first, uBool2Str(vBool)));
271  }
272  else if(pnh.getParam(iter->first, vDouble))
273  {
274  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
275  uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vDouble)));
276  }
277  else if(pnh.getParam(iter->first, vInt))
278  {
279  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
280  uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vInt)));
281  }
282  }
283 
284  //parse input arguments
285  std::vector<std::string> argList = getMyArgv();
286  char * argv[argList.size()];
287  bool deleteDbOnStart = false;
288  for(unsigned int i=0; i<argList.size(); ++i)
289  {
290  argv[i] = &argList[i].at(0);
291  if(strcmp(argv[i], "--delete_db_on_start") == 0 || strcmp(argv[i], "-d") == 0)
292  {
293  deleteDbOnStart = true;
294  }
295  }
296  rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argList.size(), argv);
297  for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
298  {
299  uInsert(parameters_, ParametersPair(iter->first, iter->second));
300  NODELET_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
301  }
302 
303  // Backward compatibility
304  for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
305  iter!=Parameters::getRemovedParameters().end();
306  ++iter)
307  {
308  std::string vStr;
309  bool vBool;
310  int vInt;
311  double vDouble;
312  std::string paramValue;
313  if(pnh.getParam(iter->first, vStr))
314  {
315  paramValue = vStr;
316  }
317  else if(pnh.getParam(iter->first, vBool))
318  {
319  paramValue = uBool2Str(vBool);
320  }
321  else if(pnh.getParam(iter->first, vDouble))
322  {
323  paramValue = uNumber2Str(vDouble);
324  }
325  else if(pnh.getParam(iter->first, vInt))
326  {
327  paramValue = uNumber2Str(vInt);
328  }
329  if(!paramValue.empty())
330  {
331  if(iter->second.first)
332  {
333  // can be migrated
334  uInsert(parameters_, ParametersPair(iter->second.second, paramValue));
335  NODELET_WARN("Rtabmap: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
336  iter->first.c_str(), iter->second.second.c_str(), paramValue.c_str());
337  }
338  else
339  {
340  if(iter->second.second.empty())
341  {
342  NODELET_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore!",
343  iter->first.c_str());
344  }
345  else
346  {
347  NODELET_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
348  iter->first.c_str(), iter->second.second.c_str());
349  }
350  }
351  }
352  }
353 
354  // Backward compatibility (MapsManager)
355  mapsManager_.backwardCompatibilityParameters(pnh, parameters_);
356 
357  bool subscribeScan2d = false;
358  bool subscribeScan3d = false;
359  pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d);
360  pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
361  if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridFromDepth()) == parameters_.end())
362  {
363  NODELET_WARN("Setting \"%s\" parameter to false (default true) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is "
364  "true. The occupancy grid map will be constructed from "
365  "laser scans. To get occupancy grid map from cloud projection, set \"%s\" "
366  "to true. To suppress this warning, "
367  "add <param name=\"%s\" type=\"string\" value=\"false\"/>",
368  Parameters::kGridFromDepth().c_str(),
369  Parameters::kGridFromDepth().c_str(),
370  Parameters::kGridFromDepth().c_str());
371  parameters_.insert(ParametersPair(Parameters::kGridFromDepth(), "false"));
372  }
373  if((subscribeScan2d || subscribeScan3d) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end())
374  {
375  NODELET_INFO("Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\" or \"subscribe_scan_cloud\" is true.",
376  Parameters::kGridRangeMax().c_str(),
377  Parameters::defaultGridRangeMax());
378  parameters_.insert(ParametersPair(Parameters::kGridRangeMax(), "0"));
379  }
380  int regStrategy = Parameters::defaultRegStrategy();
381  Parameters::parse(parameters_, Parameters::kRegStrategy(), regStrategy);
382  if(subscribeScan2d &&
383  parameters_.find(Parameters::kRGBDProximityPathMaxNeighbors()) == parameters_.end() &&
384  (regStrategy == Registration::kTypeIcp || regStrategy == Registration::kTypeVisIcp))
385  {
386  NODELET_WARN("Setting \"%s\" parameter to 10 (default 0) as \"subscribe_scan\" is "
387  "true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
388  "scans. To disable, set \"%s\" to 0. To suppress this warning, "
389  "add <param name=\"%s\" type=\"string\" value=\"10\"/>",
390  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
391  Parameters::kRegStrategy().c_str(),
392  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
393  Parameters::kRGBDProximityPathMaxNeighbors().c_str());
394  parameters_.insert(ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "10"));
395  }
396 
397  int estimationType = Parameters::defaultVisEstimationType();
398  Parameters::parse(parameters_, Parameters::kVisEstimationType(), estimationType);
399  int cameras = 0;
400  bool subscribeRGBD = false;
401  pnh.param("rgbd_cameras", cameras, cameras);
402  pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
403  if(subscribeRGBD && cameras> 1 && estimationType>0)
404  {
405  NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported "
406  "for multi-cameras) as \"subscribe_rgbd\" is "
407  "true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
408  Parameters::kVisEstimationType().c_str(),
409  estimationType,
410  Parameters::kVisEstimationType().c_str());
411  uInsert(parameters_, ParametersPair(Parameters::kVisEstimationType(), "0"));
412  }
413 
414  // modify default parameters with those in the database
415  if(!deleteDbOnStart)
416  {
417  ParametersMap dbParameters;
418  rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
419  if(driver->openConnection(databasePath_))
420  {
421  dbParameters = driver->getLastParameters(); // parameter migration is already done
422  }
423  delete driver;
424  for(ParametersMap::iterator iter=dbParameters.begin(); iter!=dbParameters.end(); ++iter)
425  {
426  if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
427  {
428  // ignore working directory
429  continue;
430  }
431  if(parameters_.find(iter->first) == parameters_.end() &&
432  allParameters.find(iter->first) != allParameters.end() &&
433  allParameters.find(iter->first)->second.compare(iter->second) !=0)
434  {
435  NODELET_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from database", iter->first.c_str(), iter->second.c_str());
436  parameters_.insert(*iter);
437  }
438  }
439  }
440 
441  // Add all other parameters (not copied if already exists)
442  parameters_.insert(allParameters.begin(), allParameters.end());
443 
444  // set public parameters
445  nh.setParam("is_rtabmap_paused", paused_);
446  for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
447  {
448  nh.setParam(iter->first, iter->second);
449  }
450  if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end())
451  {
452  Parameters::parse(parameters_, Parameters::kRtabmapDetectionRate(), rate_);
453  NODELET_INFO("RTAB-Map detection rate = %f Hz", rate_);
454  }
455  if(parameters_.find(Parameters::kRtabmapCreateIntermediateNodes()) != parameters_.end())
456  {
457  Parameters::parse(parameters_, Parameters::kRtabmapCreateIntermediateNodes(), createIntermediateNodes_);
458  if(createIntermediateNodes_)
459  {
460  NODELET_INFO("Create intermediate nodes");
461  }
462  }
463  if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
464  {
465  Parameters::parse(parameters_, Parameters::kGridGlobalMaxNodes(), maxMappingNodes_);
466  if(maxMappingNodes_>0)
467  {
468  NODELET_INFO("Max mapping nodes = %d", maxMappingNodes_);
469  }
470  }
471 
472  if(paused_)
473  {
474  NODELET_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
475  }
476 
477  if(deleteDbOnStart)
478  {
479  if(UFile::erase(databasePath_) == 0)
480  {
481  NODELET_INFO("rtabmap: Deleted database \"%s\" (--delete_db_on_start or -d are set).", databasePath_.c_str());
482  }
483  }
484 
485  if(databasePath_.size())
486  {
487  NODELET_INFO("rtabmap: Using database from \"%s\" (%ld MB).", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
488  }
489  else
490  {
491  NODELET_INFO("rtabmap: database_path parameter not set, the map will not be saved.");
492  }
493 
494  mapsManager_.setParameters(parameters_);
495 
496  // Init RTAB-Map
497  rtabmap_.init(parameters_, databasePath_);
498 
499  if(rtabmap_.getMemory() && useSavedMap_)
500  {
501  float xMin, yMin, gridCellSize;
502  cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
503  if(!map.empty())
504  {
505  NODELET_INFO("rtabmap: 2D occupancy grid map loaded (%dx%d).", map.cols, map.rows);
506  mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
507  }
508  }
509 
510  if(databasePath_.size() && rtabmap_.getMemory())
511  {
512  NODELET_INFO("rtabmap: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str());
513  }
514 
515  // setup services
516  updateSrv_ = nh.advertiseService("update_parameters", &CoreWrapper::updateRtabmapCallback, this);
517  resetSrv_ = nh.advertiseService("reset", &CoreWrapper::resetRtabmapCallback, this);
518  pauseSrv_ = nh.advertiseService("pause", &CoreWrapper::pauseRtabmapCallback, this);
519  resumeSrv_ = nh.advertiseService("resume", &CoreWrapper::resumeRtabmapCallback, this);
520  triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
521  backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this);
522  setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
523  setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
524  getMapDataSrv_ = nh.advertiseService("get_map_data", &CoreWrapper::getMapDataCallback, this);
525  getMapSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
526  getProbMapSrv_ = nh.advertiseService("get_prob_map", &CoreWrapper::getProbMapCallback, this);
527  getGridMapSrv_ = nh.advertiseService("get_grid_map", &CoreWrapper::getGridMapCallback, this);
528  getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
529  publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
530  getPlanSrv_ = nh.advertiseService("get_plan", &CoreWrapper::getPlanCallback, this);
531  setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
532  cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
533  setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
534  listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
535 #ifdef WITH_OCTOMAP_MSGS
536 #ifdef RTABMAP_OCTOMAP
537  octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
538  octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
539 #endif
540 #endif
541  //private services
542  setLogDebugSrv_ = pnh.advertiseService("log_debug", &CoreWrapper::setLogDebug, this);
543  setLogInfoSrv_ = pnh.advertiseService("log_info", &CoreWrapper::setLogInfo, this);
544  setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this);
545  setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this);
546 
547  int optimizeIterations = 0;
548  Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
549  if(publishTf && optimizeIterations != 0)
550  {
551  tfThreadRunning_ = true;
552  transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay, tfTolerance));
553  }
554  else if(publishTf)
555  {
556  UWARN("Graph optimization is disabled (%s=0), the tf between frame \"%s\" and odometry frame will not be published. You can safely ignore this warning if you are using map_optimizer node.",
557  Parameters::kOptimizerIterations().c_str(), mapFrameId_.c_str());
558  }
559 
560  setupCallbacks(nh, pnh, getName()); // do it at the end
561  if(!this->isDataSubscribed())
562  {
563  bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
564  bool incremental = uStr2Bool(parameters_.at(Parameters::kMemIncrementalMemory()).c_str());
565  if(isRGBD && !incremental)
566  {
567  NODELET_INFO("\"%s\" is true and \"%s\" is false, subscribing to RGB + camera info...",
568  Parameters::kRGBDEnabled().c_str(),
569  Parameters::kMemIncrementalMemory().c_str());
570  ros::NodeHandle rgb_nh(nh, "rgb");
571  ros::NodeHandle rgb_pnh(pnh, "rgb");
572  image_transport::ImageTransport rgb_it(rgb_nh);
573  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
574  rgbSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
575  rgbCameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
576 
577  std::string odomFrameId;
578  pnh.getParam("odom_frame_id", odomFrameId);
579  if(!odomFrameId.empty())
580  {
581  // odom from TF
582  if(isApproxSync())
583  {
584  rgbApproximateSync_ = new message_filters::Synchronizer<rgbApproximateSyncPolicy>(
585  rgbApproximateSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_);
586  rgbApproximateSync_->registerCallback(boost::bind(&CoreWrapper::rgbCallback, this, _1, _2));
587  }
588  else
589  {
590  rgbExactSync_ = new message_filters::Synchronizer<rgbExactSyncPolicy>(
591  rgbExactSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_);
592  rgbExactSync_->registerCallback(boost::bind(&CoreWrapper::rgbCallback, this, _1, _2));
593  }
594  NODELET_INFO("\n%s subscribed to:\n %s\n %s",
595  getName().c_str(),
596  rgbSub_.getTopic().c_str(),
597  rgbCameraInfoSub_.getTopic().c_str());
598  }
599  else
600  {
601  rgbOdomSub_.subscribe(nh, "odom", 1);
602  if(isApproxSync())
603  {
604  rgbOdomApproximateSync_ = new message_filters::Synchronizer<rgbOdomApproximateSyncPolicy>(
605  rgbOdomApproximateSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_, rgbOdomSub_);
606  rgbOdomApproximateSync_->registerCallback(boost::bind(&CoreWrapper::rgbOdomCallback, this, _1, _2, _3));
607  }
608  else
609  {
610  rgbOdomExactSync_ = new message_filters::Synchronizer<rgbOdomExactSyncPolicy>(
611  rgbOdomExactSyncPolicy(getQueueSize()), rgbSub_, rgbCameraInfoSub_, rgbOdomSub_);
612  rgbOdomExactSync_->registerCallback(boost::bind(&CoreWrapper::rgbOdomCallback, this, _1, _2, _3));
613  }
614  NODELET_INFO("\n%s subscribed to:\n %s\n %s\n %s",
615  getName().c_str(),
616  rgbSub_.getTopic().c_str(),
617  rgbCameraInfoSub_.getTopic().c_str(),
618  rgbOdomSub_.getTopic().c_str());
619  }
620  }
621  else
622  {
623  if(isRGBD)
624  {
625  NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
626  "parameter \"%s\" and \"%s\" are true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
627  "to true to use rtabmap node for RGB-D SLAM, set \"%s\" to false for loop closure "
628  "detection on images-only or set \"%s\" to false to localize a single RGB camera against pre-built 3D map.",
629  Parameters::kRGBDEnabled().c_str(),
630  Parameters::kMemIncrementalMemory().c_str(),
631  Parameters::kRGBDEnabled().c_str(),
632  Parameters::kMemIncrementalMemory().c_str());
633  }
634  ros::NodeHandle rgb_nh(nh, "rgb");
635  ros::NodeHandle rgb_pnh(pnh, "rgb");
636  image_transport::ImageTransport rgb_it(rgb_nh);
637  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
638  defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
639 
640  NODELET_INFO("\n%s subscribed to:\n %s", getName().c_str(), defaultSub_.getTopic().c_str());
641  }
642  }
643 
644  userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
645  globalPoseAsyncSub_ = nh.subscribe("global_pose", 1, &CoreWrapper::globalPoseAsyncCallback, this);
646 }
647 
648 CoreWrapper::~CoreWrapper()
649 {
650  if(transformThread_)
651  {
652  tfThreadRunning_ = false;
653  transformThread_->join();
654  delete transformThread_;
655  }
656 
657  SYNC_DEL(rgb);
658  SYNC_DEL(rgbOdom);
659 
660  this->saveParameters(configPath_);
661 
662  ros::NodeHandle nh;
663  for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
664  {
665  nh.deleteParam(iter->first);
666  }
667  nh.deleteParam("is_rtabmap_paused");
668 
669  printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
670  if(rtabmap_.getMemory())
671  {
672  // save the grid map
673  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
674  cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
675  if(!pixels.empty())
676  {
677  printf("rtabmap: 2D occupancy grid map saved.\n");
678  rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
679  }
680  }
681 
682  rtabmap_.close();
683  printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
684 }
685 
686 void CoreWrapper::loadParameters(const std::string & configFile, ParametersMap & parameters)
687 {
688  if(!configFile.empty())
689  {
690  NODELET_INFO("Loading parameters from %s", configFile.c_str());
691  if(!UFile::exists(configFile.c_str()))
692  {
693  NODELET_WARN("Config file doesn't exist! It will be generated...");
694  }
695  Parameters::readINI(configFile.c_str(), parameters);
696  }
697 }
698 
699 void CoreWrapper::saveParameters(const std::string & configFile)
700 {
701  if(!configFile.empty())
702  {
703  printf("Saving parameters to %s\n", configFile.c_str());
704 
705  if(!UFile::exists(configFile.c_str()))
706  {
707  printf("Config file doesn't exist, a new one will be created.\n");
708  }
709  Parameters::writeINI(configFile.c_str(), parameters_);
710  }
711  else
712  {
713  NODELET_INFO("Parameters are not saved! (No configuration file provided...)");
714  }
715 }
716 
717 void CoreWrapper::publishLoop(double tfDelay, double tfTolerance)
718 {
719  if(tfDelay == 0)
720  return;
721  ros::Rate r(1.0 / tfDelay);
722  while(tfThreadRunning_)
723  {
724  if(!odomFrameId_.empty())
725  {
726  mapToOdomMutex_.lock();
727  ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfTolerance);
728  geometry_msgs::TransformStamped msg;
729  msg.child_frame_id = odomFrameId_;
730  msg.header.frame_id = mapFrameId_;
731  msg.header.stamp = tfExpiration;
732  rtabmap_ros::transformToGeometryMsg(mapToOdom_, msg.transform);
733  tfBroadcaster_.sendTransform(msg);
734  mapToOdomMutex_.unlock();
735  }
736  r.sleep();
737  }
738 }
739 
740 void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
741 {
742  if(!paused_)
743  {
744  if(rate_>0.0f)
745  {
746  if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
747  {
748  return;
749  }
750  }
751  time_ = ros::Time::now();
752 
753  if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
754  imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
755  imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
756  imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
757  {
758  NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
759  return;
760  }
761 
762  cv_bridge::CvImageConstPtr ptrImage;
763  if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
764  imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
765  {
766  ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
767  }
768  else
769  {
770  ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
771  }
772 
773  // process data
774  UTimer timer;
775  if(rtabmap_.isIDsGenerated() || ptrImage->header.seq > 0)
776  {
777  if(!rtabmap_.process(ptrImage->image.clone(), ptrImage->header.seq))
778  {
779  NODELET_WARN("RTAB-Map could not process the data received! (ROS id = %d)", ptrImage->header.seq);
780  }
781  else
782  {
783  this->publishStats(ros::Time::now());
784  }
785  }
786  else if(!rtabmap_.isIDsGenerated())
787  {
788  NODELET_WARN("Ignoring received image because its sequence ID=0. Please "
789  "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. "
790  "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and "
791  "when you need to have IDs output of RTAB-map synchronised with the source "
792  "image sequence ID.");
793  }
794  NODELET_INFO("rtabmap: Update rate=%fs, Limit=%fs, Processing time = %fs (%d local nodes)",
795  1.0f/rate_,
796  rtabmap_.getTimeThreshold()/1000.0f,
797  timer.ticks(),
798  rtabmap_.getWMSize()+rtabmap_.getSTMSize());
799  }
800 }
801 
802 
803 void CoreWrapper::rgbCallback(
804  const sensor_msgs::ImageConstPtr& imageMsg,
805  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
806 {
807  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
808  nav_msgs::OdometryConstPtr odomMsg; // Null
809  sensor_msgs::LaserScanConstPtr scanMsg; // Null
810  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
811  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
812  cv_bridge::CvImageConstPtr depthMsg;// Null
813  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
814 }
815 
816 void CoreWrapper::rgbOdomCallback(
817  const sensor_msgs::ImageConstPtr& imageMsg,
818  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
819  const nav_msgs::OdometryConstPtr & odomMsg)
820 {
821  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
822  sensor_msgs::LaserScanConstPtr scanMsg; // Null
823  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
824  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
825  cv_bridge::CvImageConstPtr depthMsg;// Null
826  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
827 }
828 
829 bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Time stamp)
830 {
831  if(!paused_)
832  {
833  Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
834  if(!odom.isNull())
835  {
836  Transform odomTF = rtabmap_ros::getTransform(odomMsg->header.frame_id, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
837  if(odomTF.isNull())
838  {
839  static bool shown = false;
840  if(!shown)
841  {
842  NODELET_WARN("We received odometry message, but we cannot get the "
843  "corresponding TF %s->%s at data stamp %fs (odom msg stamp is %fs). Make sure TF of odometry is "
844  "also published to get more accurate pose estimation. This "
845  "warning is only printed once.", odomMsg->header.frame_id.c_str(), frameId_.c_str(), stamp.toSec(), odomMsg->header.stamp.toSec());
846  shown = true;
847  }
848  stamp = odomMsg->header.stamp;
849  }
850  else
851  {
852  odom = odomTF;
853  }
854  }
855 
856  if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || (odomMsg->pose.covariance[0] >= BAD_COVARIANCE && odomMsg->twist.covariance[0] >= BAD_COVARIANCE)))
857  {
858  UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0]));
859  rtabmap_.triggerNewMap();
860  covariance_ = cv::Mat();
861  }
862 
863  lastPoseIntermediate_ = false;
864  lastPose_ = odom;
865  lastPoseStamp_ = stamp;
866 
867  // Only update variance if odom is not null
868  if(!odom.isNull())
869  {
870  cv::Mat covariance;
871  double variance = odomMsg->twist.covariance[0];
872  if(variance == BAD_COVARIANCE || variance <= 0.0f)
873  {
874  //use the one of the pose
875  covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->pose.covariance.data()).clone();
876  covariance /= 2.0;
877  }
878  else
879  {
880  covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->twist.covariance.data()).clone();
881  }
882 
883  if(uIsFinite(covariance.at<double>(0,0)) &&
884  covariance.at<double>(0,0) != 1.0 &&
885  covariance.at<double>(0,0)>0.0)
886  {
887  // Use largest covariance error (to be independent of the odometry frame rate)
888  if(covariance_.empty() || covariance.at<double>(0,0) > covariance_.at<double>(0,0))
889  {
890  covariance_ = covariance;
891  }
892  }
893  }
894 
895  // Throttle
896  bool ignoreFrame = false;
897  if(rate_>0.0f)
898  {
899  if((previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_)) ||
900  ((previousStamp_.toSec() <= 0.0 || stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_)))
901  {
902  ignoreFrame = true;
903  }
904  }
905  if(ignoreFrame)
906  {
907  if(createIntermediateNodes_)
908  {
909  lastPoseIntermediate_ = true;
910  }
911  else
912  {
913  return false;
914  }
915  }
916  else if(!ignoreFrame)
917  {
918  time_ = ros::Time::now();
919  previousStamp_ = stamp;
920  }
921 
922  return true;
923  }
924  return false;
925 }
926 
927 bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
928 {
929  if(!paused_)
930  {
931  // Odom TF ready?
932  Transform odom = rtabmap_ros::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
933  if(odom.isNull())
934  {
935  return false;
936  }
937 
938  if(!lastPose_.isIdentity() && odom.isIdentity())
939  {
940  UWARN("Odometry is reset (identity pose detected). Increment map id!");
941  rtabmap_.triggerNewMap();
942  covariance_ = cv::Mat();
943  }
944 
945  lastPoseIntermediate_ = false;
946  lastPose_ = odom;
947  lastPoseStamp_ = stamp;
948 
949  bool ignoreFrame = false;
950  if(rate_>0.0f)
951  {
952  if((previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_)) ||
953  ((previousStamp_.toSec() <= 0.0 || stamp.toSec() <= previousStamp_.toSec()) && ros::Time::now() - time_ < ros::Duration(1.0f/rate_)))
954  {
955  ignoreFrame = true;
956  }
957  }
958  if(ignoreFrame)
959  {
960  if(createIntermediateNodes_)
961  {
962  lastPoseIntermediate_ = true;
963  }
964  else
965  {
966  return false;
967  }
968  }
969  else if(!ignoreFrame)
970  {
971  time_ = ros::Time::now();
972  previousStamp_ = stamp;
973  }
974 
975  return true;
976  }
977  return false;
978 }
979 
980 void CoreWrapper::commonDepthCallback(
981  const nav_msgs::OdometryConstPtr & odomMsg,
982  const rtabmap_ros::UserDataConstPtr & userDataMsg,
983  const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
984  const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
985  const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
986  const sensor_msgs::LaserScanConstPtr& scan2dMsg,
987  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
988  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
989 {
990  std::string odomFrameId = odomFrameId_;
991  if(odomMsg.get())
992  {
993  odomFrameId = odomMsg->header.frame_id;
994  if(scan2dMsg.get())
995  {
996  if(!odomUpdate(odomMsg, scan2dMsg->header.stamp))
997  {
998  return;
999  }
1000  }
1001  else if(scan3dMsg.get())
1002  {
1003  if(!odomUpdate(odomMsg, scan3dMsg->header.stamp))
1004  {
1005  return;
1006  }
1007  }
1008  else if(imageMsgs.size() == 0 || imageMsgs[0].get() == 0 || !odomUpdate(odomMsg, imageMsgs[0]->header.stamp))
1009  {
1010  return;
1011  }
1012  }
1013  else if(scan2dMsg.get())
1014  {
1015  if(!odomTFUpdate(scan2dMsg->header.stamp))
1016  {
1017  return;
1018  }
1019  }
1020  else if(scan3dMsg.get())
1021  {
1022  if(!odomTFUpdate(scan3dMsg->header.stamp))
1023  {
1024  return;
1025  }
1026  }
1027  else if(imageMsgs.size() == 0 || imageMsgs[0].get() == 0 || !odomTFUpdate(imageMsgs[0]->header.stamp))
1028  {
1029  return;
1030  }
1031 
1032  commonDepthCallbackImpl(odomFrameId,
1033  userDataMsg,
1034  imageMsgs,
1035  depthMsgs,
1036  cameraInfoMsgs,
1037  scan2dMsg,
1038  scan3dMsg,
1039  odomInfoMsg);
1040 }
1041 
1042 void CoreWrapper::commonDepthCallbackImpl(
1043  const std::string & odomFrameId,
1044  const rtabmap_ros::UserDataConstPtr & userDataMsg,
1045  const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
1046  const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
1047  const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
1048  const sensor_msgs::LaserScanConstPtr& scan2dMsg,
1049  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
1050  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
1051 {
1052  cv::Mat rgb;
1053  cv::Mat depth;
1054  std::vector<rtabmap::CameraModel> cameraModels;
1055  if(!rtabmap_ros::convertRGBDMsgs(
1056  imageMsgs,
1057  depthMsgs,
1058  cameraInfoMsgs,
1059  frameId_,
1060  odomSensorSync_?odomFrameId:"",
1061  lastPoseStamp_,
1062  rgb,
1063  depth,
1064  cameraModels,
1065  tfListener_,
1066  waitForTransform_?waitForTransformDuration_:0.0))
1067  {
1068  NODELET_ERROR("Could not convert rgb/depth msgs! Aborting rtabmap update...");
1069  return;
1070  }
1071 
1072  UASSERT(uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format()));
1073  if(!depth.empty() && depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
1074  {
1075  depth = rtabmap::util2d::cvtDepthFromFloat(depth);
1076  static bool shown = false;
1077  if(!shown)
1078  {
1079  NODELET_WARN("Save depth data to 16 bits format: depth type detected is "
1080  "32FC1, use 16UC1 depth format to avoid this conversion "
1081  "(or set parameter \"Mem/SaveDepth16Format=false\" to use "
1082  "32bits format). This message is only printed once...");
1083  shown = true;
1084  }
1085  }
1086 
1087  cv::Mat scan;
1088  Transform scanLocalTransform = Transform::getIdentity();
1089  bool genMaxScanPts = 0;
1090  if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && !depth.empty() && genScan_)
1091  {
1092  pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud2d(new pcl::PointCloud<pcl::PointXYZ>);
1093  *scanCloud2d = util3d::laserScanFromDepthImages(
1094  depth,
1095  cameraModels,
1096  genScanMaxDepth_,
1097  genScanMinDepth_);
1098  genMaxScanPts += depth.cols;
1099  scan = rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d);
1100  }
1101  else if(scan2dMsg.get() != 0)
1102  {
1103  if(!rtabmap_ros::convertScanMsg(
1104  scan2dMsg,
1105  frameId_,
1106  odomSensorSync_?odomFrameId:"",
1107  lastPoseStamp_,
1108  scan,
1109  scanLocalTransform,
1110  tfListener_,
1111  waitForTransform_?waitForTransformDuration_:0))
1112  {
1113  NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
1114  return;
1115  }
1116  Transform zAxis(0,0,1,0,0,0);
1117  if((scanLocalTransform.rotation()*zAxis).z() < 0)
1118  {
1119  cv::Mat flipScan;
1120  cv::flip(scan, flipScan, 1);
1121  scan = flipScan;
1122  }
1123  if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
1124  {
1125  // backward compatibility, project 2D scan in /base_link frame
1126  scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
1127  scanLocalTransform = Transform::getIdentity();
1128  }
1129  }
1130  else if(scan3dMsg.get() != 0)
1131  {
1132  if(!rtabmap_ros::convertScan3dMsg(
1133  scan3dMsg,
1134  frameId_,
1135  odomSensorSync_?odomFrameId:"",
1136  lastPoseStamp_,
1137  scan,
1138  scanLocalTransform,
1139  tfListener_,
1140  waitForTransform_?waitForTransformDuration_:0))
1141  {
1142  NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
1143  return;
1144  }
1145  }
1146 
1147  Transform groundTruthPose;
1148  if(!groundTruthFrameId_.empty())
1149  {
1150  groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
1151  }
1152 
1153  cv::Mat userData;
1154  if(userDataMsg.get())
1155  {
1156  userData = rtabmap_ros::userDataFromROS(*userDataMsg);
1157  UScopeMutex lock(userDataMutex_);
1158  if(!userData_.empty())
1159  {
1160  NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
1161  userData_ = cv::Mat();
1162  }
1163  }
1164  else
1165  {
1166  UScopeMutex lock(userDataMutex_);
1167  userData = userData_;
1168  userData_ = cv::Mat();
1169  }
1170 
1171  SensorData data(LaserScan::backwardCompatibility(scan,
1172  scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
1173  scan2dMsg.get() != 0?scan2dMsg->range_max:(genScan_?genScanMaxDepth_:0.0f),
1174  scanLocalTransform),
1175  rgb,
1176  depth,
1177  cameraModels,
1178  lastPoseIntermediate_?-1:imageMsgs[0]->header.seq,
1179  rtabmap_ros::timestampFromROS(lastPoseStamp_),
1180  userData);
1181  data.setGroundTruth(groundTruthPose);
1182 
1183  //global pose
1184  if(!globalPose_.header.stamp.isZero())
1185  {
1186  // assume sensor is fixed
1187  Transform sensorToBase = rtabmap_ros::getTransform(
1188  globalPose_.header.frame_id,
1189  frameId_,
1190  lastPoseStamp_,
1191  tfListener_,
1192  waitForTransform_?waitForTransformDuration_:0.0);
1193  if(!sensorToBase.isNull())
1194  {
1195  Transform globalPose = rtabmap_ros::transformFromPoseMsg(globalPose_.pose.pose);
1196  globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
1197 
1198  // Correction of the global pose accounting the odometry movement since we received it
1199  Transform correction = rtabmap_ros::getTransform(
1200  frameId_,
1201  odomFrameId,
1202  globalPose_.header.stamp,
1203  lastPoseStamp_,
1204  tfListener_,
1205  waitForTransform_?waitForTransformDuration_:0.0);
1206  if(!correction.isNull())
1207  {
1208  globalPose *= correction;
1209  }
1210  else
1211  {
1212  NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
1213  "If odometry is small since it received the global pose and "
1214  "covariance is large, this should not be a problem.");
1215  }
1216  cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
1217  data.setGlobalPose(globalPose, globalPoseCovariance);
1218  }
1219  }
1220  globalPose_.header.stamp = ros::Time(0);
1221 
1222  OdometryInfo odomInfo;
1223  if(odomInfoMsg.get())
1224  {
1225  odomInfo = odomInfoFromROS(*odomInfoMsg);
1226  }
1227 
1228  process(lastPoseStamp_,
1229  data,
1230  lastPose_,
1231  odomFrameId,
1232  covariance_,
1233  odomInfo);
1234  covariance_ = cv::Mat();
1235 }
1236 
1237 void CoreWrapper::commonStereoCallback(
1238  const nav_msgs::OdometryConstPtr & odomMsg,
1239  const rtabmap_ros::UserDataConstPtr & userDataMsg,
1240  const cv_bridge::CvImageConstPtr& leftImageMsg,
1241  const cv_bridge::CvImageConstPtr& rightImageMsg,
1242  const sensor_msgs::CameraInfo& leftCamInfoMsg,
1243  const sensor_msgs::CameraInfo& rightCamInfoMsg,
1244  const sensor_msgs::LaserScanConstPtr& scan2dMsg,
1245  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
1246  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
1247 {
1248  std::string odomFrameId = odomFrameId_;
1249  if(odomMsg.get())
1250  {
1251  odomFrameId = odomMsg->header.frame_id;
1252  if(scan2dMsg.get())
1253  {
1254  if(!odomUpdate(odomMsg, scan2dMsg->header.stamp))
1255  {
1256  return;
1257  }
1258  }
1259  else if(scan3dMsg.get())
1260  {
1261  if(!odomUpdate(odomMsg, scan3dMsg->header.stamp))
1262  {
1263  return;
1264  }
1265  }
1266  else if(leftImageMsg.get() == 0 || !odomUpdate(odomMsg, leftImageMsg->header.stamp))
1267  {
1268  return;
1269  }
1270  }
1271  else if(scan2dMsg.get())
1272  {
1273  if(!odomTFUpdate(scan2dMsg->header.stamp))
1274  {
1275  return;
1276  }
1277  }
1278  else if(scan3dMsg.get())
1279  {
1280  if(!odomTFUpdate(scan3dMsg->header.stamp))
1281  {
1282  return;
1283  }
1284  }
1285  else if(leftImageMsg.get() == 0 || !odomTFUpdate(leftImageMsg->header.stamp))
1286  {
1287  return;
1288  }
1289 
1290  cv::Mat left;
1291  cv::Mat right;
1292  StereoCameraModel stereoModel;
1293  if(!rtabmap_ros::convertStereoMsg(
1294  leftImageMsg,
1295  rightImageMsg,
1296  leftCamInfoMsg,
1297  rightCamInfoMsg,
1298  frameId_,
1299  odomSensorSync_?odomFrameId:"",
1300  lastPoseStamp_,
1301  left,
1302  right,
1303  stereoModel,
1304  tfListener_,
1305  waitForTransform_?waitForTransformDuration_:0.0))
1306  {
1307  NODELET_ERROR("Could not convert stereo msgs! Aborting rtabmap update...");
1308  return;
1309  }
1310 
1311  if(stereoToDepth_)
1312  {
1313  // cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters
1314  cv::Mat disparity = rtabmap::util2d::disparityFromStereoImages(
1315  left,
1316  right,
1317  parameters_);
1318  if(disparity.empty())
1319  {
1320  NODELET_ERROR("Could not compute disparity image (\"stereo_to_depth\" is true)!");
1321  return;
1322  }
1323  cv::Mat depth = rtabmap::util2d::depthFromDisparity(
1324  disparity,
1325  stereoModel.left().fx(),
1326  stereoModel.baseline());
1327 
1328  if(depth.empty())
1329  {
1330  NODELET_ERROR("Could not compute depth image (\"stereo_to_depth\" is true)!");
1331  return;
1332  }
1333  UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
1334 
1335  // move to common depth callback
1336  cv_bridge::CvImagePtr imgDepth(new cv_bridge::CvImage);
1337  if(depth.type() == CV_16UC1)
1338  {
1339  imgDepth->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
1340  }
1341  else // CV_32FC1
1342  {
1343  imgDepth->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
1344  }
1345  imgDepth->image = depth;
1346  imgDepth->header = leftImageMsg->header;
1347  std::vector<cv_bridge::CvImageConstPtr> rgbImages(1);
1348  std::vector<cv_bridge::CvImageConstPtr> depthImages(1);
1349  std::vector<sensor_msgs::CameraInfo> cameraInfos(1);
1350  rgbImages[0] = leftImageMsg;
1351  depthImages[0] = imgDepth;
1352  cameraInfos[0] = leftCamInfoMsg;
1353 
1354  commonDepthCallbackImpl(odomFrameId, rtabmap_ros::UserDataConstPtr(), rgbImages, depthImages, cameraInfos, scan2dMsg, scan3dMsg, odomInfoMsg);
1355  return;
1356  }
1357 
1358  cv::Mat scan;
1359  Transform scanLocalTransform = Transform::getIdentity();
1360  if(scan2dMsg.get() != 0)
1361  {
1362  if(!rtabmap_ros::convertScanMsg(
1363  scan2dMsg,
1364  frameId_,
1365  odomSensorSync_?odomFrameId:"",
1366  lastPoseStamp_,
1367  scan,
1368  scanLocalTransform,
1369  tfListener_,
1370  waitForTransform_?waitForTransformDuration_:0))
1371  {
1372  NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
1373  return;
1374  }
1375  Transform zAxis(0,0,1,0,0,0);
1376  if((scanLocalTransform.rotation()*zAxis).z() < 0)
1377  {
1378  cv::Mat flipScan;
1379  cv::flip(scan, flipScan, 1);
1380  scan = flipScan;
1381  }
1382  if(rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)
1383  {
1384  // backward compatibility, project 2D scan in /base_link frame
1385  scan = util3d::transformLaserScan(LaserScan::backwardCompatibility(scan), scanLocalTransform).data();
1386  scanLocalTransform = Transform::getIdentity();
1387  }
1388  }
1389  else if(scan3dMsg.get() != 0)
1390  {
1391  if(!rtabmap_ros::convertScan3dMsg(
1392  scan3dMsg,
1393  frameId_,
1394  odomSensorSync_?odomFrameId:"",
1395  lastPoseStamp_,
1396  scan,
1397  scanLocalTransform,
1398  tfListener_,
1399  waitForTransform_?waitForTransformDuration_:0))
1400  {
1401  NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
1402  return;
1403  }
1404  }
1405 
1406  Transform groundTruthPose;
1407  if(!groundTruthFrameId_.empty())
1408  {
1409  groundTruthPose = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
1410  }
1411 
1412  cv::Mat userData;
1413  if(userDataMsg.get())
1414  {
1415  userData = rtabmap_ros::userDataFromROS(*userDataMsg);
1416  UScopeMutex lock(userDataMutex_);
1417  if(!userData_.empty())
1418  {
1419  NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
1420  userData_ = cv::Mat();
1421  }
1422  }
1423  else
1424  {
1425  UScopeMutex lock(userDataMutex_);
1426  userData = userData_;
1427  userData_ = cv::Mat();
1428  }
1429 
1430  SensorData data(LaserScan::backwardCompatibility(scan,
1431  scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():scan3dMsg.get() != 0?scanCloudMaxPoints_:0,
1432  scan2dMsg.get() != 0?scan2dMsg->range_max:0,
1433  scanLocalTransform),
1434  left,
1435  right,
1436  stereoModel,
1437  lastPoseIntermediate_?-1:leftImageMsg->header.seq,
1438  rtabmap_ros::timestampFromROS(lastPoseStamp_),
1439  userData);
1440  data.setGroundTruth(groundTruthPose);
1441 
1442  OdometryInfo odomInfo;
1443  if(odomInfoMsg.get())
1444  {
1445  odomInfo = odomInfoFromROS(*odomInfoMsg);
1446  }
1447 
1448  process(lastPoseStamp_,
1449  data,
1450  lastPose_,
1451  odomFrameId,
1452  covariance_,
1453  odomInfo);
1454 
1455  covariance_ = cv::Mat();
1456 }
1457 
1458 void CoreWrapper::process(
1459  const ros::Time & stamp,
1460  const SensorData & data,
1461  const Transform & odom,
1462  const std::string & odomFrameId,
1463  const cv::Mat & odomCovariance,
1464  const OdometryInfo & odomInfo)
1465 {
1466  UTimer timer;
1467  if(rtabmap_.isIDsGenerated() || data.id() > 0)
1468  {
1469  double timeRtabmap = 0.0;
1470  double timeUpdateMaps = 0.0;
1471  double timePublishMaps = 0.0;
1472 
1473  cv::Mat covariance = odomCovariance;
1474  if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
1475  {
1476  covariance = cv::Mat::eye(6,6,CV_64FC1);
1477  if(odomDefaultLinVariance_ > 0.0f)
1478  {
1479  covariance.at<double>(0,0) = odomDefaultLinVariance_;
1480  covariance.at<double>(1,1) = odomDefaultLinVariance_;
1481  covariance.at<double>(2,2) = odomDefaultLinVariance_;
1482  }
1483  if(odomDefaultAngVariance_ > 0.0f)
1484  {
1485  covariance.at<double>(3,3) = odomDefaultAngVariance_;
1486  covariance.at<double>(4,4) = odomDefaultAngVariance_;
1487  covariance.at<double>(5,5) = odomDefaultAngVariance_;
1488  }
1489  }
1490 
1491  std::map<std::string, float> externalStats;
1492  std::vector<float> odomVelocity;
1493  if(odomInfo.timeEstimation != 0.0f)
1494  {
1495  externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
1496  externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
1497  externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
1498  externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
1499  externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
1500  float speed = 0.0f;
1501  if(odomInfo.interval>0.0)
1502  speed = odomInfo.transform.x()/odomInfo.interval*3.6;
1503  externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
1504  externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
1505  externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
1506  externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
1507  externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
1508  externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
1509  externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
1510  externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
1511  externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", odomInfo.memoryUsage));
1512 
1513  if(odomInfo.interval>0.0)
1514  {
1515  odomVelocity.resize(6);
1516  float x,y,z,roll,pitch,yaw;
1517  odomInfo.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1518  odomVelocity[0] = x/odomInfo.interval;
1519  odomVelocity[1] = y/odomInfo.interval;
1520  odomVelocity[2] = z/odomInfo.interval;
1521  odomVelocity[3] = roll/odomInfo.interval;
1522  odomVelocity[4] = pitch/odomInfo.interval;
1523  odomVelocity[5] = yaw/odomInfo.interval;
1524  }
1525  }
1526  if(rtabmapROSStats_.size())
1527  {
1528  externalStats.insert(rtabmapROSStats_.begin(), rtabmapROSStats_.end());
1529  rtabmapROSStats_.clear();
1530  }
1531 
1532  if(rtabmap_.process(data, odom, covariance, odomVelocity, externalStats))
1533  {
1534  timeRtabmap = timer.ticks();
1535  mapToOdomMutex_.lock();
1536  mapToOdom_ = rtabmap_.getMapCorrection();
1537  odomFrameId_ = odomFrameId;
1538  mapToOdomMutex_.unlock();
1539 
1540  if(data.id() < 0)
1541  {
1542  NODELET_INFO("Intermediate node added");
1543  }
1544  else
1545  {
1546  // Publish local graph, info
1547  this->publishStats(stamp);
1548  if(localizationPosePub_.getNumSubscribers() &&
1549  !rtabmap_.getStatistics().localizationCovariance().empty())
1550  {
1551  geometry_msgs::PoseWithCovarianceStamped poseMsg;
1552  poseMsg.header.frame_id = mapFrameId_;
1553  poseMsg.header.stamp = stamp;
1554  rtabmap_ros::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose);
1555  poseMsg.pose.covariance;
1556  const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance();
1557  memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double));
1558  localizationPosePub_.publish(poseMsg);
1559  }
1560  std::map<int, rtabmap::Transform> filteredPoses = rtabmap_.getLocalOptimizedPoses();
1561 
1562  // create a tmp signature with latest sensory data if latest signature was ignored
1563  std::map<int, rtabmap::Signature> tmpSignature;
1564  if(rtabmap_.getMemory() == 0 ||
1565  filteredPoses.size() == 0 ||
1566  rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first ||
1567  rtabmap_.getMemory()->getLastWorkingSignature() == 0 ||
1568  rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 ||
1569  (!mapsManager_.getOccupancyGrid()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data
1570  {
1571  SensorData tmpData = data;
1572  tmpData.setId(-1);
1573  tmpSignature.insert(std::make_pair(-1, Signature(-1, -1, 0, data.stamp(), "", odom, Transform(), tmpData)));
1574  filteredPoses.insert(std::make_pair(-1, mapToOdom_*odom));
1575  }
1576 
1577  if(maxMappingNodes_ > 0 && filteredPoses.size()>1)
1578  {
1579  std::map<int, Transform> nearestPoses;
1580  std::vector<int> nodes = graph::findNearestNodes(filteredPoses, mapToOdom_*odom, maxMappingNodes_);
1581  for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
1582  {
1583  std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
1584  if(pter != filteredPoses.end())
1585  {
1586  nearestPoses.insert(*pter);
1587  }
1588  }
1589  //add negative and make sure those on a planned path are not filtered
1590  std::set<int> onPath;
1591  if(rtabmap_.getPath().size())
1592  {
1593  std::vector<int> nextNodes = rtabmap_.getPathNextNodes();
1594  onPath.insert(nextNodes.begin(), nextNodes.end());
1595  }
1596  for(std::map<int, Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
1597  {
1598  if(iter->first < 0 || onPath.find(iter->first) != onPath.end())
1599  {
1600  nearestPoses.insert(*iter);
1601  }
1602  else if(onPath.empty())
1603  {
1604  break;
1605  }
1606  }
1607 
1608  filteredPoses = nearestPoses;
1609  }
1610 
1611  // Update maps
1612  filteredPoses = mapsManager_.updateMapCaches(
1613  filteredPoses,
1614  rtabmap_.getMemory(),
1615  false,
1616  false,
1617  tmpSignature);
1618 
1619  timeUpdateMaps = timer.ticks();
1620 
1621  mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_);
1622 
1623  // update goal if planning is enabled
1624  if(!currentMetricGoal_.isNull())
1625  {
1626  if(rtabmap_.getPath().size() == 0)
1627  {
1628  if(rtabmap_.getPathStatus() > 0)
1629  {
1630  // Goal reached
1631  NODELET_INFO("Planning: Publishing goal reached!");
1632  }
1633  else
1634  {
1635  NODELET_WARN("Planning: Plan failed!");
1636  if(mbClient_.isServerConnected())
1637  {
1638  mbClient_.cancelGoal();
1639  }
1640  }
1641  if(goalReachedPub_.getNumSubscribers())
1642  {
1643  std_msgs::Bool result;
1644  result.data = rtabmap_.getPathStatus() > 0;
1645  goalReachedPub_.publish(result);
1646  }
1647  currentMetricGoal_.setNull();
1648  lastPublishedMetricGoal_.setNull();
1649  latestNodeWasReached_ = false;
1650  }
1651  else
1652  {
1653  currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
1654  if(!currentMetricGoal_.isNull())
1655  {
1656  // Adjust the target pose relative to last node
1657  if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
1658  {
1659  if(latestNodeWasReached_ ||
1660  rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius())
1661  {
1662  latestNodeWasReached_ = true;
1663  currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
1664  }
1665  }
1666 
1667  // publish next goal with updated currentMetricGoal_
1668  publishCurrentGoal(stamp);
1669 
1670  // publish local path
1671  publishLocalPath(stamp);
1672 
1673  // publish global path
1674  publishGlobalPath(stamp);
1675  }
1676  else
1677  {
1678  NODELET_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)",
1679  rtabmap_.getPathCurrentGoalId());
1680  rtabmap_.clearPath(-1);
1681  if(goalReachedPub_.getNumSubscribers())
1682  {
1683  std_msgs::Bool result;
1684  result.data = false;
1685  goalReachedPub_.publish(result);
1686  }
1687  currentMetricGoal_.setNull();
1688  lastPublishedMetricGoal_.setNull();
1689  latestNodeWasReached_ = false;
1690  }
1691  }
1692  }
1693 
1694  timePublishMaps = timer.ticks();
1695  }
1696  }
1697  else
1698  {
1699  timeRtabmap = timer.ticks();
1700  }
1701  NODELET_INFO("rtabmap (%d): Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)",
1702  rtabmap_.getLastLocationId(),
1703  rate_>0?1.0f/rate_:0,
1704  rtabmap_.getTimeThreshold()/1000.0f,
1705  timeRtabmap,
1706  timeUpdateMaps,
1707  timePublishMaps,
1708  (int)rtabmap_.getLocalOptimizedPoses().size(),
1709  rtabmap_.getWMSize()+rtabmap_.getSTMSize());
1710  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0));
1711  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeRtabmap/ms"), timeRtabmap*1000.0f));
1712  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeUpdatingMaps/ms"), timeUpdateMaps*1000.0f));
1713  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimePublishing/ms"), timePublishMaps*1000.0f));
1714  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeTotal/ms"), (timeRtabmap+timeUpdateMaps+timePublishMaps)*1000.0f));
1715  }
1716  else if(!rtabmap_.isIDsGenerated())
1717  {
1718  NODELET_WARN("Ignoring received image because its sequence ID=0. Please "
1719  "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. "
1720  "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and "
1721  "when you need to have IDs output of RTAB-map synchronized with the source "
1722  "image sequence ID.");
1723  }
1724 }
1725 
1726 void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg)
1727 {
1728  if(!paused_)
1729  {
1730  UScopeMutex lock(userDataMutex_);
1731  if(!userData_.empty())
1732  {
1733  ROS_WARN("Overwriting previous user data set. Asynchronous user "
1734  "data input topic should be used with user data published at "
1735  "lower rate than map update rate (current %s=%f).",
1736  Parameters::kRtabmapDetectionRate().c_str(), rate_);
1737  }
1738  userData_ = rtabmap_ros::userDataFromROS(*dataMsg);
1739  }
1740 }
1741 
1742 void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg)
1743 {
1744  if(!paused_)
1745  {
1746  globalPose_ = *globalPoseMsg;
1747  }
1748 }
1749 
1750 void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
1751 {
1752  Transform intialPose = rtabmap_ros::transformFromPoseMsg(msg->pose.pose);
1753  if(intialPose.isNull())
1754  {
1755  NODELET_ERROR("Pose received is null!");
1756  return;
1757  }
1758 
1759  rtabmap_.setInitialPose(intialPose);
1760 }
1761 
1762 void CoreWrapper::goalCommonCallback(
1763  int id,
1764  const std::string & label,
1765  const Transform & pose,
1766  const ros::Time & stamp,
1767  double * planningTime)
1768 {
1769  UTimer timer;
1770 
1771  if(id == 0 && !label.empty() && rtabmap_.getMemory())
1772  {
1773  id = rtabmap_.getMemory()->getSignatureIdByLabel(label);
1774  }
1775 
1776  if(id > 0)
1777  {
1778  NODELET_INFO("Planning: set goal %d", id);
1779  }
1780  else if(!pose.isNull())
1781  {
1782  NODELET_INFO("Planning: set goal %s", pose.prettyPrint().c_str());
1783  }
1784 
1785  if(planningTime)
1786  {
1787  *planningTime = 0.0;
1788  }
1789 
1790  bool success = false;
1791  if((id > 0 && rtabmap_.computePath(id, true)) ||
1792  (!pose.isNull() && rtabmap_.computePath(pose)))
1793  {
1794  if(planningTime)
1795  {
1796  *planningTime = timer.elapsed();
1797  }
1798  NODELET_INFO("Planning: Time computing path = %f s", timer.ticks());
1799  const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
1800 
1801  currentMetricGoal_.setNull();
1802  lastPublishedMetricGoal_.setNull();
1803  latestNodeWasReached_ = false;
1804  if(poses.size() == 0)
1805  {
1806  NODELET_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).",
1807  rtabmap_.getGoalReachedRadius());
1808  rtabmap_.clearPath(1);
1809  if(goalReachedPub_.getNumSubscribers())
1810  {
1811  std_msgs::Bool result;
1812  result.data = true;
1813  goalReachedPub_.publish(result);
1814  }
1815  success = true;
1816  }
1817  else
1818  {
1819  currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
1820  if(!currentMetricGoal_.isNull())
1821  {
1822  NODELET_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
1823 
1824  // Adjust the target pose relative to last node
1825  if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
1826  {
1827  if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius())
1828  {
1829  latestNodeWasReached_ = true;
1830  currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
1831  }
1832  }
1833 
1834  publishCurrentGoal(stamp);
1835  publishLocalPath(stamp);
1836  publishGlobalPath(stamp);
1837 
1838  // Just output the path on screen
1839  std::stringstream stream;
1840  for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1841  {
1842  if(iter != poses.begin())
1843  {
1844  stream << " ";
1845  }
1846  stream << iter->first;
1847  }
1848  NODELET_INFO("Global path: [%s]", stream.str().c_str());
1849  success=true;
1850  }
1851  else
1852  {
1853  NODELET_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
1854  }
1855  }
1856  }
1857  else if(!label.empty())
1858  {
1859  NODELET_ERROR("Planning: Node with label \"%s\" not found!", label.c_str());
1860  }
1861  else if(pose.isNull())
1862  {
1863  if(id > 0)
1864  {
1865  NODELET_ERROR("Planning: Could not plan to node %d! The node is not in map's graph (look for warnings before this message for more details).", id);
1866  }
1867  else
1868  {
1869  NODELET_ERROR("Planning: Node id should be > 0 !");
1870  }
1871  }
1872  else
1873  {
1874  NODELET_ERROR("Planning: A node near the goal's pose not found! The pose may be to far from the graph (RGBD/LocalRadius=%f m)", rtabmap_.getLocalRadius());
1875  }
1876 
1877  if(!success)
1878  {
1879  rtabmap_.clearPath(-1);
1880  if(goalReachedPub_.getNumSubscribers())
1881  {
1882  std_msgs::Bool result;
1883  result.data = false;
1884  goalReachedPub_.publish(result);
1885  }
1886  }
1887 }
1888 
1889 void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
1890 {
1891  Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose);
1892  if(targetPose.isNull())
1893  {
1894  NODELET_ERROR("Pose received is null!");
1895  return;
1896  }
1897 
1898  // transform goal in /map frame
1899  if(mapFrameId_.compare(msg->header.frame_id) != 0)
1900  {
1901  Transform t = rtabmap_ros::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
1902  if(t.isNull())
1903  {
1904  NODELET_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",
1905  msg->header.frame_id.c_str(), mapFrameId_.c_str());
1906  return;
1907  }
1908  targetPose = t * targetPose;
1909  }
1910 
1911  goalCommonCallback(0, "", targetPose, msg->header.stamp);
1912 }
1913 
1914 void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
1915 {
1916  if(msg->node_id <= 0 && msg->node_label.empty())
1917  {
1918  NODELET_ERROR("Node id or label should be set!");
1919  return;
1920  }
1921  goalCommonCallback(msg->node_id, msg->node_label, Transform(), msg->header.stamp);
1922 }
1923 
1924 bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
1925 {
1926  ros::NodeHandle nh;
1927  for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
1928  {
1929  std::string vStr;
1930  bool vBool;
1931  int vInt;
1932  double vDouble;
1933  if(nh.getParam(iter->first, vStr))
1934  {
1935  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
1936  iter->second = vStr;
1937  }
1938  else if(nh.getParam(iter->first, vBool))
1939  {
1940  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
1941  iter->second = uBool2Str(vBool);
1942  }
1943  else if(nh.getParam(iter->first, vInt))
1944  {
1945  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
1946  iter->second = uNumber2Str(vInt).c_str();
1947  }
1948  else if(nh.getParam(iter->first, vDouble))
1949  {
1950  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
1951  iter->second = uNumber2Str(vDouble).c_str();
1952  }
1953  }
1954  NODELET_INFO("rtabmap: Updating parameters");
1955  if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end())
1956  {
1957  rate_ = uStr2Float(parameters_.at(Parameters::kRtabmapDetectionRate()));
1958  NODELET_INFO("RTAB-Map rate detection = %f Hz", rate_);
1959  }
1960  rtabmap_.parseParameters(parameters_);
1961  mapsManager_.setParameters(parameters_);
1962  return true;
1963 }
1964 
1965 bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
1966 {
1967  NODELET_INFO("rtabmap: Reset");
1968  rtabmap_.resetMemory();
1969  covariance_ = cv::Mat();
1970  lastPose_.setIdentity();
1971  lastPoseIntermediate_ = false;
1972  currentMetricGoal_.setNull();
1973  lastPublishedMetricGoal_.setNull();
1974  latestNodeWasReached_ = false;
1975  mapsManager_.clear();
1976  previousStamp_ = ros::Time(0);
1977  globalPose_.header.stamp = ros::Time(0);
1978  userDataMutex_.lock();
1979  userData_ = cv::Mat();
1980  userDataMutex_.unlock();
1981  return true;
1982 }
1983 
1984 bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
1985 {
1986  if(paused_)
1987  {
1988  NODELET_WARN("rtabmap: Already paused!");
1989  }
1990  else
1991  {
1992  paused_ = true;
1993  NODELET_INFO("rtabmap: paused!");
1994  ros::NodeHandle nh;
1995  nh.setParam("is_rtabmap_paused", true);
1996  }
1997  return true;
1998 }
1999 
2000 bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2001 {
2002  if(!paused_)
2003  {
2004  NODELET_WARN("rtabmap: Already running!");
2005  }
2006  else
2007  {
2008  paused_ = false;
2009  NODELET_INFO("rtabmap: resumed!");
2010  ros::NodeHandle nh;
2011  nh.setParam("is_rtabmap_paused", false);
2012  }
2013  return true;
2014 }
2015 
2016 bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2017 {
2018  NODELET_INFO("rtabmap: Trigger new map");
2019  rtabmap_.triggerNewMap();
2020  return true;
2021 }
2022 
2023 bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2024 {
2025  NODELET_INFO("Backup: Saving memory...");
2026  rtabmap_.close();
2027  NODELET_INFO("Backup: Saving memory... done!");
2028 
2029  covariance_ = cv::Mat();
2030  lastPose_.setIdentity();
2031  currentMetricGoal_.setNull();
2032  lastPublishedMetricGoal_.setNull();
2033  latestNodeWasReached_ = false;
2034  userDataMutex_.lock();
2035  userData_ = cv::Mat();
2036  userDataMutex_.unlock();
2037  globalPose_.header.stamp = ros::Time(0);
2038 
2039  NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
2040  UFile::copy(databasePath_, databasePath_+".back");
2041  NODELET_INFO("Backup: Saving \"%s\" to \"%s\"... done!", databasePath_.c_str(), (databasePath_+".back").c_str());
2042 
2043  NODELET_INFO("Backup: Reloading memory...");
2044  rtabmap_.init(parameters_, databasePath_);
2045  NODELET_INFO("Backup: Reloading memory... done!");
2046 
2047  return true;
2048 }
2049 
2050 bool CoreWrapper::setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2051 {
2052  NODELET_INFO("rtabmap: Set localization mode");
2053  rtabmap::ParametersMap parameters;
2054  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
2055  ros::NodeHandle & nh = getNodeHandle();
2056  nh.setParam(rtabmap::Parameters::kMemIncrementalMemory(), "false");
2057  rtabmap_.parseParameters(parameters);
2058  return true;
2059 }
2060 
2061 bool CoreWrapper::setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2062 {
2063  NODELET_INFO("rtabmap: Set mapping mode");
2064  rtabmap::ParametersMap parameters;
2065  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "true"));
2066  ros::NodeHandle & nh = getNodeHandle();
2067  nh.setParam(rtabmap::Parameters::kMemIncrementalMemory(), "true");
2068  rtabmap_.parseParameters(parameters);
2069  return true;
2070 }
2071 
2072 bool CoreWrapper::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2073 {
2074  NODELET_INFO("rtabmap: Set log level to Debug");
2075  ULogger::setLevel(ULogger::kDebug);
2076  return true;
2077 }
2078 bool CoreWrapper::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2079 {
2080  NODELET_INFO("rtabmap: Set log level to Info");
2081  ULogger::setLevel(ULogger::kInfo);
2082  return true;
2083 }
2084 bool CoreWrapper::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2085 {
2086  NODELET_INFO("rtabmap: Set log level to Warning");
2087  ULogger::setLevel(ULogger::kWarning);
2088  return true;
2089 }
2090 bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2091 {
2092  NODELET_INFO("rtabmap: Set log level to Error");
2093  ULogger::setLevel(ULogger::kError);
2094  return true;
2095 }
2096 
2097 bool CoreWrapper::getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res)
2098 {
2099  NODELET_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...",
2100  req.global?"true":"false",
2101  req.optimized?"true":"false",
2102  req.graphOnly?"true":"false");
2103  std::map<int, Signature> signatures;
2104  std::map<int, Transform> poses;
2105  std::multimap<int, rtabmap::Link> constraints;
2106 
2107  if(req.graphOnly)
2108  {
2109  rtabmap_.getGraph(
2110  poses,
2111  constraints,
2112  req.optimized,
2113  req.global,
2114  &signatures);
2115  }
2116  else
2117  {
2118  rtabmap_.get3DMap(
2119  signatures,
2120  poses,
2121  constraints,
2122  req.optimized,
2123  req.global);
2124  }
2125 
2126  //RGB-D SLAM data
2127  rtabmap_ros::mapDataToROS(poses,
2128  constraints,
2129  signatures,
2130  mapToOdom_,
2131  res.data);
2132 
2133  res.data.header.stamp = ros::Time::now();
2134  res.data.header.frame_id = mapFrameId_;
2135 
2136  return true;
2137 }
2138 
2139 bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
2140 {
2141  if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() &&
2142  !uStr2Bool(parameters_.at(Parameters::kGridFromDepth())))
2143  {
2144  NODELET_WARN("/get_proj_map service is deprecated! Call /get_grid_map service "
2145  "instead with <param name=\"%s\" type=\"string\" value=\"true\"/>. "
2146  "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
2147  "all occupancy grid parameters.",
2148  Parameters::kGridFromDepth().c_str());
2149  }
2150  else
2151  {
2152  NODELET_WARN("/get_proj_map service is deprecated! Call /get_grid_map service instead.");
2153  }
2154  return getGridMapCallback(req, res);
2155 }
2156 
2157 bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
2158 {
2159  NODELET_WARN("/get_grid_map service is deprecated! Call /get_map service instead.");
2160  return getMapCallback(req, res);
2161 }
2162 
2163 bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
2164 {
2165  // create the grid map
2166  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
2167  cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
2168 
2169  if(!pixels.empty())
2170  {
2171  //init
2172  res.map.info.resolution = gridCellSize;
2173  res.map.info.origin.position.x = 0.0;
2174  res.map.info.origin.position.y = 0.0;
2175  res.map.info.origin.position.z = 0.0;
2176  res.map.info.origin.orientation.x = 0.0;
2177  res.map.info.origin.orientation.y = 0.0;
2178  res.map.info.origin.orientation.z = 0.0;
2179  res.map.info.origin.orientation.w = 1.0;
2180 
2181  res.map.info.width = pixels.cols;
2182  res.map.info.height = pixels.rows;
2183  res.map.info.origin.position.x = xMin;
2184  res.map.info.origin.position.y = yMin;
2185  res.map.data.resize(res.map.info.width * res.map.info.height);
2186 
2187  memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
2188 
2189  res.map.header.frame_id = mapFrameId_;
2190  res.map.header.stamp = ros::Time::now();
2191  return true;
2192  }
2193  return false;
2194 }
2195 
2196 bool CoreWrapper::getProbMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
2197 {
2198  // create the grid map
2199  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
2200  cv::Mat pixels = mapsManager_.getGridProbMap(xMin, yMin, gridCellSize);
2201 
2202  if(!pixels.empty())
2203  {
2204  //init
2205  res.map.info.resolution = gridCellSize;
2206  res.map.info.origin.position.x = 0.0;
2207  res.map.info.origin.position.y = 0.0;
2208  res.map.info.origin.position.z = 0.0;
2209  res.map.info.origin.orientation.x = 0.0;
2210  res.map.info.origin.orientation.y = 0.0;
2211  res.map.info.origin.orientation.z = 0.0;
2212  res.map.info.origin.orientation.w = 1.0;
2213 
2214  res.map.info.width = pixels.cols;
2215  res.map.info.height = pixels.rows;
2216  res.map.info.origin.position.x = xMin;
2217  res.map.info.origin.position.y = yMin;
2218  res.map.data.resize(res.map.info.width * res.map.info.height);
2219 
2220  memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
2221 
2222  res.map.header.frame_id = mapFrameId_;
2223  res.map.header.stamp = ros::Time::now();
2224  return true;
2225  }
2226  return false;
2227 }
2228 
2229 bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtabmap_ros::PublishMap::Response& res)
2230 {
2231  NODELET_INFO("rtabmap: Publishing map...");
2232 
2233  if(mapDataPub_.getNumSubscribers() ||
2234  (!req.graphOnly && mapsManager_.hasSubscribers()) ||
2235  (req.graphOnly && (labelsPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers() || mapPathPub_.getNumSubscribers())))
2236  {
2237  std::map<int, Transform> poses;
2238  std::multimap<int, rtabmap::Link> constraints;
2239  std::map<int, Signature > signatures;
2240 
2241  if(req.graphOnly)
2242  {
2243  rtabmap_.getGraph(
2244  poses,
2245  constraints,
2246  req.optimized,
2247  req.global,
2248  &signatures);
2249  }
2250  else
2251  {
2252  rtabmap_.get3DMap(
2253  signatures,
2254  poses,
2255  constraints,
2256  req.optimized,
2257  req.global);
2258  }
2259 
2260  ros::Time now = ros::Time::now();
2261  if(mapDataPub_.getNumSubscribers())
2262  {
2263  rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
2264  msg->header.stamp = now;
2265  msg->header.frame_id = mapFrameId_;
2266 
2267  rtabmap_ros::mapDataToROS(poses,
2268  constraints,
2269  signatures,
2270  mapToOdom_,
2271  *msg);
2272 
2273  mapDataPub_.publish(msg);
2274  }
2275 
2276  if(mapGraphPub_.getNumSubscribers())
2277  {
2278  rtabmap_ros::MapGraphPtr msg(new rtabmap_ros::MapGraph);
2279  msg->header.stamp = now;
2280  msg->header.frame_id = mapFrameId_;
2281 
2282  rtabmap_ros::mapGraphToROS(poses,
2283  constraints,
2284  mapToOdom_,
2285  *msg);
2286 
2287  mapGraphPub_.publish(msg);
2288  }
2289 
2290  if(!req.graphOnly && mapsManager_.hasSubscribers())
2291  {
2292  std::map<int, Transform> filteredPoses = poses;
2293  if(maxMappingNodes_ > 0 && poses.size()>1)
2294  {
2295  std::map<int, Transform> nearestPoses;
2296  std::vector<int> nodes = graph::findNearestNodes(filteredPoses, filteredPoses.rbegin()->second, maxMappingNodes_);
2297  for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
2298  {
2299  std::map<int, Transform>::iterator pter = filteredPoses.find(*iter);
2300  if(pter != filteredPoses.end())
2301  {
2302  nearestPoses.insert(*pter);
2303  }
2304  }
2305  }
2306  if(signatures.size())
2307  {
2308  filteredPoses = mapsManager_.updateMapCaches(
2309  filteredPoses,
2310  rtabmap_.getMemory(),
2311  false,
2312  false,
2313  signatures);
2314  }
2315  else
2316  {
2317  filteredPoses = mapsManager_.getFilteredPoses(filteredPoses);
2318  }
2319  mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
2320  }
2321 
2322  bool pubLabels = labelsPub_.getNumSubscribers();
2323  bool pubPath = mapPathPub_.getNumSubscribers();
2324  if(pubLabels || pubPath)
2325  {
2326  if(poses.size() && signatures.size())
2327  {
2328  visualization_msgs::MarkerArray markers;
2329  nav_msgs::Path path;
2330  if(pubPath)
2331  {
2332  path.poses.resize(poses.size());
2333  }
2334  int oi=0;
2335  for(std::map<int, Signature>::const_iterator iter=signatures.begin();
2336  iter!=signatures.end();
2337  ++iter)
2338  {
2339  std::map<int, Transform>::const_iterator poseIter= poses.find(iter->first);
2340  if(poseIter!=poses.end())
2341  {
2342  if(pubLabels)
2343  {
2344  // Add labels
2345  if(!iter->second.getLabel().empty())
2346  {
2347  visualization_msgs::Marker marker;
2348  marker.header.frame_id = mapFrameId_;
2349  marker.header.stamp = now;
2350  marker.ns = "labels";
2351  marker.id = -iter->first;
2352  marker.action = visualization_msgs::Marker::ADD;
2353  marker.pose.position.x = poseIter->second.x();
2354  marker.pose.position.y = poseIter->second.y();
2355  marker.pose.position.z = poseIter->second.z();
2356  marker.pose.orientation.x = 0.0;
2357  marker.pose.orientation.y = 0.0;
2358  marker.pose.orientation.z = 0.0;
2359  marker.pose.orientation.w = 1.0;
2360  marker.scale.x = 1;
2361  marker.scale.y = 1;
2362  marker.scale.z = 0.5;
2363  marker.color.a = 0.7;
2364  marker.color.r = 1.0;
2365  marker.color.g = 0.0;
2366  marker.color.b = 0.0;
2367 
2368  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
2369  marker.text = iter->second.getLabel();
2370 
2371  markers.markers.push_back(marker);
2372  }
2373  // Add node ids
2374  visualization_msgs::Marker marker;
2375  marker.header.frame_id = mapFrameId_;
2376  marker.header.stamp = now;
2377  marker.ns = "ids";
2378  marker.id = iter->first;
2379  marker.action = visualization_msgs::Marker::ADD;
2380  marker.pose.position.x = poseIter->second.x();
2381  marker.pose.position.y = poseIter->second.y();
2382  marker.pose.position.z = poseIter->second.z();
2383  marker.pose.orientation.x = 0.0;
2384  marker.pose.orientation.y = 0.0;
2385  marker.pose.orientation.z = 0.0;
2386  marker.pose.orientation.w = 1.0;
2387  marker.scale.x = 1;
2388  marker.scale.y = 1;
2389  marker.scale.z = 0.2;
2390  marker.color.a = 0.5;
2391  marker.color.r = 1.0;
2392  marker.color.g = 1.0;
2393  marker.color.b = 1.0;
2394  marker.lifetime = ros::Duration(2.0f/rate_);
2395 
2396  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
2397  marker.text = uNumber2Str(iter->first);
2398 
2399  markers.markers.push_back(marker);
2400  }
2401  if(pubPath)
2402  {
2403  rtabmap_ros::transformToPoseMsg(poseIter->second, path.poses.at(oi).pose);
2404  path.poses.at(oi).header.frame_id = mapFrameId_;
2405  path.poses.at(oi).header.stamp = ros::Time(iter->second.getStamp());
2406  ++oi;
2407  }
2408  }
2409  }
2410 
2411  if(pubLabels && markers.markers.size())
2412  {
2413  labelsPub_.publish(markers);
2414  }
2415  if(pubPath && oi)
2416  {
2417  path.header.frame_id = mapFrameId_;
2418  path.header.stamp = now;
2419  path.poses.resize(oi);
2420  mapPathPub_.publish(path);
2421  }
2422  }
2423  }
2424  }
2425  else
2426  {
2427  UWARN("No subscribers, don't need to publish!");
2428  }
2429 
2430  return true;
2431 }
2432 
2433 bool CoreWrapper::getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &res)
2434 {
2435  Transform pose = rtabmap_ros::transformFromPoseMsg(req.goal.pose);
2436  UTimer timer;
2437  if(!pose.isNull())
2438  {
2439  // transform goal in /map frame
2440  if(mapFrameId_.compare(req.goal.header.frame_id) != 0)
2441  {
2442  Transform t = rtabmap_ros::getTransform(mapFrameId_, req.goal.header.frame_id, req.goal.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
2443  if(t.isNull())
2444  {
2445  NODELET_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",
2446  req.goal.header.frame_id.c_str(), mapFrameId_.c_str());
2447  return true;
2448  }
2449  pose = t * pose;
2450  }
2451 
2452  if(rtabmap_.computePath(pose, req.tolerance))
2453  {
2454  NODELET_INFO("Planning: Time computing path = %f s", timer.ticks());
2455  const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
2456  res.plan.header.frame_id = mapFrameId_;
2457  res.plan.header.stamp = ros::Time::now();
2458  if(poses.size() == 0)
2459  {
2460  NODELET_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).",
2461  rtabmap_.getGoalReachedRadius());
2462  // just set the goal directly
2463  res.plan.poses.resize(1);
2464  rtabmap_ros::transformToPoseMsg(pose, res.plan.poses[0].pose);
2465  }
2466  else
2467  {
2468  res.plan.poses.resize(poses.size());
2469  int oi = 0;
2470  for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2471  {
2472  res.plan.poses[oi].header = res.plan.header;
2473  rtabmap_ros::transformToPoseMsg(iter->second, res.plan.poses[oi].pose);
2474  ++oi;
2475  }
2476  if(!rtabmap_.getPathTransformToGoal().isIdentity())
2477  {
2478  res.plan.poses.resize(res.plan.poses.size()+1);
2479  Transform p = rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal();
2480  rtabmap_ros::transformToPoseMsg(p, res.plan.poses[res.plan.poses.size()-1].pose);
2481  }
2482 
2483  // Just output the path on screen
2484  std::stringstream stream;
2485  for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2486  {
2487  if(iter != poses.begin())
2488  {
2489  stream << " ";
2490  }
2491  stream << iter->first;
2492  }
2493  NODELET_INFO("Planned path: [%s]", stream.str().c_str());
2494  }
2495  }
2496  rtabmap_.clearPath(0);
2497  }
2498  return true;
2499 }
2500 
2501 bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
2502 {
2503  double planningTime = 0.0;
2504  goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now(), &planningTime);
2505  const std::vector<std::pair<int, Transform> > & path = rtabmap_.getPath();
2506  res.path_ids.resize(path.size());
2507  res.path_poses.resize(path.size());
2508  res.planning_time = planningTime;
2509  for(unsigned int i=0; i<path.size(); ++i)
2510  {
2511  res.path_ids[i] = path[i].first;
2512  rtabmap_ros::transformToPoseMsg(path[i].second, res.path_poses[i]);
2513  }
2514  return true;
2515 }
2516 
2517 bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
2518 {
2519  if(rtabmap_.getPath().size())
2520  {
2521  NODELET_WARN("Goal cancelled!");
2522  rtabmap_.clearPath(0);
2523  currentMetricGoal_.setNull();
2524  lastPublishedMetricGoal_.setNull();
2525  latestNodeWasReached_ = false;
2526  if(goalReachedPub_.getNumSubscribers())
2527  {
2528  std_msgs::Bool result;
2529  result.data = false;
2530  goalReachedPub_.publish(result);
2531  }
2532  }
2533  if(mbClient_.isServerConnected())
2534  {
2535  mbClient_.cancelGoal();
2536  }
2537 
2538  return true;
2539 }
2540 
2541 bool CoreWrapper::setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res)
2542 {
2543  if(rtabmap_.labelLocation(req.node_id, req.node_label))
2544  {
2545  if(req.node_id > 0)
2546  {
2547  NODELET_INFO("Set label \"%s\" to node %d", req.node_label.c_str(), req.node_id);
2548  }
2549  else
2550  {
2551  NODELET_INFO("Set label \"%s\" to last node", req.node_label.c_str());
2552  }
2553  }
2554  else
2555  {
2556  if(req.node_id > 0)
2557  {
2558  NODELET_ERROR("Could not set label \"%s\" to node %d", req.node_label.c_str(), req.node_id);
2559  }
2560  else
2561  {
2562  NODELET_ERROR("Could not set label \"%s\" to last node", req.node_label.c_str());
2563  }
2564  }
2565  return true;
2566 }
2567 
2568 bool CoreWrapper::listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res)
2569 {
2570  if(rtabmap_.getMemory())
2571  {
2572  std::map<int, std::string> labels = rtabmap_.getMemory()->getAllLabels();
2573  res.labels = uValues(labels);
2574  NODELET_INFO("List labels service: %d labels found.", (int)res.labels.size());
2575  }
2576  return true;
2577 }
2578 
2579 void CoreWrapper::publishStats(const ros::Time & stamp)
2580 {
2581  UDEBUG("Publishing stats...");
2582  const rtabmap::Statistics & stats = rtabmap_.getStatistics();
2583 
2584  if(infoPub_.getNumSubscribers())
2585  {
2586  //NODELET_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId());
2587  rtabmap_ros::InfoPtr msg(new rtabmap_ros::Info);
2588  msg->header.stamp = stamp;
2589  msg->header.frame_id = mapFrameId_;
2590 
2591  rtabmap_ros::infoToROS(stats, *msg);
2592  infoPub_.publish(msg);
2593  }
2594 
2595  if(mapDataPub_.getNumSubscribers())
2596  {
2597  rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
2598  msg->header.stamp = stamp;
2599  msg->header.frame_id = mapFrameId_;
2600 
2601  rtabmap_ros::mapDataToROS(
2602  stats.poses(),
2603  stats.constraints(),
2604  stats.getSignatures(),
2605  stats.mapCorrection(),
2606  *msg);
2607 
2608  mapDataPub_.publish(msg);
2609  }
2610 
2611  if(mapGraphPub_.getNumSubscribers())
2612  {
2613  rtabmap_ros::MapGraphPtr msg(new rtabmap_ros::MapGraph);
2614  msg->header.stamp = stamp;
2615  msg->header.frame_id = mapFrameId_;
2616 
2617  rtabmap_ros::mapGraphToROS(
2618  stats.poses(),
2619  stats.constraints(),
2620  stats.mapCorrection(),
2621  *msg);
2622 
2623  mapGraphPub_.publish(msg);
2624  }
2625 
2626  bool pubLabels = labelsPub_.getNumSubscribers();
2627  bool pubPath = mapPathPub_.getNumSubscribers();
2628  if(pubLabels || pubPath)
2629  {
2630  if(stats.poses().size() && stats.getSignatures().size())
2631  {
2632  visualization_msgs::MarkerArray markers;
2633  nav_msgs::Path path;
2634  if(pubPath)
2635  {
2636  path.poses.resize(stats.poses().size());
2637  }
2638  int oi = 0;
2639  for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
2640  iter!=stats.getSignatures().end();
2641  ++iter)
2642  {
2643  std::map<int, Transform>::const_iterator poseIter= stats.poses().find(iter->first);
2644  if(poseIter!=stats.poses().end())
2645  {
2646  if(pubLabels)
2647  {
2648  // Add labels
2649  if(!iter->second.getLabel().empty())
2650  {
2651  visualization_msgs::Marker marker;
2652  marker.header.frame_id = mapFrameId_;
2653  marker.header.stamp = stamp;
2654  marker.ns = "labels";
2655  marker.id = -iter->first;
2656  marker.action = visualization_msgs::Marker::ADD;
2657  marker.pose.position.x = poseIter->second.x();
2658  marker.pose.position.y = poseIter->second.y();
2659  marker.pose.position.z = poseIter->second.z();
2660  marker.pose.orientation.x = 0.0;
2661  marker.pose.orientation.y = 0.0;
2662  marker.pose.orientation.z = 0.0;
2663  marker.pose.orientation.w = 1.0;
2664  marker.scale.x = 1;
2665  marker.scale.y = 1;
2666  marker.scale.z = 0.5;
2667  marker.color.a = 0.7;
2668  marker.color.r = 1.0;
2669  marker.color.g = 0.0;
2670  marker.color.b = 0.0;
2671 
2672  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
2673  marker.text = iter->second.getLabel();
2674 
2675  markers.markers.push_back(marker);
2676  }
2677  // Add node ids
2678  visualization_msgs::Marker marker;
2679  marker.header.frame_id = mapFrameId_;
2680  marker.header.stamp = stamp;
2681  marker.ns = "ids";
2682  marker.id = iter->first;
2683  marker.action = visualization_msgs::Marker::ADD;
2684  marker.pose.position.x = poseIter->second.x();
2685  marker.pose.position.y = poseIter->second.y();
2686  marker.pose.position.z = poseIter->second.z();
2687  marker.pose.orientation.x = 0.0;
2688  marker.pose.orientation.y = 0.0;
2689  marker.pose.orientation.z = 0.0;
2690  marker.pose.orientation.w = 1.0;
2691  marker.scale.x = 1;
2692  marker.scale.y = 1;
2693  marker.scale.z = 0.2;
2694  marker.color.a = 0.5;
2695  marker.color.r = 1.0;
2696  marker.color.g = 1.0;
2697  marker.color.b = 1.0;
2698  marker.lifetime = ros::Duration(2.0f/rate_);
2699 
2700  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
2701  marker.text = uNumber2Str(iter->first);
2702 
2703  markers.markers.push_back(marker);
2704  }
2705  if(pubPath)
2706  {
2707  rtabmap_ros::transformToPoseMsg(poseIter->second, path.poses.at(oi).pose);
2708  path.poses.at(oi).header.frame_id = mapFrameId_;
2709  path.poses.at(oi).header.stamp = ros::Time(iter->second.getStamp());
2710  ++oi;
2711  }
2712  }
2713  }
2714 
2715  if(pubLabels && markers.markers.size())
2716  {
2717  labelsPub_.publish(markers);
2718  }
2719  if(pubPath && oi)
2720  {
2721  path.header.frame_id = mapFrameId_;
2722  path.header.stamp = stamp;
2723  path.poses.resize(oi);
2724  mapPathPub_.publish(path);
2725  }
2726  }
2727  }
2728 }
2729 
2730 void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
2731 {
2732  if(!currentMetricGoal_.isNull() && currentMetricGoal_ != lastPublishedMetricGoal_)
2733  {
2734  NODELET_INFO("Publishing next goal: %d -> %s",
2735  rtabmap_.getPathCurrentGoalId(), currentMetricGoal_.prettyPrint().c_str());
2736 
2737  geometry_msgs::PoseStamped poseMsg;
2738  poseMsg.header.frame_id = mapFrameId_;
2739  poseMsg.header.stamp = stamp;
2740  rtabmap_ros::transformToPoseMsg(currentMetricGoal_, poseMsg.pose);
2741 
2742  if(useActionForGoal_)
2743  {
2744  if(!mbClient_.isServerConnected())
2745  {
2746  NODELET_INFO("Connecting to move_base action server...");
2747  mbClient_.waitForServer(ros::Duration(5.0));
2748  }
2749  if(mbClient_.isServerConnected())
2750  {
2751  move_base_msgs::MoveBaseGoal goal;
2752  goal.target_pose = poseMsg;
2753 
2754  mbClient_.sendGoal(goal,
2755  boost::bind(&CoreWrapper::goalDoneCb, this, _1, _2),
2756  boost::bind(&CoreWrapper::goalActiveCb, this),
2757  boost::bind(&CoreWrapper::goalFeedbackCb, this, _1));
2758  lastPublishedMetricGoal_ = currentMetricGoal_;
2759  }
2760  else
2761  {
2762  NODELET_ERROR("Cannot connect to move_base action server!");
2763  }
2764  }
2765  if(nextMetricGoalPub_.getNumSubscribers())
2766  {
2767  nextMetricGoalPub_.publish(poseMsg);
2768  if(!useActionForGoal_)
2769  {
2770  lastPublishedMetricGoal_ = currentMetricGoal_;
2771  }
2772  }
2773  }
2774 }
2775 
2776 // Called once when the goal completes
2777 void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
2778  const move_base_msgs::MoveBaseResultConstPtr& result)
2779 {
2780  bool ignore = false;
2781  if(!currentMetricGoal_.isNull())
2782  {
2783  if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
2784  {
2785  if(rtabmap_.getPath().size() &&
2786  rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first &&
2787  (!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first) || !latestNodeWasReached_))
2788  {
2789  NODELET_WARN("Planning: move_base reached current goal but it is not "
2790  "the last one planned by rtabmap. A new goal should be sent when "
2791  "rtabmap will be able to retrieve next locations on the path.");
2792  ignore = true;
2793  }
2794  else
2795  {
2796  NODELET_INFO("Planning: move_base success!");
2797  }
2798  }
2799  else
2800  {
2801  NODELET_ERROR("Planning: move_base failed for some reason. Aborting the plan...");
2802  }
2803 
2804  if(!ignore && goalReachedPub_.getNumSubscribers())
2805  {
2806  std_msgs::Bool result;
2807  result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED;
2808  goalReachedPub_.publish(result);
2809  }
2810  }
2811 
2812  if(!ignore)
2813  {
2814  rtabmap_.clearPath(1);
2815  currentMetricGoal_.setNull();
2816  lastPublishedMetricGoal_.setNull();
2817  latestNodeWasReached_ = false;
2818  }
2819 }
2820 
2821 // Called once when the goal becomes active
2822 void CoreWrapper::goalActiveCb()
2823 {
2824  //NODELET_INFO("Planning: Goal just went active");
2825 }
2826 
2827 // Called every time feedback is received for the goal
2828 void CoreWrapper::goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
2829 {
2830  //Transform basePosition = rtabmap_ros::transformFromPoseMsg(feedback->base_position.pose);
2831  //NODELET_INFO("Planning: feedback base_position = %s", basePosition.prettyPrint().c_str());
2832 }
2833 
2834 void CoreWrapper::publishLocalPath(const ros::Time & stamp)
2835 {
2836  if(rtabmap_.getPath().size())
2837  {
2838  std::vector<std::pair<int, Transform> > poses = rtabmap_.getPathNextPoses();
2839  if(poses.size())
2840  {
2841  if(localPathPub_.getNumSubscribers())
2842  {
2843  nav_msgs::Path path;
2844  path.header.frame_id = mapFrameId_;
2845  path.header.stamp = stamp;
2846  path.poses.resize(poses.size());
2847  int oi = 0;
2848  for(std::vector<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2849  {
2850  path.poses[oi].header = path.header;
2851  rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose);
2852  ++oi;
2853  }
2854  localPathPub_.publish(path);
2855  }
2856  }
2857  }
2858 }
2859 
2860 void CoreWrapper::publishGlobalPath(const ros::Time & stamp)
2861 {
2862  if(globalPathPub_.getNumSubscribers() && rtabmap_.getPath().size())
2863  {
2864  Transform pose = uValue(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPathCurrentGoalId(), Transform());
2865  if(!pose.isNull() && rtabmap_.getPathCurrentGoalIndex() < rtabmap_.getPath().size())
2866  {
2867  // transform the global path in the goal referential
2868  Transform t = pose * rtabmap_.getPath().at(rtabmap_.getPathCurrentGoalIndex()).second.inverse();
2869 
2870  nav_msgs::Path path;
2871  path.header.frame_id = mapFrameId_;
2872  path.header.stamp = stamp;
2873  path.poses.resize(rtabmap_.getPath().size());
2874  int oi = 0;
2875  for(std::vector<std::pair<int, Transform> >::const_iterator iter=rtabmap_.getPath().begin(); iter!=rtabmap_.getPath().end(); ++iter)
2876  {
2877  path.poses[oi].header = path.header;
2878  rtabmap_ros::transformToPoseMsg(t*iter->second, path.poses[oi].pose);
2879  ++oi;
2880  }
2881  if(!rtabmap_.getPathTransformToGoal().isIdentity())
2882  {
2883  path.poses.resize(path.poses.size()+1);
2884  Transform p = t * rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal();
2885  rtabmap_ros::transformToPoseMsg(p, path.poses[path.poses.size()-1].pose);
2886  }
2887  globalPathPub_.publish(path);
2888  }
2889  }
2890 }
2891 
2892 #ifdef WITH_OCTOMAP_MSGS
2893 #ifdef RTABMAP_OCTOMAP
2894 bool CoreWrapper::octomapBinaryCallback(
2895  octomap_msgs::GetOctomap::Request &req,
2896  octomap_msgs::GetOctomap::Response &res)
2897 {
2898  NODELET_INFO("Sending binary map data on service request");
2899  res.map.header.frame_id = mapFrameId_;
2900  res.map.header.stamp = ros::Time::now();
2901 
2902  std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
2903  if(maxMappingNodes_ > 0 && poses.size()>1)
2904  {
2905  std::map<int, Transform> nearestPoses;
2906  std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
2907  for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
2908  {
2909  std::map<int, Transform>::iterator pter = poses.find(*iter);
2910  if(pter != poses.end())
2911  {
2912  nearestPoses.insert(*pter);
2913  }
2914  }
2915  poses = nearestPoses;
2916  }
2917 
2918  poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
2919 
2920  const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
2921  bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
2922  return success;
2923 }
2924 
2925 bool CoreWrapper::octomapFullCallback(
2926  octomap_msgs::GetOctomap::Request &req,
2927  octomap_msgs::GetOctomap::Response &res)
2928 {
2929  NODELET_INFO("Sending full map data on service request");
2930  res.map.header.frame_id = mapFrameId_;
2931  res.map.header.stamp = ros::Time::now();
2932 
2933  std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
2934  if(maxMappingNodes_ > 0 && poses.size()>1)
2935  {
2936  std::map<int, Transform> nearestPoses;
2937  std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxMappingNodes_);
2938  for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
2939  {
2940  std::map<int, Transform>::iterator pter = poses.find(*iter);
2941  if(pter != poses.end())
2942  {
2943  nearestPoses.insert(*pter);
2944  }
2945  }
2946  poses = nearestPoses;
2947  }
2948 
2949  poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
2950 
2951  const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
2952  bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
2953  return success;
2954 }
2955 #endif
2956 #endif
2957 
2958 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet);
2959 
2960 }
rtabmap_ros::CoreWrapper::globalPose_
geometry_msgs::PoseWithCovarianceStamped globalPose_
Definition: CoreWrapper.h:284
rtabmap_ros::CoreWrapper::setLogErrorSrv_
ros::ServiceServer setLogErrorSrv_
Definition: CoreWrapper.h:247
rtabmap_ros::convertScanMsg
bool convertScanMsg(const sensor_msgs::LaserScanConstPtr &scan2dMsg, const std::string &frameId, const std::string &odomFrameId, const ros::Time &odomStamp, cv::Mat &scan, rtabmap::Transform &scanLocalTransform, tf::TransformListener &listener, double waitForTransform)
Definition: MsgConversion.cpp:1547
rtabmap_ros::CoreWrapper::getMapDataSrv_
ros::ServiceServer getMapDataSrv_
Definition: CoreWrapper.h:248
rtabmap::Rtabmap::getMapCorrection
Transform getMapCorrection() const
rtabmap::util2d::disparityFromStereoImages
cv::Mat RTABMAP_EXP disparityFromStereoImages(const cv::Mat &leftImage, const cv::Mat &rightImage, const ParametersMap &parameters=ParametersMap())
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
UDirectory::homeDir
static std::string homeDir()
message_filters::Synchronizer
roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Transform::data
const float * data() const
image_transport::ImageTransport::subscribe
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
rtabmap::Rtabmap::getPathTransformToGoal
const Transform & getPathTransformToGoal() const
message_filters::Subscriber::getTopic
std::string getTopic() const
rtabmap_ros::CoreWrapper::getMapDataCallback
bool getMapDataCallback(rtabmap_ros::GetMap::Request &req, rtabmap_ros::GetMap::Response &res)
Definition: CoreWrapper.cpp:2097
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:293
MapsManager::getFilteredPoses
std::map< int, rtabmap::Transform > getFilteredPoses(const std::map< int, rtabmap::Transform > &poses)
Definition: MapsManager.cpp:379
rtabmap::Rtabmap::get3DMap
void get3DMap(std::map< int, Signature > &signatures, std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global) const
rtabmap_ros::CoreWrapper::userData_
cv::Mat userData_
Definition: CoreWrapper.h:280
rtabmap::StereoCameraModel::baseline
double baseline() const
rtabmap_ros::CoreWrapper::getProjMapSrv_
ros::ServiceServer getProjMapSrv_
Definition: CoreWrapper.h:249
sensor_msgs::image_encodings::BGR8
const std::string BGR8
rtabmap_ros::CoreWrapper::pauseSrv_
ros::ServiceServer pauseSrv_
Definition: CoreWrapper.h:238
rtabmap_ros::CoreWrapper::userDataMutex_
UMutex userDataMutex_
Definition: CoreWrapper.h:281
nodelet::Nodelet::getMyArgv
const V_string & getMyArgv() const
rtabmap_ros::CoreWrapper::defaultCallback
void defaultCallback(const sensor_msgs::ImageConstPtr &imageMsg)
Definition: CoreWrapper.cpp:740
MapsManager::setParameters
void setParameters(const rtabmap::ParametersMap &parameters)
Definition: MapsManager.cpp:276
rtabmap_ros::CoreWrapper::userDataAsyncCallback
void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr &dataMsg)
Definition: CoreWrapper.cpp:1726
rtabmap_ros::CommonDataSubscriber::commonSingleDepthCallback
void commonSingleDepthCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_ros::UserDataConstPtr &userDataMsg, const cv_bridge::CvImageConstPtr &imageMsg, const cv_bridge::CvImageConstPtr &depthMsg, const sensor_msgs::CameraInfo &rgbCameraInfoMsg, const sensor_msgs::CameraInfo &depthCameraInfoMsg, const sensor_msgs::LaserScanConstPtr &scanMsg, const sensor_msgs::PointCloud2ConstPtr &scan3dMsg, const rtabmap_ros::OdomInfoConstPtr &odomInfoMsg)
Definition: CommonDataSubscriber.cpp:587
ros::NodeHandle
actionlib::SimpleActionClient::waitForServer
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
NODELET_ERROR
#define NODELET_ERROR(...)
rtabmap_ros::CoreWrapper::onInit
virtual void onInit()
Definition: CoreWrapper.cpp:124
rtabmap_ros::CoreWrapper::setLogDebugSrv_
ros::ServiceServer setLogDebugSrv_
Definition: CoreWrapper.h:244
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
sensor_msgs::image_encodings::RGB8
const std::string RGB8
UFile::length
long length()
test_prior_tf_to_pose.covariance
covariance
Definition: test_prior_tf_to_pose.py:41
rtabmap::Statistics::localizationCovariance
const cv::Mat & localizationCovariance() const
rtabmap_ros::CoreWrapper::lastPose_
rtabmap::Transform lastPose_
Definition: CoreWrapper.h:184
rtabmap::Rtabmap::clearPath
void clearPath(int status)
NODELET_WARN
#define NODELET_WARN(...)
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rtabmap_ros::convertScan3dMsg
bool convertScan3dMsg(const sensor_msgs::PointCloud2ConstPtr &scan3dMsg, const std::string &frameId, const std::string &odomFrameId, const ros::Time &odomStamp, cv::Mat &scan, rtabmap::Transform &scanLocalTransform, tf::TransformListener &listener, double waitForTransform)
Definition: MsgConversion.cpp:1627
rtabmap_ros::CoreWrapper::setLogWarn
bool setLogWarn(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2084
rtabmap::Rtabmap::getPose
Transform getPose(int locationId) const
rtabmap_ros::CoreWrapper::tfThreadRunning_
bool tfThreadRunning_
Definition: CoreWrapper.h:267
UMutex::lock
int lock() const
rtabmap_ros::CoreWrapper::publishMapDataSrv_
ros::ServiceServer publishMapDataSrv_
Definition: CoreWrapper.h:253
rtabmap_ros::CoreWrapper::goalDoneCb
void goalDoneCb(const actionlib::SimpleClientGoalState &state, const move_base_msgs::MoveBaseResultConstPtr &result)
Definition: CoreWrapper.cpp:2777
UTimer::elapsed
double elapsed()
f
f
rtabmap_ros::CoreWrapper::odomDefaultLinVariance_
double odomDefaultLinVariance_
Definition: CoreWrapper.h:202
rtabmap_ros::CoreWrapper::setLogWarnSrv_
ros::ServiceServer setLogWarnSrv_
Definition: CoreWrapper.h:246
uReplaceChar
std::string uReplaceChar(const std::string &str, char before, char after)
rtabmap_ros::CoreWrapper::defaultSub_
image_transport::Subscriber defaultSub_
Definition: CoreWrapper.h:270
MAX
#define MAX(A, B)
rtabmap::Statistics::getSignatures
const std::map< int, Signature > & getSignatures() const
rtabmap_ros::CoreWrapper::resetSrv_
ros::ServiceServer resetSrv_
Definition: CoreWrapper.h:237
rtabmap_ros::mapGraphToROS
void mapGraphToROS(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, rtabmap::Link > &links, const rtabmap::Transform &mapToOdom, rtabmap_ros::MapGraph &msg)
Definition: MsgConversion.cpp:756
rtabmap::Rtabmap::computePath
bool computePath(int targetNode, bool global)
rtabmap_ros::CoreWrapper::getProbMapCallback
bool getProbMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: CoreWrapper.cpp:2196
rtabmap_ros::CoreWrapper::parameters_
rtabmap::ParametersMap parameters_
Definition: CoreWrapper.h:191
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
rtabmap_ros::CoreWrapper::createIntermediateNodes_
bool createIntermediateNodes_
Definition: CoreWrapper.h:289
rtabmap_ros::odomInfoFromROS
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo &msg)
Definition: MsgConversion.cpp:1070
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
rtabmap_ros::CoreWrapper::publishCurrentGoal
void publishCurrentGoal(const ros::Time &stamp)
Definition: CoreWrapper.cpp:2730
rtabmap_ros::CoreWrapper::saveParameters
void saveParameters(const std::string &configFile)
Definition: CoreWrapper.cpp:699
rtabmap_ros::CoreWrapper::odomSensorSync_
bool odomSensorSync_
Definition: CoreWrapper.h:287
rtabmap_ros::CoreWrapper::resetRtabmapCallback
bool resetRtabmapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:1965
rtabmap::Transform::rotation
Transform rotation() const
ros::NodeHandle::deleteParam
bool deleteParam(const std::string &key) const
ParametersPair
std::pair< std::string, std::string > ParametersPair
rtabmap_ros::CoreWrapper::~CoreWrapper
virtual ~CoreWrapper()
Definition: CoreWrapper.cpp:648
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
rtabmap_ros::CoreWrapper::getMapSrv_
ros::ServiceServer getMapSrv_
Definition: CoreWrapper.h:250
rtabmap_ros::CoreWrapper::setModeMappingCallback
bool setModeMappingCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2061
rtabmap_ros::CoreWrapper::getGridMapSrv_
ros::ServiceServer getGridMapSrv_
Definition: CoreWrapper.h:252
SYNC_DEL
#define SYNC_DEL(PREFIX)
Definition: CommonDataSubscriberDefines.h:103
rtabmap_ros::CoreWrapper::rtabmap_
rtabmap::Rtabmap rtabmap_
Definition: CoreWrapper.h:182
rtabmap::Rtabmap::setInitialPose
void setInitialPose(const Transform &initialPose)
rtabmap::Rtabmap::labelLocation
bool labelLocation(int id, const std::string &label)
octomap_msgs::fullMapToMsg
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
rtabmap_ros::CoreWrapper::goalFeedbackCb
void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)
Definition: CoreWrapper.cpp:2828
MapsManager::getOctomap
const rtabmap::OctoMap * getOctomap() const
Definition: MapsManager.h:82
rtabmap_ros::CoreWrapper::waitForTransform_
bool waitForTransform_
Definition: CoreWrapper.h:203
rtabmap_ros::transformToGeometryMsg
void transformToGeometryMsg(const rtabmap::Transform &transform, geometry_msgs::Transform &msg)
Definition: MsgConversion.cpp:69
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
rtabmap::util2d::depthFromDisparity
cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat &disparity, float fx, float baseline, int type=CV_32FC1)
rtabmap_ros::CoreWrapper::publishLocalPath
void publishLocalPath(const ros::Time &stamp)
Definition: CoreWrapper.cpp:2834
UFile::erase
static int erase(const std::string &filePath)
nodelet::Nodelet::getName
const std::string & getName() const
rtabmap_ros::CoreWrapper::localizationPosePub_
ros::Publisher localizationPosePub_
Definition: CoreWrapper.h:222
rtabmap_ros::CoreWrapper::publishMapCallback
bool publishMapCallback(rtabmap_ros::PublishMap::Request &, rtabmap_ros::PublishMap::Response &)
Definition: CoreWrapper.cpp:2229
UFile::copy
void copy(const std::string &to)
rtabmap_ros::CoreWrapper::lastPublishedMetricGoal_
rtabmap::Transform lastPublishedMetricGoal_
Definition: CoreWrapper.h:189
rtabmap_ros::CoreWrapper::listLabelsCallback
bool listLabelsCallback(rtabmap_ros::ListLabels::Request &req, rtabmap_ros::ListLabels::Response &res)
Definition: CoreWrapper.cpp:2568
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
MapsManager::getGridProbMap
cv::Mat getGridProbMap(float &xMin, float &yMin, float &gridCellSize)
Definition: MapsManager.cpp:1355
rtabmap_ros::CoreWrapper::infoPub_
ros::Publisher infoPub_
Definition: CoreWrapper.h:217
rtabmap::SensorData::gridCellSize
float gridCellSize() const
rtabmap_ros::CoreWrapper::getProbMapSrv_
ros::ServiceServer getProbMapSrv_
Definition: CoreWrapper.h:251
rtabmap_ros::CoreWrapper::genScan_
bool genScan_
Definition: CoreWrapper.h:207
rtabmap::LaserScan::is2d
bool is2d() const
rtabmap::SensorData::setId
void setId(int id)
rtabmap_ros::CoreWrapper
Definition: CoreWrapper.h:78
rtabmap_ros::CoreWrapper::cancelGoalCallback
bool cancelGoalCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: CoreWrapper.cpp:2517
rtabmap_ros::CoreWrapper::getPlanCallback
bool getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &res)
Definition: CoreWrapper.cpp:2433
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
rtabmap::Rtabmap::getGoalReachedRadius
float getGoalReachedRadius() const
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::Memory::getDatabaseVersion
std::string getDatabaseVersion() const
rtabmap_ros::CoreWrapper::setLabelCallback
bool setLabelCallback(rtabmap_ros::SetLabel::Request &req, rtabmap_ros::SetLabel::Response &res)
Definition: CoreWrapper.cpp:2541
rtabmap_ros::CoreWrapper::lastPoseIntermediate_
bool lastPoseIntermediate_
Definition: CoreWrapper.h:186
rtabmap::Rtabmap::getPathNextPoses
std::vector< std::pair< int, Transform > > getPathNextPoses() const
rtabmap_ros::CoreWrapper::process
void process(const ros::Time &stamp, const rtabmap::SensorData &data, const rtabmap::Transform &odom=rtabmap::Transform(), const std::string &odomFrameId="", const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const rtabmap::OdometryInfo &odomInfo=rtabmap::OdometryInfo())
Definition: CoreWrapper.cpp:1458
rtabmap_ros
Definition: CommonDataSubscriber.h:55
rtabmap_ros::CoreWrapper::useActionForGoal_
bool useActionForGoal_
Definition: CoreWrapper.h:205
ULogger::setLevel
static void setLevel(ULogger::Level level)
rtabmap_ros::CoreWrapper::commonDepthCallbackImpl
void commonDepthCallbackImpl(const std::string &odomFrameId, const rtabmap_ros::UserDataConstPtr &userDataMsg, const std::vector< cv_bridge::CvImageConstPtr > &imageMsgs, const std::vector< cv_bridge::CvImageConstPtr > &depthMsgs, const std::vector< sensor_msgs::CameraInfo > &cameraInfoMsgs, const sensor_msgs::LaserScanConstPtr &scan2dMsg, const sensor_msgs::PointCloud2ConstPtr &scan3dMsg, const rtabmap_ros::OdomInfoConstPtr &odomInfoMsg)
Definition: CoreWrapper.cpp:1042
ROS_WARN
#define ROS_WARN(...)
rtabmap::Rtabmap::getGraph
void getGraph(std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global, std::map< int, Signature > *signatures=0)
rtabmap_ros::CoreWrapper::databasePath_
std::string databasePath_
Definition: CoreWrapper.h:200
rtabmap_ros::getTransform
rtabmap::Transform getTransform(const std::string &fromFrameId, const std::string &toFrameId, const ros::Time &stamp, tf::TransformListener &listener, double waitForTransform)
Definition: MsgConversion.cpp:1220
rtabmap_ros::CoreWrapper::resumeRtabmapCallback
bool resumeRtabmapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2000
octomap_msgs::binaryMapToMsg
static bool binaryMapToMsg(const OctomapT &octomap, Octomap &msg)
rtabmap_ros::CoreWrapper::odomUpdate
bool odomUpdate(const nav_msgs::OdometryConstPtr &odomMsg, ros::Time stamp)
Definition: CoreWrapper.cpp:829
uBool2Str
std::string uBool2Str(bool boolean)
rtabmap_ros::CoreWrapper::setLogDebug
bool setLogDebug(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2072
rtabmap::Rtabmap::getLocalOptimizedPoses
const std::map< int, Transform > & getLocalOptimizedPoses() const
rtabmap_ros::CoreWrapper::scanCloudMaxPoints_
int scanCloudMaxPoints_
Definition: CoreWrapper.h:210
rtabmap_ros::CoreWrapper::commonDepthCallback
virtual void commonDepthCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_ros::UserDataConstPtr &userDataMsg, const std::vector< cv_bridge::CvImageConstPtr > &imageMsgs, const std::vector< cv_bridge::CvImageConstPtr > &depthMsgs, const std::vector< sensor_msgs::CameraInfo > &cameraInfoMsgs, const sensor_msgs::LaserScanConstPtr &scanMsg, const sensor_msgs::PointCloud2ConstPtr &scan3dMsg, const rtabmap_ros::OdomInfoConstPtr &odomInfoMsg)
Definition: CoreWrapper.cpp:980
rtabmap_ros::CoreWrapper::nextMetricGoalPub_
ros::Publisher nextMetricGoalPub_
Definition: CoreWrapper.h:228
rtabmap::Rtabmap::isIDsGenerated
bool isIDsGenerated() const
rtabmap::Transform::isIdentity
bool isIdentity() const
rtabmap_ros::CoreWrapper::initialPoseCallback
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
Definition: CoreWrapper.cpp:1750
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
boost::shared_ptr
rtabmap_ros::CoreWrapper::initialPoseSub_
ros::Subscriber initialPoseSub_
Definition: CoreWrapper.h:223
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
rtabmap_ros::CoreWrapper::goalCallback
void goalCallback(const geometry_msgs::PoseStampedConstPtr &msg)
Definition: CoreWrapper.cpp:1889
uIsFinite
bool uIsFinite(const T &value)
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
rtabmap_ros::CoreWrapper::commonStereoCallback
virtual void commonStereoCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_ros::UserDataConstPtr &userDataMsg, const cv_bridge::CvImageConstPtr &leftImageMsg, const cv_bridge::CvImageConstPtr &rightImageMsg, const sensor_msgs::CameraInfo &leftCamInfoMsg, const sensor_msgs::CameraInfo &rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr &scanMsg, const sensor_msgs::PointCloud2ConstPtr &scan3dMsg, const rtabmap_ros::OdomInfoConstPtr &odomInfoMsg)
Definition: CoreWrapper.cpp:1237
UASSERT
#define UASSERT(condition)
rtabmap_ros::CoreWrapper::mapDataPub_
ros::Publisher mapDataPub_
Definition: CoreWrapper.h:218
MapsManager::init
void init(ros::NodeHandle &nh, ros::NodeHandle &pnh, const std::string &name, bool usePublicNamespace)
Definition: MapsManager.cpp:76
rtabmap_ros::CoreWrapper::globalPoseAsyncCallback
void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &globalPoseMsg)
Definition: CoreWrapper.cpp:1742
rtabmap_ros::CoreWrapper::genScanMaxDepth_
double genScanMaxDepth_
Definition: CoreWrapper.h:208
rtabmap_ros::CoreWrapper::globalPoseAsyncSub_
ros::Subscriber globalPoseAsyncSub_
Definition: CoreWrapper.h:283
rtabmap::Rtabmap::getSTMSize
int getSTMSize() const
rtabmap_ros::CoreWrapper::goalSub_
ros::Subscriber goalSub_
Definition: CoreWrapper.h:226
UDirectory::currentDir
static std::string currentDir(bool trailingSeparator=false)
rtabmap_ros::CoreWrapper::odomTFUpdate
bool odomTFUpdate(const ros::Time &stamp)
Definition: CoreWrapper.cpp:927
rtabmap::StereoCameraModel::left
const CameraModel & left() const
uNumber2Str
std::string uNumber2Str(unsigned int number)
true
#define true
rtabmap::Rtabmap::close
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
rtabmap::Transform::isNull
bool isNull() const
cv_bridge.h
rtabmap_ros::CoreWrapper::covariance_
cv::Mat covariance_
Definition: CoreWrapper.h:187
message_filters::Synchronizer::registerCallback
Connection registerCallback(C &callback)
rtabmap_ros::CoreWrapper::paused_
bool paused_
Definition: CoreWrapper.h:183
image_transport::Subscriber::getTopic
std::string getTopic() const
rtabmap_ros::CoreWrapper::setModeMappingSrv_
ros::ServiceServer setModeMappingSrv_
Definition: CoreWrapper.h:243
rtabmap_ros::CoreWrapper::userDataAsyncSub_
ros::Subscriber userDataAsyncSub_
Definition: CoreWrapper.h:279
rtabmap_ros::CoreWrapper::publishGlobalPath
void publishGlobalPath(const ros::Time &stamp)
Definition: CoreWrapper.cpp:2860
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:390
rtabmap_ros::CoreWrapper::odomDefaultAngVariance_
double odomDefaultAngVariance_
Definition: CoreWrapper.h:201
rtabmap_ros::CoreWrapper::updateSrv_
ros::ServiceServer updateSrv_
Definition: CoreWrapper.h:236
rtabmap::Rtabmap::getPathCurrentGoalIndex
unsigned int getPathCurrentGoalIndex() const
rtabmap_ros::CoreWrapper::latestNodeWasReached_
bool latestNodeWasReached_
Definition: CoreWrapper.h:190
rtabmap_ros::CoreWrapper::pauseRtabmapCallback
bool pauseRtabmapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:1984
class_list_macros.h
rtabmap_ros::CoreWrapper::backupDatabase_
ros::ServiceServer backupDatabase_
Definition: CoreWrapper.h:241
rtabmap_ros::CommonDataSubscriber::setupCallbacks
void setupCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, const std::string &name)
Definition: CommonDataSubscriber.cpp:214
rtabmap_ros::CoreWrapper::setLogInfoSrv_
ros::ServiceServer setLogInfoSrv_
Definition: CoreWrapper.h:245
rtabmap_ros::mapDataToROS
void mapDataToROS(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, rtabmap::Link > &links, const std::map< int, rtabmap::Signature > &signatures, const rtabmap::Transform &mapToOdom, rtabmap_ros::MapData &msg)
Definition: MsgConversion.cpp:716
rtabmap_ros::CoreWrapper::rtabmapROSStats_
std::map< std::string, float > rtabmapROSStats_
Definition: CoreWrapper.h:192
rtabmap::Rtabmap::getWMSize
int getWMSize() const
rtabmap_ros::CoreWrapper::setGoalCallback
bool setGoalCallback(rtabmap_ros::SetGoal::Request &req, rtabmap_ros::SetGoal::Response &res)
Definition: CoreWrapper.cpp:2501
rtabmap::Memory::getSignatureIdByLabel
int getSignatureIdByLabel(const std::string &label, bool lookInDatabase=true) const
rtabmap::Rtabmap::getLastLocationId
int getLastLocationId() const
rtabmap_ros::CoreWrapper::rate_
float rate_
Definition: CoreWrapper.h:288
rtabmap_ros::CoreWrapper::getGridMapCallback
bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: CoreWrapper.cpp:2157
rtabmap_ros::CoreWrapper::updateRtabmapCallback
bool updateRtabmapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:1924
rtabmap_ros::CoreWrapper::odomFrameId_
std::string odomFrameId_
Definition: CoreWrapper.h:195
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
test_prior_tf_to_pose.cov
cov
Definition: test_prior_tf_to_pose.py:14
rtabmap_ros::CoreWrapper::useSavedMap_
bool useSavedMap_
Definition: CoreWrapper.h:206
rtabmap::Rtabmap::getStatistics
const Statistics & getStatistics() const
rtabmap_ros::CoreWrapper::lastPoseStamp_
ros::Time lastPoseStamp_
Definition: CoreWrapper.h:185
rtabmap_ros::CoreWrapper::loadParameters
void loadParameters(const std::string &configFile, rtabmap::ParametersMap &parameters)
Definition: CoreWrapper.cpp:686
cv_bridge::CvImage
sensor_msgs::image_encodings::TYPE_32FC1
const std::string TYPE_32FC1
rtabmap_ros::CommonDataSubscriber::isDataSubscribed
bool isDataSubscribed() const
Definition: CommonDataSubscriber.h:68
sensor_msgs::image_encodings::MONO8
const std::string MONO8
rtabmap::Rtabmap::process
bool process(const SensorData &data, Transform odomPose, const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >())
sensor_msgs::image_encodings::TYPE_16UC1
const std::string TYPE_16UC1
rtabmap_ros::CoreWrapper::setLabelSrv_
ros::ServiceServer setLabelSrv_
Definition: CoreWrapper.h:257
rtabmap::OctoMap::octree
const RtabmapColorOcTree * octree() const
MapsManager::getOccupancyGrid
const rtabmap::OccupancyGrid * getOccupancyGrid() const
Definition: MapsManager.h:83
rtabmap_ros::CommonDataSubscriber::getQueueSize
int getQueueSize() const
Definition: CommonDataSubscriber.h:70
rtabmap_ros::CoreWrapper::mbClient_
MoveBaseClient mbClient_
Definition: CoreWrapper.h:264
rtabmap_ros::CoreWrapper::tfBroadcaster_
tf2_ros::TransformBroadcaster tfBroadcaster_
Definition: CoreWrapper.h:233
rtabmap_ros::CoreWrapper::triggerNewMapCallback
bool triggerNewMapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2016
rtabmap_ros::CommonDataSubscriber::isApproxSync
bool isApproxSync() const
Definition: CommonDataSubscriber.h:71
rtabmap::Memory::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
rtabmap_ros::CoreWrapper::maxMappingNodes_
int maxMappingNodes_
Definition: CoreWrapper.h:290
ParametersMap
QMap< QString, QVariant > ParametersMap
rtabmap_ros::CoreWrapper::rgbOdomSub_
message_filters::Subscriber< nav_msgs::Odometry > rgbOdomSub_
Definition: CoreWrapper.h:274
rtabmap::Rtabmap::getTimeThreshold
float getTimeThreshold() const
rtabmap_ros::CoreWrapper::mapToOdomMutex_
boost::mutex mapToOdomMutex_
Definition: CoreWrapper.h:213
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
uStr2Bool
bool uStr2Bool(const char *str)
rtabmap_ros::CoreWrapper::cancelGoalSrv_
ros::ServiceServer cancelGoalSrv_
Definition: CoreWrapper.h:256
sensor_msgs::image_encodings::MONO16
const std::string MONO16
rtabmap_ros::convertStereoMsg
bool convertStereoMsg(const cv_bridge::CvImageConstPtr &leftImageMsg, const cv_bridge::CvImageConstPtr &rightImageMsg, const sensor_msgs::CameraInfo &leftCamInfoMsg, const sensor_msgs::CameraInfo &rightCamInfoMsg, const std::string &frameId, const std::string &odomFrameId, const ros::Time &odomStamp, cv::Mat &left, cv::Mat &right, rtabmap::StereoCameraModel &stereoModel, tf::TransformListener &listener, double waitForTransform)
Definition: MsgConversion.cpp:1456
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
util3d_transforms.h
rtabmap_ros::CoreWrapper::tfListener_
tf::TransformListener tfListener_
Definition: CoreWrapper.h:234
rtabmap_ros::CoreWrapper::setModeLocalizationCallback
bool setModeLocalizationCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2050
rtabmap::Rtabmap::init
void init(const ParametersMap &parameters, const std::string &databasePath="")
uValues
std::list< V > uValues(const std::multimap< K, V > &mm)
rtabmap_ros::CoreWrapper::goalNodeCallback
void goalNodeCallback(const rtabmap_ros::GoalConstPtr &msg)
Definition: CoreWrapper.cpp:1914
rtabmap_ros::transformFromPoseMsg
rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose &msg)
Definition: MsgConversion.cpp:116
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
rtabmap_ros::CoreWrapper::getProjMapCallback
bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: CoreWrapper.cpp:2139
uContains
bool uContains(const std::list< V > &list, const V &value)
rtabmap::SensorData::stamp
double stamp() const
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
rtabmap::Rtabmap::parseParameters
void parseParameters(const ParametersMap &parameters)
rtabmap_ros::userDataFromROS
cv::Mat userDataFromROS(const rtabmap_ros::UserData &dataMsg)
Definition: MsgConversion.cpp:1174
rtabmap_ros::CoreWrapper::mapToOdom_
rtabmap::Transform mapToOdom_
Definition: CoreWrapper.h:212
ros::Rate::sleep
bool sleep()
rtabmap_ros::transformToPoseMsg
void transformToPoseMsg(const rtabmap::Transform &transform, geometry_msgs::Pose &msg)
Definition: MsgConversion.cpp:104
image_transport::SubscriberFilter::subscribe
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
BAD_COVARIANCE
#define BAD_COVARIANCE
Definition: CoreWrapper.cpp:69
false
#define false
rtabmap::Memory::getLastSignatureId
int getLastSignatureId() const
rtabmap::Statistics::poses
const std::map< int, Transform > & poses() const
rtabmap_ros::CoreWrapper::stereoToDepth_
bool stereoToDepth_
Definition: CoreWrapper.h:286
MapsManager::publishMaps
void publishMaps(const std::map< int, rtabmap::Transform > &poses, const ros::Time &stamp, const std::string &mapFrameId)
Definition: MapsManager.cpp:769
transform_to_tf.TransformStamped
TransformStamped
Definition: transform_to_tf.py:31
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
actionlib::SimpleActionClient::sendGoal
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
NODELET_INFO
#define NODELET_INFO(...)
rtabmap_ros::CoreWrapper::getMapCallback
bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: CoreWrapper.cpp:2163
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
rtabmap::Signature::sensorData
SensorData & sensorData()
rtabmap::Rtabmap::getPathStatus
int getPathStatus() const
message_filters::Subscriber::subscribe
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
rtabmap_ros::CoreWrapper::mapsManager_
MapsManager mapsManager_
Definition: CoreWrapper.h:215
rtabmap_ros::CoreWrapper::configPath_
std::string configPath_
Definition: CoreWrapper.h:199
UFile::exists
bool exists()
rtabmap::Rtabmap::getPath
const std::vector< std::pair< int, Transform > > & getPath() const
rtabmap::Memory::getLastWorkingSignature
const Signature * getLastWorkingSignature() const
rtabmap::Memory::getAllLabels
std::map< int, std::string > getAllLabels() const
image_transport::TransportHints
UMutex::unlock
int unlock() const
rtabmap::Statistics::constraints
const std::multimap< int, Link > & constraints() const
MapsManager::hasSubscribers
bool hasSubscribers() const
Definition: MapsManager.cpp:361
rtabmap_ros::CoreWrapper::frameId_
std::string frameId_
Definition: CoreWrapper.h:194
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
rtabmap_ros::CoreWrapper::transformThread_
boost::thread * transformThread_
Definition: CoreWrapper.h:266
rtabmap_ros::CoreWrapper::currentMetricGoal_
rtabmap::Transform currentMetricGoal_
Definition: CoreWrapper.h:188
rtabmap_ros::CoreWrapper::rgbCameraInfoSub_
message_filters::Subscriber< sensor_msgs::CameraInfo > rgbCameraInfoSub_
Definition: CoreWrapper.h:275
rtabmap_ros::CoreWrapper::rgbSub_
image_transport::SubscriberFilter rgbSub_
Definition: CoreWrapper.h:273
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
rtabmap_ros::CoreWrapper::setLogError
bool setLogError(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2090
rtabmap::Memory::load2DMap
cv::Mat load2DMap(float &xMin, float &yMin, float &cellSize) const
UWARN
#define UWARN(...)
rtabmap::Rtabmap::getMemory
const Memory * getMemory() const
rtabmap_ros::CoreWrapper::goalNodeSub_
ros::Subscriber goalNodeSub_
Definition: CoreWrapper.h:227
rtabmap_ros::CoreWrapper::listLabelsSrv_
ros::ServiceServer listLabelsSrv_
Definition: CoreWrapper.h:258
ros::Time::now
static Time now()
UTimer::ticks
double ticks()
rtabmap_ros::timestampFromROS
double timestampFromROS(const ros::Time &stamp)
Definition: MsgConversion.h:153
yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
uStr2Float
float uStr2Float(const std::string &str)
rtabmap_ros::CoreWrapper::goalActiveCb
void goalActiveCb()
Definition: CoreWrapper.cpp:2822
rtabmap_ros::CoreWrapper::labelsPub_
ros::Publisher labelsPub_
Definition: CoreWrapper.h:220
image_transport::ImageTransport
rtabmap_ros::CoreWrapper::setLogInfo
bool setLogInfo(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2078
header
const std::string header
rtabmap_ros::CoreWrapper::mapPathPub_
ros::Publisher mapPathPub_
Definition: CoreWrapper.h:221
rtabmap_ros::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
rtabmap::OdometryInfo::reg
RegistrationInfo reg
rtabmap::Rtabmap::getLastLocalizationPose
const Transform & getLastLocalizationPose() const
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat &depth32F)
MapsManager::getGridMap
cv::Mat getGridMap(float &xMin, float &yMin, float &gridCellSize)
Definition: MapsManager.cpp:1346
image_encodings.h
rtabmap_ros::CoreWrapper::genScanMinDepth_
double genScanMinDepth_
Definition: CoreWrapper.h:209
rtabmap_ros::CoreWrapper::localPathPub_
ros::Publisher localPathPub_
Definition: CoreWrapper.h:231
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
rtabmap_ros::CoreWrapper::previousStamp_
ros::Time previousStamp_
Definition: CoreWrapper.h:292
rtabmap_ros::CoreWrapper::waitForTransformDuration_
double waitForTransformDuration_
Definition: CoreWrapper.h:204
rtabmap_ros::CoreWrapper::time_
ros::Time time_
Definition: CoreWrapper.h:291
rtabmap_ros::CoreWrapper::groundTruthFrameId_
std::string groundTruthFrameId_
Definition: CoreWrapper.h:197
rtabmap_ros::CommonDataSubscriber
Definition: CommonDataSubscriber.h:57
r
r
rtabmap::Statistics::mapCorrection
const Transform & mapCorrection() const
rtabmap_ros::CoreWrapper::backupDatabaseCallback
bool backupDatabaseCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2023
rtabmap_ros::CoreWrapper::getPlanSrv_
ros::ServiceServer getPlanSrv_
Definition: CoreWrapper.h:254
ROS_ERROR
#define ROS_ERROR(...)
rtabmap::OccupancyGrid::isGridFromDepth
bool isGridFromDepth() const
ros::NodeHandle::setParam
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
rtabmap::Rtabmap::getPathCurrentGoalId
int getPathCurrentGoalId() const
rtabmap_ros::CoreWrapper::publishStats
void publishStats(const ros::Time &stamp)
Definition: CoreWrapper.cpp:2579
rtabmap_ros::CoreWrapper::mapFrameId_
std::string mapFrameId_
Definition: CoreWrapper.h:196
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
rtabmap_ros::CoreWrapper::setGoalSrv_
ros::ServiceServer setGoalSrv_
Definition: CoreWrapper.h:255
rtabmap_ros::CoreWrapper::publishLoop
void publishLoop(double tfDelay, double tfTolerance)
Definition: CoreWrapper.cpp:717
rtabmap_ros::CoreWrapper::globalPathPub_
ros::Publisher globalPathPub_
Definition: CoreWrapper.h:230
image_transport::SubscriberFilter::getTopic
std::string getTopic() const
rtabmap::Rtabmap::getPathNextNodes
std::vector< int > getPathNextNodes() const
rtabmap_ros::CoreWrapper::groundTruthBaseFrameId_
std::string groundTruthBaseFrameId_
Definition: CoreWrapper.h:198
rtabmap_ros::CoreWrapper::setModeLocalizationSrv_
ros::ServiceServer setModeLocalizationSrv_
Definition: CoreWrapper.h:242
rtabmap_ros::CoreWrapper::goalReachedPub_
ros::Publisher goalReachedPub_
Definition: CoreWrapper.h:229
rtabmap::Rtabmap::getLocalRadius
float getLocalRadius() const
rtabmap_ros::convertRGBDMsgs
bool convertRGBDMsgs(const std::vector< cv_bridge::CvImageConstPtr > &imageMsgs, const std::vector< cv_bridge::CvImageConstPtr > &depthMsgs, const std::vector< sensor_msgs::CameraInfo > &cameraInfoMsgs, const std::string &frameId, const std::string &odomFrameId, const ros::Time &odomStamp, cv::Mat &rgb, cv::Mat &depth, std::vector< rtabmap::CameraModel > &cameraModels, tf::TransformListener &listener, double waitForTransform)
Definition: MsgConversion.cpp:1291
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
rtabmap_ros::CoreWrapper::goalCommonCallback
void goalCommonCallback(int id, const std::string &label, const rtabmap::Transform &pose, const ros::Time &stamp, double *planningTime=0)
Definition: CoreWrapper.cpp:1762
SYNC_INIT
#define SYNC_INIT(PREFIX)
Definition: CommonDataSubscriberDefines.h:98
rtabmap_ros::infoToROS
void infoToROS(const rtabmap::Statistics &stats, rtabmap_ros::Info &info)
Definition: MsgConversion.cpp:426
rtabmap_ros::CoreWrapper::triggerNewMapSrv_
ros::ServiceServer triggerNewMapSrv_
Definition: CoreWrapper.h:240
rtabmap_ros::CoreWrapper::mapGraphPub_
ros::Publisher mapGraphPub_
Definition: CoreWrapper.h:219
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
rtabmap_ros::CoreWrapper::resumeSrv_
ros::ServiceServer resumeSrv_
Definition: CoreWrapper.h:239
util3d_surface.h
MapsManager::backwardCompatibilityParameters
void backwardCompatibilityParameters(ros::NodeHandle &pnh, rtabmap::ParametersMap &parameters) const
Definition: MapsManager.cpp:233

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:03