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_slam/CoreWrapper.h"
29 
30 #include <stdio.h>
31 #include <ros/ros.h>
32 #include "pluginlib/class_list_macros.hpp"
33 
34 #include <nav_msgs/Path.h>
35 #include <std_msgs/Int32MultiArray.h>
36 #include <std_msgs/Bool.h>
37 #include <geometry_msgs/PoseArray.h>
38 #include <sensor_msgs/image_encodings.h>
39 #include <cv_bridge/cv_bridge.h>
40 #include <pcl_conversions/pcl_conversions.h>
41 #include <pcl/io/io.h>
42 
43 #include <visualization_msgs/MarkerArray.h>
44 
45 #include <rtabmap/utilite/UTimer.h>
46 #include <rtabmap/utilite/UDirectory.h>
47 #include <rtabmap/utilite/UFile.h>
48 #include <rtabmap/utilite/UConversion.h>
49 #include <rtabmap/utilite/UStl.h>
50 #include <rtabmap/utilite/UMath.h>
51 
52 #include <rtabmap/core/util2d.h>
53 #include <rtabmap/core/util3d.h>
54 #include <rtabmap/core/util3d_transforms.h>
55 #include <rtabmap/core/util3d_surface.h>
56 #include <rtabmap/core/Memory.h>
57 #include <rtabmap/core/OdometryEvent.h>
58 #include <rtabmap/core/Version.h>
59 #include <rtabmap/core/OccupancyGrid.h>
60 #include <rtabmap/core/DBDriver.h>
61 #include <rtabmap/core/Registration.h>
62 #include <rtabmap/core/Graph.h>
63 #include <rtabmap/core/LocalGridMaker.h>
64 #include <rtabmap/core/Optimizer.h>
65 
66 #ifdef WITH_OCTOMAP_MSGS
67 #ifdef RTABMAP_OCTOMAP
68 #include <octomap_msgs/conversions.h>
69 #include <rtabmap/core/OctoMap.h>
70 #endif
71 #endif
72 
73 #define BAD_COVARIANCE 9999
74 
75 //msgs
76 #include "rtabmap_msgs/Info.h"
77 #include "rtabmap_msgs/MapData.h"
78 #include "rtabmap_msgs/MapGraph.h"
79 #include "rtabmap_msgs/Path.h"
80 
81 #include "rtabmap_conversions/MsgConversion.h"
82 
83 using namespace rtabmap;
84 
85 namespace rtabmap_slam {
86 
87 CoreWrapper::CoreWrapper() :
88  CommonDataSubscriber(false),
89  paused_(false),
90  lastPose_(Transform::getIdentity()),
91  lastPoseIntermediate_(false),
92  latestNodeWasReached_(false),
93  pubLocPoseOnlyWhenLocalizing_(false),
94  graphLatched_(false),
95  frameId_("base_link"),
96  odomFrameId_(""),
97  mapFrameId_("map"),
98  groundTruthFrameId_(""), // e.g., "world"
99  groundTruthBaseFrameId_(""), // e.g., "base_link_gt"
100  configPath_(""),
101  odomDefaultAngVariance_(0.001),
102  odomDefaultLinVariance_(0.001),
103  landmarkDefaultAngVariance_(0.001),
104  landmarkDefaultLinVariance_(0.001),
105  waitForTransform_(true),
106  waitForTransformDuration_(0.2), // 200 ms
107  useActionForGoal_(false),
108  useSavedMap_(true),
109  genScan_(false),
110  genScanMaxDepth_(4.0),
111  genScanMinDepth_(0.0),
112  genDepth_(false),
113  genDepthDecimation_(1),
114  genDepthFillHolesSize_(0),
115  genDepthFillIterations_(1),
116  genDepthFillHolesError_(0.1),
117  scanCloudMaxPoints_(0),
118  scanCloudIs2d_(false),
119  mapToOdom_(rtabmap::Transform::getIdentity()),
120  transformThread_(0),
121  tfThreadRunning_(false),
122  stereoToDepth_(false),
123  interOdomSync_(0),
124  odomSensorSync_(false),
125  rate_(Parameters::defaultRtabmapDetectionRate()),
126  createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
127  mappingMaxNodes_(Parameters::defaultGridGlobalMaxNodes()),
128  mappingAltitudeDelta_(Parameters::defaultGridGlobalAltitudeDelta()),
129  alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()),
130  twoDMapping_(Parameters::defaultRegForce3DoF()),
131  previousStamp_(0),
132  mbClient_(0)
133 {
134  char * rosHomePath = getenv("ROS_HOME");
135  std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
136  databasePath_ = workingDir+"/"+rtabmap::Parameters::getDefaultDatabaseName();
137  globalPose_.header.stamp = ros::Time(0);
138 }
139 
140 void CoreWrapper::onInit()
141 {
142  ros::NodeHandle & nh = getNodeHandle();
143  ros::NodeHandle & pnh = getPrivateNodeHandle();
144 
145  mapsManager_.init(nh, pnh, getName(), true);
146 
147  bool publishTf = true;
148  std::string initialPoseStr;
149  double tfDelay = 0.05; // 20 Hz
150  double tfTolerance = 0.1; // 100 ms
151  std::string odomFrameIdInit;
152 
153  pnh.param("config_path", configPath_, configPath_);
154  pnh.param("database_path", databasePath_, databasePath_);
155 
156  pnh.param("frame_id", frameId_, frameId_);
157  pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
158  pnh.param("odom_frame_id_init", odomFrameIdInit, odomFrameIdInit); // set to publish map->odom TF before receiving odom topic
159  pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
160  pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
161  pnh.param("ground_truth_base_frame_id", groundTruthBaseFrameId_, frameId_);
162  if(pnh.hasParam("depth_cameras") && !pnh.hasParam("depth_cameras"))
163  {
164  NODELET_ERROR("\"depth_cameras\" parameter doesn't exist "
165  "anymore! It is replaced by \"rgbd_cameras\" parameter "
166  "used when \"subscribe_rgbd\" is true");
167  }
168  if(!odomFrameIdInit.empty())
169  {
170  if(odomFrameId_.empty())
171  {
172  ROS_INFO("rtabmap: odom_frame_id_init = %s", odomFrameIdInit.c_str());
173  odomFrameId_ = odomFrameIdInit;
174  }
175  else
176  {
177  ROS_WARN("odom_frame_id_init (%s) is ignored if odom_frame_id (%s) is set.", odomFrameIdInit.c_str(), odomFrameId_.c_str());
178  }
179  }
180 
181  int eventLevel = ULogger::kFatal;
182  pnh.param("log_to_rosout_level", eventLevel, eventLevel);
183  UASSERT(eventLevel >= ULogger::kDebug && eventLevel <= ULogger::kFatal);
184  ULogger::setEventLevel((ULogger::Level)eventLevel);
185 
186  pnh.param("publish_tf", publishTf, publishTf);
187  pnh.param("tf_delay", tfDelay, tfDelay);
188  if(pnh.hasParam("tf_prefix"))
189  {
190  ROS_ERROR("tf_prefix parameter has been removed, use directly map_frame_id, odom_frame_id and frame_id parameters.");
191  }
192  pnh.param("tf_tolerance", tfTolerance, tfTolerance);
193  pnh.param("odom_tf_angular_variance", odomDefaultAngVariance_, odomDefaultAngVariance_);
194  pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_);
195  pnh.param("landmark_angular_variance", landmarkDefaultAngVariance_, landmarkDefaultAngVariance_);
196  pnh.param("landmark_linear_variance", landmarkDefaultLinVariance_, landmarkDefaultLinVariance_);
197  pnh.param("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_,pubLocPoseOnlyWhenLocalizing_);
198  pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
199  pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
200  pnh.param("initial_pose", initialPoseStr, initialPoseStr);
201  pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
202  pnh.param("use_saved_map", useSavedMap_, useSavedMap_);
203  pnh.param("gen_scan", genScan_, genScan_);
204  pnh.param("gen_scan_max_depth", genScanMaxDepth_, genScanMaxDepth_);
205  pnh.param("gen_scan_min_depth", genScanMinDepth_, genScanMinDepth_);
206  pnh.param("gen_depth", genDepth_, genDepth_);
207  pnh.param("gen_depth_decimation", genDepthDecimation_, genDepthDecimation_);
208  pnh.param("gen_depth_fill_holes_size", genDepthFillHolesSize_, genDepthFillHolesSize_);
209  pnh.param("gen_depth_fill_iterations", genDepthFillIterations_, genDepthFillIterations_);
210  pnh.param("gen_depth_fill_holes_error", genDepthFillHolesError_, genDepthFillHolesError_);
211  pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
212  pnh.param("scan_cloud_is_2d", scanCloudIs2d_, scanCloudIs2d_);
213  if(pnh.hasParam("scan_cloud_normal_k"))
214  {
215  ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been removed. RTAB-Map's parameter \"%s\" should be used instead. "
216  "The value is copied. Use \"%s\" to avoid this warning.",
217  Parameters::kMemLaserScanNormalK().c_str(),
218  Parameters::kMemLaserScanNormalK().c_str());
219  double value;
220  pnh.getParam("scan_cloud_normal_k", value);
221  uInsert(parameters_, ParametersPair(Parameters::kMemLaserScanNormalK(), uNumber2Str(value)));
222  }
223  pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_);
224  pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
225  if(pnh.hasParam("flip_scan"))
226  {
227  NODELET_WARN("Parameter \"flip_scan\" doesn't exist anymore. Rtabmap now "
228  "detects automatically if the laser is upside down with /tf, then if so, it "
229  "switches scan values.");
230  }
231 
232  NODELET_INFO("rtabmap: frame_id = %s", frameId_.c_str());
233  if(!odomFrameId_.empty())
234  {
235  NODELET_INFO("rtabmap: odom_frame_id = %s", odomFrameId_.c_str());
236  }
237  if(!groundTruthFrameId_.empty())
238  {
239  NODELET_INFO("rtabmap: ground_truth_frame_id = %s -> ground_truth_base_frame_id = %s",
240  groundTruthFrameId_.c_str(),
241  groundTruthBaseFrameId_.c_str());
242  }
243  NODELET_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
244  NODELET_INFO("rtabmap: log_to_rosout_level = %d", eventLevel);
245  NODELET_INFO("rtabmap: initial_pose = %s", initialPoseStr.c_str());
246  NODELET_INFO("rtabmap: use_action_for_goal = %s", useActionForGoal_?"true":"false");
247  NODELET_INFO("rtabmap: tf_delay = %f", tfDelay);
248  NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
249  NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false");
250  NODELET_INFO("rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false");
251  bool subscribeStereo = false;
252  pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
253  if(subscribeStereo)
254  {
255  NODELET_INFO("rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false");
256  }
257 
258  NODELET_INFO("rtabmap: gen_scan = %s", genScan_?"true":"false");
259  if(genScan_)
260  {
261  NODELET_INFO("rtabmap: gen_scan_max_depth = %f", genScanMaxDepth_);
262  NODELET_INFO("rtabmap: gen_scan_min_depth = %f", genScanMinDepth_);
263  }
264 
265  NODELET_INFO("rtabmap: gen_depth = %s", genDepth_?"true":"false");
266  if(genDepth_)
267  {
268  NODELET_INFO("rtabmap: gen_depth_decimation = %d", genDepthDecimation_);
269  NODELET_INFO("rtabmap: gen_depth_fill_holes_size = %d", genDepthFillHolesSize_);
270  NODELET_INFO("rtabmap: gen_depth_fill_iterations = %d", genDepthFillIterations_);
271  NODELET_INFO("rtabmap: gen_depth_fill_holes_error = %f", genDepthFillHolesError_);
272  }
273  bool subscribeScanCloud = false;
274  pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
275  if(subscribeScanCloud)
276  {
277  NODELET_INFO("rtabmap: scan_cloud_max_points = %d", scanCloudMaxPoints_);
278  NODELET_INFO("rtabmap: scan_cloud_is_2d = %s", scanCloudIs2d_?"true":"false");
279  }
280 
281  infoPub_ = nh.advertise<rtabmap_msgs::Info>("info", 1);
282  mapDataPub_ = nh.advertise<rtabmap_msgs::MapData>("mapData", 1);
283  mapGraphPub_ = nh.advertise<rtabmap_msgs::MapGraph>("mapGraph", 1, mapsManager_.isLatching());
284  odomCachePub_ = nh.advertise<rtabmap_msgs::MapGraph>("mapOdomCache", 1);
285  landmarksPub_ = nh.advertise<geometry_msgs::PoseArray>("landmarks", 1);
286  labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
287  mapPathPub_ = nh.advertise<nav_msgs::Path>("mapPath", 1);
288  localGridObstacle_ = nh.advertise<sensor_msgs::PointCloud2>("local_grid_obstacle", 1);
289  localGridEmpty_ = nh.advertise<sensor_msgs::PointCloud2>("local_grid_empty", 1);
290  localGridGround_ = nh.advertise<sensor_msgs::PointCloud2>("local_grid_ground", 1);
291  localizationPosePub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("localization_pose", 1);
292  initialPoseSub_ = nh.subscribe("initialpose", 1, &CoreWrapper::initialPoseCallback, this);
293 
294  // planning topics
295  goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
296  goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
297  nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_out", 1);
298  goalReachedPub_ = nh.advertise<std_msgs::Bool>("goal_reached", 1);
299  globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
300  localPathPub_ = nh.advertise<nav_msgs::Path>("local_path", 1);
301  globalPathNodesPub_ = nh.advertise<rtabmap_msgs::Path>("global_path_nodes", 1);
302  localPathNodesPub_ = nh.advertise<rtabmap_msgs::Path>("local_path_nodes", 1);
303 
304  ros::Publisher nextMetricGoal_;
305  ros::Publisher goalReached_;
306  ros::Publisher path_;
307 
308  configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
309  databasePath_ = uReplaceChar(databasePath_, '~', UDirectory::homeDir());
310 #ifndef _WIN32
311  if(configPath_.size() && configPath_.at(0) != '/')
312  {
313  configPath_ = UDirectory::currentDir(true) + configPath_;
314  }
315  if(databasePath_.size() && databasePath_.at(0) != '/')
316  {
317  databasePath_ = UDirectory::currentDir(true) + databasePath_;
318  }
319 #endif
320 
321  ParametersMap allParameters = Parameters::getDefaultParameters();
322  // remove Odom parameters
323  for(ParametersMap::iterator iter=allParameters.begin(); iter!=allParameters.end();)
324  {
325  if(iter->first.find("Odom") == 0)
326  {
327  allParameters.erase(iter++);
328  }
329  else
330  {
331  ++iter;
332  }
333  }
334  uInsert(allParameters, ParametersPair(Parameters::kRGBDCreateOccupancyGrid(), "true")); // default true in ROS
335  char * rosHomePath = getenv("ROS_HOME");
336  std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros";
337  uInsert(allParameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), workingDir)); // change default to ~/.ros
338 
339  // load parameters
340  loadParameters(configPath_, parameters_);
341  for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end();)
342  {
343  if(iter->first.find("Odom") == 0)
344  {
345  parameters_.erase(iter++);
346  }
347  else
348  {
349  ++iter;
350  }
351  }
352 
353  // update parameters with user input parameters (private)
354  for(ParametersMap::iterator iter=allParameters.begin(); iter!=allParameters.end(); ++iter)
355  {
356  std::string vStr;
357  bool vBool;
358  int vInt;
359  double vDouble;
360  if(pnh.getParam(iter->first, vStr))
361  {
362  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
363 
364  if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
365  {
366  vStr = uReplaceChar(vStr, '~', UDirectory::homeDir());
367  }
368  else if(iter->first.compare(Parameters::kKpDictionaryPath()) == 0)
369  {
370  vStr = uReplaceChar(vStr, '~', UDirectory::homeDir());
371  }
372  uInsert(parameters_, ParametersPair(iter->first, vStr));
373  }
374  else if(pnh.getParam(iter->first, vBool))
375  {
376  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
377  uInsert(parameters_, ParametersPair(iter->first, uBool2Str(vBool)));
378  }
379  else if(pnh.getParam(iter->first, vDouble))
380  {
381  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
382  uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vDouble)));
383  }
384  else if(pnh.getParam(iter->first, vInt))
385  {
386  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
387  uInsert(parameters_, ParametersPair(iter->first, uNumber2Str(vInt)));
388  }
389  }
390 
391  //parse input arguments
392  std::vector<std::string> argList = getMyArgv();
393  char ** argv = new char*[argList.size()];
394  bool deleteDbOnStart = false;
395  for(unsigned int i=0; i<argList.size(); ++i)
396  {
397  argv[i] = &argList[i].at(0);
398  if(strcmp(argv[i], "--delete_db_on_start") == 0 || strcmp(argv[i], "-d") == 0)
399  {
400  deleteDbOnStart = true;
401  }
402  }
403  rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argList.size(), argv);
404  delete [] argv;
405  for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
406  {
407  uInsert(parameters_, ParametersPair(iter->first, iter->second));
408  NODELET_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
409  }
410 
411  // Backward compatibility
412  for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
413  iter!=Parameters::getRemovedParameters().end();
414  ++iter)
415  {
416  std::string vStr;
417  bool vBool;
418  int vInt;
419  double vDouble;
420  std::string paramValue;
421  if(pnh.getParam(iter->first, vStr))
422  {
423  paramValue = vStr;
424  }
425  else if(pnh.getParam(iter->first, vBool))
426  {
427  paramValue = uBool2Str(vBool);
428  }
429  else if(pnh.getParam(iter->first, vDouble))
430  {
431  paramValue = uNumber2Str(vDouble);
432  }
433  else if(pnh.getParam(iter->first, vInt))
434  {
435  paramValue = uNumber2Str(vInt);
436  }
437  if(!paramValue.empty())
438  {
439  if(!iter->second.second.empty() && parameters_.find(iter->second.second)!=parameters_.end())
440  {
441  NODELET_WARN("Rtabmap: Parameter name changed: \"%s\" -> \"%s\". The new parameter is already used with value \"%s\", ignoring the old one with value \"%s\".",
442  iter->first.c_str(), iter->second.second.c_str(), parameters_.find(iter->second.second)->second.c_str(), paramValue.c_str());
443  }
444  else if(iter->second.first)
445  {
446  // can be migrated
447  uInsert(parameters_, ParametersPair(iter->second.second, paramValue));
448  NODELET_WARN("Rtabmap: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
449  iter->first.c_str(), iter->second.second.c_str(), paramValue.c_str());
450  }
451  else
452  {
453  if(iter->second.second.empty())
454  {
455  NODELET_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore!",
456  iter->first.c_str());
457  }
458  else
459  {
460  NODELET_ERROR("Rtabmap: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
461  iter->first.c_str(), iter->second.second.c_str());
462  }
463  }
464  }
465  }
466 
467  // Backward compatibility (MapsManager)
468  mapsManager_.backwardCompatibilityParameters(pnh, parameters_);
469 
470  bool subscribeScan2d = false;
471  bool subscribeScan3d = false;
472  pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d);
473  pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
474  int gridSensor = Parameters::defaultGridSensor();
475  if((subscribeScan2d || subscribeScan3d || genScan_) && parameters_.find(Parameters::kGridSensor()) == parameters_.end())
476  {
477  NODELET_WARN("Setting \"%s\" parameter to 0 (default 1) as \"subscribe_scan\", \"subscribe_scan_cloud\" or \"gen_scan\" is "
478  "true. The occupancy grid map will be constructed from "
479  "laser scans. To get occupancy grid map from camera's cloud projection, set \"%s\" "
480  "to 1. To suppress this warning, "
481  "add <param name=\"%s\" type=\"string\" value=\"0\"/>",
482  Parameters::kGridSensor().c_str(),
483  Parameters::kGridSensor().c_str(),
484  Parameters::kGridSensor().c_str());
485  parameters_.insert(ParametersPair(Parameters::kGridSensor(), "0"));
486  }
487  Parameters::parse(parameters_, Parameters::kGridSensor(), gridSensor);
488  if((subscribeScan2d || subscribeScan3d || genScan_) && parameters_.find(Parameters::kGridRangeMax()) == parameters_.end() && gridSensor==0)
489  {
490  NODELET_INFO("Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan\", \"subscribe_scan_cloud\" or \"gen_scan\" is true and %s is 0.",
491  Parameters::kGridRangeMax().c_str(),
492  Parameters::defaultGridRangeMax(),
493  Parameters::kGridSensor().c_str());
494  parameters_.insert(ParametersPair(Parameters::kGridRangeMax(), "0"));
495  }
496  if(subscribeScan3d && !scanCloudIs2d_ && parameters_.find(Parameters::kIcpPointToPlaneRadius()) == parameters_.end())
497  {
498  NODELET_INFO("Setting \"%s\" parameter to 0 (default %f) as \"subscribe_scan_cloud\" is true.",
499  Parameters::kIcpPointToPlaneRadius().c_str(),
500  Parameters::defaultIcpPointToPlaneRadius());
501  parameters_.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), "0"));
502  }
503  int regStrategy = Parameters::defaultRegStrategy();
504  Parameters::parse(parameters_, Parameters::kRegStrategy(), regStrategy);
505  if(parameters_.find(Parameters::kRGBDProximityPathMaxNeighbors()) == parameters_.end() &&
506  (regStrategy == Registration::kTypeIcp || regStrategy == Registration::kTypeVisIcp))
507  {
508  if(subscribeScan2d || (subscribeScan3d && scanCloudIs2d_))
509  {
510  NODELET_WARN("Setting \"%s\" parameter to 10 (default 0) as \"%s\" is "
511  "true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
512  "scans. To disable, set \"%s\" to 0. To suppress this warning, "
513  "add <param name=\"%s\" type=\"string\" value=\"10\"/>",
514  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
515  subscribeScan2d?"subscribe_scan":"scan_cloud_is_2d",
516  Parameters::kRegStrategy().c_str(),
517  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
518  Parameters::kRGBDProximityPathMaxNeighbors().c_str());
519  parameters_.insert(ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "10"));
520  }
521  else if(subscribeScan3d)
522  {
523  NODELET_WARN("Setting \"%s\" parameter to 1 (default 0) as \"subscribe_scan_cloud\" is "
524  "true and \"%s\" uses ICP. To disable, set \"%s\" to 0. To suppress this warning, "
525  "add <param name=\"%s\" type=\"string\" value=\"1\"/>",
526  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
527  Parameters::kRegStrategy().c_str(),
528  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
529  Parameters::kRGBDProximityPathMaxNeighbors().c_str());
530  parameters_.insert(ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "1"));
531  }
532  }
533 
534  int estimationType = Parameters::defaultVisEstimationType();
535  Parameters::parse(parameters_, Parameters::kVisEstimationType(), estimationType);
536  int cameras = 0;
537  bool subscribeRGBD = false;
538  pnh.param("rgbd_cameras", cameras, cameras);
539  pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
540 
541  // modify default parameters with those in the database
542  if(!deleteDbOnStart)
543  {
544  ParametersMap dbParameters;
545  rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
546  if(driver->openConnection(databasePath_))
547  {
548  dbParameters = driver->getLastParameters(); // parameter migration is already done
549  }
550  delete driver;
551  for(ParametersMap::iterator iter=dbParameters.begin(); iter!=dbParameters.end(); ++iter)
552  {
553  if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
554  {
555  // ignore working directory
556  continue;
557  }
558  if(parameters_.find(iter->first) == parameters_.end() &&
559  allParameters.find(iter->first) != allParameters.end() &&
560  allParameters.find(iter->first)->second.compare(iter->second) !=0)
561  {
562  NODELET_INFO("Update RTAB-Map parameter \"%s\"=\"%s\" from database", iter->first.c_str(), iter->second.c_str());
563  parameters_.insert(*iter);
564  }
565  }
566  }
567  ParametersMap modifiedParameters = parameters_;
568  // Add all other parameters (not copied if already exists)
569  parameters_.insert(allParameters.begin(), allParameters.end());
570 
571  if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end())
572  {
573  Parameters::parse(parameters_, Parameters::kRtabmapDetectionRate(), rate_);
574  NODELET_INFO("RTAB-Map detection rate = %f Hz", rate_);
575  }
576  if(parameters_.find(Parameters::kRtabmapCreateIntermediateNodes()) != parameters_.end())
577  {
578  Parameters::parse(parameters_, Parameters::kRtabmapCreateIntermediateNodes(), createIntermediateNodes_);
579  if(createIntermediateNodes_)
580  {
581  NODELET_INFO("Create intermediate nodes");
582  if(rate_ == 0.0f)
583  {
584  bool interOdomInfo = false;
585  pnh.getParam("subscribe_inter_odom_info", interOdomInfo);
586  if(interOdomInfo)
587  {
588  NODELET_INFO("Subscribe to inter odom + info messages");
589  interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(100), interOdomSyncSub_, interOdomInfoSyncSub_);
590  interOdomSync_->registerCallback(boost::bind(&CoreWrapper::interOdomInfoCallback, this, boost::placeholders::_1, boost::placeholders::_2));
591  interOdomSyncSub_.subscribe(nh, "inter_odom", 100);
592  interOdomInfoSyncSub_.subscribe(nh, "inter_odom_info", 100);
593  }
594  else
595  {
596  NODELET_INFO("Subscribe to inter odom messages");
597  interOdomSub_ = nh.subscribe("inter_odom", 100, &CoreWrapper::interOdomCallback, this);
598  }
599 
600  }
601  }
602  }
603  if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
604  {
605  Parameters::parse(parameters_, Parameters::kGridGlobalMaxNodes(), mappingMaxNodes_);
606  if(mappingMaxNodes_>0)
607  {
608  NODELET_INFO("Max mapping nodes = %d", mappingMaxNodes_);
609  }
610  }
611  if(parameters_.find(Parameters::kGridGlobalAltitudeDelta()) != parameters_.end())
612  {
613  Parameters::parse(parameters_, Parameters::kGridGlobalAltitudeDelta(), mappingAltitudeDelta_);
614  if(mappingAltitudeDelta_>0.0)
615  {
616  NODELET_INFO("Mapping altitude delta = %f", mappingAltitudeDelta_);
617  }
618  }
619  if(parameters_.find(Parameters::kRtabmapImagesAlreadyRectified()) != parameters_.end())
620  {
621  Parameters::parse(parameters_, Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectifiedImages_);
622  }
623  if(parameters_.find(Parameters::kRegForce3DoF()) != parameters_.end())
624  {
625  Parameters::parse(parameters_, Parameters::kRegForce3DoF(), twoDMapping_);
626  }
627 
628  paused_ = pnh.param("is_rtabmap_paused", paused_);
629  if(paused_)
630  {
631  NODELET_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
632  }
633 
634  if(deleteDbOnStart)
635  {
636  if(UFile::erase(databasePath_) == 0)
637  {
638  NODELET_INFO("rtabmap: Deleted database \"%s\" (--delete_db_on_start or -d are set).", databasePath_.c_str());
639  }
640  }
641 
642  if(databasePath_.size())
643  {
644  NODELET_INFO("rtabmap: Using database from \"%s\" (%ld MB).", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
645  }
646  else
647  {
648  NODELET_INFO("rtabmap: database_path parameter not set, the map will not be saved.");
649  }
650 
651  mapsManager_.setParameters(parameters_);
652 
653  // Init RTAB-Map
654  rtabmap_.init(parameters_, databasePath_);
655 
656  if(rtabmap_.getMemory())
657  {
658  if(useSavedMap_)
659  {
660  float xMin, yMin, gridCellSize;
661  cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
662  if(!map.empty())
663  {
664  NODELET_INFO("rtabmap: 2D occupancy grid map loaded (%dx%d).", map.cols, map.rows);
665  mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
666  }
667  }
668 
669  if(rtabmap_.getMemory()->getWorkingMem().size()>1)
670  {
671  NODELET_INFO("rtabmap: Working Memory = %d, Local map = %d.",
672  (int)rtabmap_.getMemory()->getWorkingMem().size()-1,
673  (int)rtabmap_.getLocalOptimizedPoses().size());
674  }
675 
676  if(databasePath_.size())
677  {
678  NODELET_INFO("rtabmap: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str());
679  }
680 
681  if(rtabmap_.getMemory()->isIncremental())
682  {
683  NODELET_INFO("rtabmap: SLAM mode (%s=true)", Parameters::kMemIncrementalMemory().c_str());
684  }
685  else
686  {
687  NODELET_INFO("rtabmap: Localization mode (%s=false)", Parameters::kMemIncrementalMemory().c_str());
688  }
689  }
690 
691  // setup services
692  updateSrv_ = nh.advertiseService("update_parameters", &CoreWrapper::updateRtabmapCallback, this);
693  resetSrv_ = nh.advertiseService("reset", &CoreWrapper::resetRtabmapCallback, this);
694  pauseSrv_ = nh.advertiseService("pause", &CoreWrapper::pauseRtabmapCallback, this);
695  resumeSrv_ = nh.advertiseService("resume", &CoreWrapper::resumeRtabmapCallback, this);
696  loadDatabaseSrv_ = nh.advertiseService("load_database", &CoreWrapper::loadDatabaseCallback, this);
697  triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
698  backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this);
699  detectMoreLoopClosuresSrv_ = nh.advertiseService("detect_more_loop_closures", &CoreWrapper::detectMoreLoopClosuresCallback, this);
700  globalBundleAdjustmentSrv_ = nh.advertiseService("global_bundle_adjustment", &CoreWrapper::globalBundleAdjustmentCallback, this);
701  cleanupLocalGridsSrv_ = nh.advertiseService("cleanup_local_grids", &CoreWrapper::cleanupLocalGridsCallback, this);
702  setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
703  setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
704  getNodeDataSrv_ = nh.advertiseService("get_node_data", &CoreWrapper::getNodeDataCallback, this);
705  getMapDataSrv_ = nh.advertiseService("get_map_data", &CoreWrapper::getMapDataCallback, this);
706  getMapData2Srv_ = nh.advertiseService("get_map_data2", &CoreWrapper::getMapData2Callback, this);
707  getMapSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
708  getProbMapSrv_ = nh.advertiseService("get_prob_map", &CoreWrapper::getProbMapCallback, this);
709  getGridMapSrv_ = nh.advertiseService("get_grid_map", &CoreWrapper::getGridMapCallback, this);
710  getProjMapSrv_ = nh.advertiseService("get_proj_map", &CoreWrapper::getProjMapCallback, this);
711  publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
712  getPlanSrv_ = nh.advertiseService("get_plan", &CoreWrapper::getPlanCallback, this);
713  getPlanNodesSrv_ = nh.advertiseService("get_plan_nodes", &CoreWrapper::getPlanNodesCallback, this);
714  setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
715  cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
716  setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
717  listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
718  removeLabelSrv_ = nh.advertiseService("remove_label", &CoreWrapper::removeLabelCallback, this);
719  addLinkSrv_ = nh.advertiseService("add_link", &CoreWrapper::addLinkCallback, this);
720  getNodesInRadiusSrv_ = nh.advertiseService("get_nodes_in_radius", &CoreWrapper::getNodesInRadiusCallback, this);
721 #ifdef WITH_OCTOMAP_MSGS
722 #ifdef RTABMAP_OCTOMAP
723  octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
724  octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
725 #endif
726 #endif
727  //private services
728  setLogDebugSrv_ = pnh.advertiseService("log_debug", &CoreWrapper::setLogDebug, this);
729  setLogInfoSrv_ = pnh.advertiseService("log_info", &CoreWrapper::setLogInfo, this);
730  setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this);
731  setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this);
732 
733  int optimizeIterations = 0;
734  Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
735  if(publishTf && optimizeIterations != 0)
736  {
737  tfThreadRunning_ = true;
738  transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay, tfTolerance));
739  }
740  else if(publishTf)
741  {
742  NODELET_WARN("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.",
743  Parameters::kOptimizerIterations().c_str(), mapFrameId_.c_str());
744  }
745 
746  std::vector<diagnostic_updater::DiagnosticTask*> tasks;
747  double localizationThreshold = 0.0f;
748  pnh.param("loc_thr", localizationThreshold, localizationThreshold);
749  if(rtabmap_.getMemory() && !rtabmap_.getMemory()->isIncremental() && localizationThreshold > 0.0)
750  {
751  NODELET_INFO("rtabmap: loc_thr = %f", localizationThreshold);
752  localizationDiagnostic_.setLocalizationThreshold(localizationThreshold);
753  tasks.push_back(&localizationDiagnostic_);
754  }
755  setupCallbacks(nh, pnh, getName(), tasks); // do it at the end
756  if(!this->isDataSubscribed())
757  {
758  bool isRGBD = uStr2Bool(parameters_.at(Parameters::kRGBDEnabled()).c_str());
759  if(isRGBD)
760  {
761  NODELET_WARN("ROS param subscribe_depth, subscribe_stereo and subscribe_rgbd are false, but RTAB-Map "
762  "parameter \"%s\" is true! Please set subscribe_depth, subscribe_stereo or subscribe_rgbd "
763  "to true to use rtabmap node for RGB-D SLAM, set \"%s\" to false for loop closure "
764  "detection on images-only or set subscribe_rgb to true to localize a single RGB camera against pre-built 3D map.",
765  Parameters::kRGBDEnabled().c_str(),
766  Parameters::kRGBDEnabled().c_str());
767  }
768  ros::NodeHandle rgb_nh(nh, "rgb");
769  ros::NodeHandle rgb_pnh(pnh, "rgb");
770  image_transport::ImageTransport rgb_it(rgb_nh);
771  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
772  defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
773 
774  NODELET_INFO("\n%s subscribed to:\n %s", getName().c_str(), defaultSub_.getTopic().c_str());
775  }
776  else if(!this->isSubscribedToDepth() &&
777  !this->isSubscribedToStereo() &&
778  !this->isSubscribedToRGBD() &&
779  !this->isSubscribedToRGB() &&
780  (this->isSubscribedToScan2d() || this->isSubscribedToScan3d() || this->isSubscribedToOdom()) &&
781  !this->isSubscribedToSensorData())
782  {
783  NODELET_WARN("There is no image subscription, bag-of-words loop closure detection will be disabled...");
784  int kpMaxFeatures = Parameters::defaultKpMaxFeatures();
785  int registrationStrategy = Parameters::defaultRegStrategy();
786  Parameters::parse(parameters_, Parameters::kKpMaxFeatures(), kpMaxFeatures);
787  Parameters::parse(parameters_, Parameters::kRegStrategy(), registrationStrategy);
788  bool updateParams = false;
789  if(kpMaxFeatures!= -1)
790  {
791  uInsert(parameters_, ParametersPair(Parameters::kKpMaxFeatures(), "-1"));
792  NODELET_WARN("Setting %s=-1 (bag-of-words disabled)", Parameters::kKpMaxFeatures().c_str());
793  updateParams = true;
794  }
795  if((this->isSubscribedToScan2d() || this->isSubscribedToScan3d()) && registrationStrategy != 1)
796  {
797  uInsert(parameters_, ParametersPair(Parameters::kRegStrategy(), "1"));
798  NODELET_WARN("Setting %s=1 (ICP)", Parameters::kRegStrategy().c_str());
799  updateParams = true;
800 
801  if(modifiedParameters.find(Parameters::kRGBDProximityPathMaxNeighbors()) == modifiedParameters.end())
802  {
803  if(this->isSubscribedToScan2d())
804  {
805  NODELET_WARN("Setting \"%s\" parameter to 10 (default 0) as \"subscribe_scan\" is "
806  "true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
807  "scans. To disable, set \"%s\" to 0. To suppress this warning, "
808  "add <param name=\"%s\" type=\"string\" value=\"10\"/>",
809  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
810  Parameters::kRegStrategy().c_str(),
811  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
812  Parameters::kRGBDProximityPathMaxNeighbors().c_str());
813  uInsert(parameters_, ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "10"));
814  }
815  else if(this->isSubscribedToScan3d())
816  {
817  NODELET_WARN("Setting \"%s\" parameter to 1 (default 0) as \"subscribe_scan_cloud\" is "
818  "true and \"%s\" uses ICP. To disable, set \"%s\" to 0. To suppress this warning, "
819  "add <param name=\"%s\" type=\"string\" value=\"1\"/>",
820  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
821  Parameters::kRegStrategy().c_str(),
822  Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
823  Parameters::kRGBDProximityPathMaxNeighbors().c_str());
824  uInsert(parameters_, ParametersPair(Parameters::kRGBDProximityPathMaxNeighbors(), "1"));
825  }
826  }
827  }
828  if(updateParams)
829  {
830  rtabmap_.parseParameters(parameters_);
831  }
832  }
833 
834  // Set initial pose if set
835  if(!initialPoseStr.empty())
836  {
837  Transform intialPose = Transform::fromString(initialPoseStr);
838  if(!intialPose.isNull())
839  {
840  NODELET_INFO("Setting initial pose: \"%s\"", intialPose.prettyPrint().c_str());
841  rtabmap_.setInitialPose(intialPose);
842  }
843  else
844  {
845  NODELET_ERROR("Invalid initial_pose: \"%s\"", initialPoseStr.c_str());
846  }
847  }
848 
849  // set private parameters
850  for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
851  {
852  pnh.setParam(iter->first, iter->second);
853  }
854 
855  userDataAsyncSub_ = nh.subscribe("user_data_async", 1, &CoreWrapper::userDataAsyncCallback, this);
856  globalPoseAsyncSub_ = nh.subscribe("global_pose", 1, &CoreWrapper::globalPoseAsyncCallback, this);
857  gpsFixAsyncSub_ = nh.subscribe("gps/fix", 1, &CoreWrapper::gpsFixAsyncCallback, this);
858 #ifdef WITH_APRILTAG_ROS
859  tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this);
860 #endif
861 #ifdef WITH_FIDUCIAL_MSGS
862  fiducialTransfromsSub_ = nh.subscribe("fiducial_transforms", 1, &CoreWrapper::fiducialDetectionsAsyncCallback, this);
863 #endif
864  imuSub_ = nh.subscribe("imu", 100, &CoreWrapper::imuAsyncCallback, this);
865  republishNodeDataSub_ = nh.subscribe("republish_node_data", 100, &CoreWrapper::republishNodeDataCallback, this);
866 }
867 
868 CoreWrapper::~CoreWrapper()
869 {
870  if(transformThread_)
871  {
872  tfThreadRunning_ = false;
873  transformThread_->join();
874  delete transformThread_;
875  }
876 
877  this->saveParameters(configPath_);
878 
879  printf("rtabmap: Saving database/long-term memory... (located at %s)\n", databasePath_.c_str());
880  if(rtabmap_.getMemory())
881  {
882  // save the grid map
883  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
884  cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
885  if(!pixels.empty())
886  {
887  printf("rtabmap: 2D occupancy grid map saved.\n");
888  rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
889  }
890  }
891 
892  rtabmap_.close();
893  printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
894 
895  delete interOdomSync_;
896  delete mbClient_;
897 }
898 
899 void CoreWrapper::loadParameters(const std::string & configFile, ParametersMap & parameters)
900 {
901  if(!configFile.empty())
902  {
903  NODELET_INFO("Loading parameters from %s", configFile.c_str());
904  if(!UFile::exists(configFile.c_str()))
905  {
906  NODELET_WARN("Config file doesn't exist! It will be generated...");
907  }
908  Parameters::readINI(configFile.c_str(), parameters);
909  }
910 }
911 
912 void CoreWrapper::saveParameters(const std::string & configFile)
913 {
914  if(!configFile.empty())
915  {
916  printf("Saving parameters to %s\n", configFile.c_str());
917 
918  if(!UFile::exists(configFile.c_str()))
919  {
920  printf("Config file doesn't exist, a new one will be created.\n");
921  }
922  Parameters::writeINI(configFile.c_str(), parameters_);
923  }
924  else
925  {
926  NODELET_INFO("Parameters are not saved! (No configuration file provided...)");
927  }
928 }
929 
930 void CoreWrapper::publishLoop(double tfDelay, double tfTolerance)
931 {
932  if(tfDelay == 0)
933  return;
934  ros::Rate r(1.0 / tfDelay);
935  while(tfThreadRunning_)
936  {
937  if(!odomFrameId_.empty())
938  {
939  mapToOdomMutex_.lock();
940  ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfTolerance);
941  geometry_msgs::TransformStamped msg;
942  msg.child_frame_id = odomFrameId_;
943  msg.header.frame_id = mapFrameId_;
944  msg.header.stamp = tfExpiration;
945  rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform);
946  tfBroadcaster_.sendTransform(msg);
947  mapToOdomMutex_.unlock();
948  }
949  r.sleep();
950  }
951 }
952 
953 void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
954 {
955  if(!paused_)
956  {
957  ros::Time stamp = imageMsg->header.stamp;
958  if(stamp.toSec() == 0.0)
959  {
960  ROS_WARN("A null stamp has been detected in the input topic. Make sure the stamp is set.");
961  return;
962  }
963 
964  if(rate_>0.0f)
965  {
966  if(previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_))
967  {
968  return;
969  }
970  }
971  previousStamp_ = stamp;
972 
973  if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
974  imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
975  imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
976  imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
977  {
978  NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
979  return;
980  }
981 
982  cv_bridge::CvImageConstPtr ptrImage;
983  if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
984  imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
985  {
986  ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
987  }
988  else
989  {
990  ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
991  }
992 
993  // process data
994  UTimer timer;
995  if(rtabmap_.isIDsGenerated() || ptrImage->header.seq > 0)
996  {
997  if(!rtabmap_.process(ptrImage->image.clone(), ptrImage->header.seq))
998  {
999  NODELET_WARN("RTAB-Map could not process the data received! (ROS id = %d)", ptrImage->header.seq);
1000  }
1001  else
1002  {
1003  this->publishStats(ros::Time::now());
1004  }
1005  }
1006  else if(!rtabmap_.isIDsGenerated())
1007  {
1008  NODELET_WARN("Ignoring received image because its sequence ID=0. Please "
1009  "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. "
1010  "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and "
1011  "when you need to have IDs output of RTAB-map synchronised with the source "
1012  "image sequence ID.");
1013  }
1014  NODELET_INFO("rtabmap: Update rate=%fs, Limit=%fs, Processing time = %fs (%d local nodes)",
1015  1.0f/rate_,
1016  rtabmap_.getTimeThreshold()/1000.0f,
1017  timer.ticks(),
1018  rtabmap_.getWMSize()+rtabmap_.getSTMSize());
1019  }
1020 }
1021 
1022 bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Time stamp)
1023 {
1024  if(!paused_)
1025  {
1026  Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose);
1027  if(!odom.isNull())
1028  {
1029  Transform odomTF;
1030  if(!stamp.isZero()) {
1031  odomTF = rtabmap_conversions::getTransform(odomMsg->header.frame_id, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
1032  }
1033  if(odomTF.isNull())
1034  {
1035  static bool shown = false;
1036  if(!shown)
1037  {
1038  NODELET_WARN("We received odometry message, but we cannot get the "
1039  "corresponding TF %s->%s at data stamp %fs (odom msg stamp is %fs). Make sure TF of odometry is "
1040  "also published to get more accurate pose estimation. This "
1041  "warning is only printed once.", odomMsg->header.frame_id.c_str(), frameId_.c_str(), stamp.toSec(), odomMsg->header.stamp.toSec());
1042  shown = true;
1043  }
1044  stamp = odomMsg->header.stamp;
1045  }
1046  else
1047  {
1048  odom = odomTF;
1049  }
1050  }
1051 
1052  if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || (odomMsg->pose.covariance[0] >= BAD_COVARIANCE && odomMsg->twist.covariance[0] >= BAD_COVARIANCE)))
1053  {
1054  UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0]));
1055  rtabmap_.triggerNewMap();
1056  covariance_ = cv::Mat();
1057  }
1058 
1059  lastPoseIntermediate_ = false;
1060  lastPose_ = odom;
1061  lastPoseStamp_ = stamp;
1062  lastPoseVelocity_.resize(6);
1063  lastPoseVelocity_[0] = odomMsg->twist.twist.linear.x;
1064  lastPoseVelocity_[1] = odomMsg->twist.twist.linear.y;
1065  lastPoseVelocity_[2] = odomMsg->twist.twist.linear.z;
1066  lastPoseVelocity_[3] = odomMsg->twist.twist.angular.x;
1067  lastPoseVelocity_[4] = odomMsg->twist.twist.angular.y;
1068  lastPoseVelocity_[5] = odomMsg->twist.twist.angular.z;
1069 
1070  // Only update variance if odom is not null
1071  if(!odom.isNull())
1072  {
1073  cv::Mat covariance;
1074  double variance = odomMsg->twist.covariance[0];
1075  if(variance == BAD_COVARIANCE || variance <= 0.0f)
1076  {
1077  //use the one of the pose
1078  covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->pose.covariance.data()).clone();
1079  covariance /= 2.0;
1080  }
1081  else
1082  {
1083  covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->twist.covariance.data()).clone();
1084  }
1085 
1086  if(uIsFinite(covariance.at<double>(0,0)) &&
1087  covariance.at<double>(0,0) != 1.0 &&
1088  covariance.at<double>(0,0)>0.0)
1089  {
1090  // Use largest covariance error (to be independent of the odometry frame rate)
1091  if(covariance_.empty() || covariance.at<double>(0,0) > covariance_.at<double>(0,0))
1092  {
1093  covariance_ = covariance;
1094  }
1095  }
1096  }
1097 
1098  // Throttle
1099  bool ignoreFrame = false;
1100  if(stamp.toSec() == 0.0)
1101  {
1102  ROS_WARN("A null stamp has been detected in the input topics. Make sure the stamp in all input topics is set.");
1103  ignoreFrame = true;
1104  }
1105  if(rate_>0.0f)
1106  {
1107  if(previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_))
1108  {
1109  ignoreFrame = true;
1110  }
1111  }
1112  if(ignoreFrame)
1113  {
1114  if(createIntermediateNodes_)
1115  {
1116  lastPoseIntermediate_ = true;
1117  }
1118  else
1119  {
1120  return false;
1121  }
1122  }
1123  else if(!ignoreFrame)
1124  {
1125  previousStamp_ = stamp;
1126  }
1127 
1128  return true;
1129  }
1130  return false;
1131 }
1132 
1133 bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
1134 {
1135  if(!paused_)
1136  {
1137  // Odom TF ready?
1138  Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
1139  if(odom.isNull())
1140  {
1141  return false;
1142  }
1143 
1144  if(!lastPose_.isIdentity() && odom.isIdentity())
1145  {
1146  UWARN("Odometry is reset (identity pose detected). Increment map id!");
1147  rtabmap_.triggerNewMap();
1148  covariance_ = cv::Mat();
1149  }
1150 
1151  lastPoseIntermediate_ = false;
1152  lastPose_ = odom;
1153  lastPoseStamp_ = stamp;
1154  lastPoseVelocity_.clear();
1155 
1156  bool ignoreFrame = false;
1157  if(stamp.toSec() == 0.0)
1158  {
1159  ROS_WARN("A null stamp has been detected in the input topics. Make sure the stamp in all input topics is set.");
1160  ignoreFrame = true;
1161  }
1162  if(rate_>0.0f)
1163  {
1164  if(previousStamp_.toSec() > 0.0 && stamp.toSec() > previousStamp_.toSec() && stamp - previousStamp_ < ros::Duration(1.0f/rate_))
1165  {
1166  ignoreFrame = true;
1167  }
1168  }
1169  if(ignoreFrame)
1170  {
1171  if(createIntermediateNodes_)
1172  {
1173  lastPoseIntermediate_ = true;
1174  }
1175  else
1176  {
1177  return false;
1178  }
1179  }
1180  else if(!ignoreFrame)
1181  {
1182  previousStamp_ = stamp;
1183  }
1184 
1185  return true;
1186  }
1187  return false;
1188 }
1189 
1190 void CoreWrapper::commonMultiCameraCallback(
1191  const nav_msgs::OdometryConstPtr & odomMsg,
1192  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
1193  const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
1194  const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
1195  const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
1196  const std::vector<sensor_msgs::CameraInfo> & depthCameraInfoMsgs,
1197  const sensor_msgs::LaserScan& scan2dMsg,
1198  const sensor_msgs::PointCloud2& scan3dMsg,
1199  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg,
1200  const std::vector<rtabmap_msgs::GlobalDescriptor> & globalDescriptorMsgs,
1201  const std::vector<std::vector<rtabmap_msgs::KeyPoint> > & localKeyPoints,
1202  const std::vector<std::vector<rtabmap_msgs::Point3f> > & localPoints3d,
1203  const std::vector<cv::Mat> & localDescriptors)
1204 {
1205  std::string odomFrameId = odomFrameId_;
1206  if(odomMsg.get())
1207  {
1208  odomFrameId = odomMsg->header.frame_id;
1209  if(!scan2dMsg.ranges.empty())
1210  {
1211  if(!odomUpdate(odomMsg, scan2dMsg.header.stamp))
1212  {
1213  return;
1214  }
1215  }
1216  else if(!scan3dMsg.data.empty())
1217  {
1218  if(!odomUpdate(odomMsg, scan3dMsg.header.stamp))
1219  {
1220  return;
1221  }
1222  }
1223  else if(cameraInfoMsgs.size() == 0 || !odomUpdate(odomMsg, cameraInfoMsgs[0].header.stamp))
1224  {
1225  return;
1226  }
1227  }
1228  else if(!scan2dMsg.ranges.empty())
1229  {
1230  if(!odomTFUpdate(scan2dMsg.header.stamp))
1231  {
1232  return;
1233  }
1234  }
1235  else if(!scan3dMsg.data.empty())
1236  {
1237  if(!odomTFUpdate(scan3dMsg.header.stamp))
1238  {
1239  return;
1240  }
1241  }
1242  else if(cameraInfoMsgs.size() == 0 || !odomTFUpdate(cameraInfoMsgs[0].header.stamp))
1243  {
1244  return;
1245  }
1246 
1247  commonMultiCameraCallbackImpl(odomFrameId,
1248  userDataMsg,
1249  imageMsgs,
1250  depthMsgs,
1251  cameraInfoMsgs,
1252  depthCameraInfoMsgs,
1253  scan2dMsg,
1254  scan3dMsg,
1255  odomInfoMsg,
1256  globalDescriptorMsgs,
1257  localKeyPoints,
1258  localPoints3d,
1259  localDescriptors);
1260 }
1261 
1262 void CoreWrapper::commonMultiCameraCallbackImpl(
1263  const std::string & odomFrameId,
1264  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
1265  const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
1266  const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
1267  const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
1268  const std::vector<sensor_msgs::CameraInfo> & depthCameraInfoMsgs,
1269  const sensor_msgs::LaserScan& scan2dMsg,
1270  const sensor_msgs::PointCloud2& scan3dMsg,
1271  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg,
1272  const std::vector<rtabmap_msgs::GlobalDescriptor> & globalDescriptorMsgs,
1273  const std::vector<std::vector<rtabmap_msgs::KeyPoint> > & localKeyPointsMsgs,
1274  const std::vector<std::vector<rtabmap_msgs::Point3f> > & localPoints3dMsgs,
1275  const std::vector<cv::Mat> & localDescriptorsMsgs)
1276 {
1277  UTimer timerConversion;
1278  cv::Mat rgb;
1279  cv::Mat depth;
1280  std::vector<rtabmap::CameraModel> cameraModels;
1281  std::vector<rtabmap::StereoCameraModel> stereoCameraModels;
1282  std::vector<cv::KeyPoint> keypoints;
1283  std::vector<cv::Point3f> points;
1284  cv::Mat descriptors;
1285 
1286  if(!rtabmap_conversions::convertRGBDMsgs(
1287  imageMsgs,
1288  depthMsgs,
1289  cameraInfoMsgs,
1290  depthCameraInfoMsgs,
1291  frameId_,
1292  odomSensorSync_?odomFrameId:"",
1293  lastPoseStamp_,
1294  rgb,
1295  depth,
1296  cameraModels,
1297  stereoCameraModels,
1298  tfListener_,
1299  waitForTransform_?waitForTransformDuration_:0.0,
1300  alreadyRectifiedImages_,
1301  localKeyPointsMsgs,
1302  localPoints3dMsgs,
1303  localDescriptorsMsgs,
1304  &keypoints,
1305  &points,
1306  &descriptors))
1307  {
1308  NODELET_ERROR("Could not convert rgb/depth msgs! Aborting rtabmap update...");
1309  return;
1310  }
1311  UDEBUG("cameraModels=%ld stereoCameraModels=%ld", cameraModels.size(), stereoCameraModels.size());
1312  UDEBUG("rgb=%dx%d(type=%d), depth/right=%dx%d(type=%d)", rgb.rows, rgb.cols, rgb.type(), depth.rows, depth.cols, depth.type());
1313 
1314  if(stereoCameraModels.size() && stereoToDepth_)
1315  {
1316  UASSERT(depth.type() == CV_8UC1);
1317  cv::Mat leftMono;
1318  if(rgb.channels() == 3)
1319  {
1320  cv::cvtColor(rgb, leftMono, CV_BGR2GRAY);
1321  }
1322  else
1323  {
1324  leftMono = rgb;
1325  }
1326  cv::Mat rightMono = depth;
1327  depth = cv::Mat();
1328 
1329  UASSERT(int((leftMono.cols/stereoCameraModels.size())*stereoCameraModels.size()) == leftMono.cols);
1330  UASSERT(int((rightMono.cols/stereoCameraModels.size())*stereoCameraModels.size()) == rightMono.cols);
1331  int subImageWidth = leftMono.cols/stereoCameraModels.size();
1332  for(size_t i=0; i<stereoCameraModels.size(); ++i)
1333  {
1334  cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
1335  cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows));
1336 
1337  // cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters
1338  cv::Mat disparity = rtabmap::util2d::disparityFromStereoImages(
1339  left,
1340  right,
1341  parameters_);
1342  if(disparity.empty())
1343  {
1344  NODELET_ERROR("Could not compute disparity image (\"stereo_to_depth\" is true)!");
1345  return;
1346  }
1347  cv::Mat subDepth = rtabmap::util2d::depthFromDisparity(
1348  disparity,
1349  stereoCameraModels[i].left().fx(),
1350  stereoCameraModels[i].baseline());
1351 
1352  if(subDepth.empty())
1353  {
1354  NODELET_ERROR("Could not compute depth image (\"stereo_to_depth\" is true)!");
1355  return;
1356  }
1357  UASSERT(subDepth.type() == CV_16UC1 || subDepth.type() == CV_32FC1);
1358 
1359  if(depth.empty())
1360  {
1361  depth = cv::Mat(subDepth.rows, subDepth.cols*stereoCameraModels.size(), subDepth.type());
1362  }
1363 
1364  if(subDepth.type() == depth.type())
1365  {
1366  subDepth.copyTo(cv::Mat(depth, cv::Rect(i*subDepth.cols, 0, subDepth.cols, subDepth.rows)));
1367  }
1368  else
1369  {
1370  ROS_ERROR("Some Depth images are not the same type!");
1371  return;
1372  }
1373 
1374  cameraModels.push_back(stereoCameraModels[i].left());
1375  }
1376  stereoCameraModels.clear();
1377  }
1378 
1379  UASSERT(uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format()));
1380  if(!depth.empty() && depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
1381  {
1382  depth = rtabmap::util2d::cvtDepthFromFloat(depth);
1383  static bool shown = false;
1384  if(!shown)
1385  {
1386  NODELET_WARN("Save depth data to 16 bits format: depth type detected is "
1387  "32FC1, use 16UC1 depth format to avoid this conversion "
1388  "(or set parameter \"Mem/SaveDepth16Format=false\" to use "
1389  "32bits format). This message is only printed once...");
1390  shown = true;
1391  }
1392  }
1393 
1394  LaserScan scan;
1395  bool genMaxScanPts = 0;
1396  if(scan2dMsg.ranges.empty() && scan3dMsg.data.empty() && !depth.empty() && stereoCameraModels.empty() && genScan_)
1397  {
1398  pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud2d(new pcl::PointCloud<pcl::PointXYZ>);
1399  *scanCloud2d = util3d::laserScanFromDepthImages(
1400  depth,
1401  cameraModels,
1402  genScanMaxDepth_,
1403  genScanMinDepth_);
1404  genMaxScanPts += depth.cols;
1405  scan = LaserScan(rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d), 0, genScanMaxDepth_);
1406  }
1407  else if(!scan2dMsg.ranges.empty())
1408  {
1409  if(!rtabmap_conversions::convertScanMsg(
1410  scan2dMsg,
1411  frameId_,
1412  odomSensorSync_?odomFrameId:"",
1413  lastPoseStamp_,
1414  scan,
1415  tfListener_,
1416  waitForTransform_?waitForTransformDuration_:0,
1417  // backward compatibility, project 2D scan in /base_link frame
1418  rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
1419  {
1420  NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
1421  return;
1422  }
1423  }
1424  else if(!scan3dMsg.data.empty())
1425  {
1426  if(!rtabmap_conversions::convertScan3dMsg(
1427  scan3dMsg,
1428  frameId_,
1429  odomSensorSync_?odomFrameId:"",
1430  lastPoseStamp_,
1431  scan,
1432  tfListener_,
1433  waitForTransform_?waitForTransformDuration_:0,
1434  scanCloudMaxPoints_,
1435  0,
1436  scanCloudIs2d_))
1437  {
1438  NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
1439  return;
1440  }
1441 
1442  ROS_DEBUG("%d %d %d %d", rgb.empty()?1:0, depth.empty()?1:0, scan.isEmpty()?1:0, genDepth_?1:0);
1443  if(!rgb.empty() && depth.empty() && !scan.isEmpty() && genDepth_)
1444  {
1445  for(size_t i=0; i<cameraModels.size(); ++i)
1446  {
1447  rtabmap::CameraModel model = cameraModels[i];
1448  if(genDepthDecimation_ > 1)
1449  {
1450  if(model.imageWidth()%genDepthDecimation_ == 0 && model.imageHeight()%genDepthDecimation_ == 0)
1451  {
1452  model = model.scaled(1.0f/float(genDepthDecimation_));
1453  }
1454  else
1455  {
1456  ROS_ERROR("decimation (%d) not valid for image size %dx%d! Aborting depth generation from scan...",
1457  genDepthDecimation_,
1458  model.imageWidth(),
1459  model.imageHeight());
1460  depth = cv::Mat();
1461  break;
1462  }
1463  }
1464 
1465  cv::Mat depthProjected = util3d::projectCloudToCamera(
1466  model.imageSize(),
1467  model.K(),
1468  scan.data(),
1469  scan.localTransform().inverse()*model.localTransform());
1470 
1471  if(genDepthFillHolesSize_ > 0 && genDepthFillIterations_ > 0)
1472  {
1473  for(int i=0; i<genDepthFillIterations_;++i)
1474  {
1475  depthProjected = util2d::fillDepthHoles(
1476  depthProjected,
1477  genDepthFillHolesSize_,
1478  genDepthFillHolesError_);
1479  }
1480  }
1481 
1482  if(depth.empty())
1483  {
1484  depth = cv::Mat::zeros(model.imageHeight(), model.imageWidth()*cameraModels.size(), CV_32FC1);
1485  }
1486  depthProjected.copyTo(depth.colRange(i*model.imageWidth(), (i+1)*model.imageWidth()));
1487  }
1488  }
1489  }
1490 
1491  cv::Mat userData;
1492  if(userDataMsg.get())
1493  {
1494  userData = rtabmap_conversions::userDataFromROS(*userDataMsg);
1495  UScopeMutex lock(userDataMutex_);
1496  if(!userData_.empty())
1497  {
1498  NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
1499  userData_ = cv::Mat();
1500  }
1501  }
1502  else
1503  {
1504  UScopeMutex lock(userDataMutex_);
1505  userData = userData_;
1506  userData_ = cv::Mat();
1507  }
1508 
1509  SensorData data;
1510  if(!stereoCameraModels.empty())
1511  {
1512  data = SensorData(
1513  scan,
1514  rgb,
1515  depth,
1516  stereoCameraModels,
1517  lastPoseIntermediate_?-1:!cameraInfoMsgs.empty()?cameraInfoMsgs[0].header.seq:0,
1518  rtabmap_conversions::timestampFromROS(lastPoseStamp_),
1519  userData);
1520  }
1521  else
1522  {
1523  data = SensorData(
1524  scan,
1525  rgb,
1526  depth,
1527  cameraModels,
1528  lastPoseIntermediate_?-1:!cameraInfoMsgs.empty()?cameraInfoMsgs[0].header.seq:0,
1529  rtabmap_conversions::timestampFromROS(lastPoseStamp_),
1530  userData);
1531  }
1532 
1533  OdometryInfo odomInfo;
1534  if(odomInfoMsg.get())
1535  {
1536  odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg);
1537  }
1538 
1539  if(!globalDescriptorMsgs.empty())
1540  {
1541  data.setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(globalDescriptorMsgs));
1542  }
1543 
1544  if(!keypoints.empty())
1545  {
1546  UASSERT(points.empty() || points.size() == keypoints.size());
1547  UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
1548  data.setFeatures(keypoints, points, descriptors);
1549  }
1550 
1551  process(lastPoseStamp_,
1552  data,
1553  lastPose_,
1554  lastPoseVelocity_,
1555  odomFrameId,
1556  covariance_,
1557  odomInfo,
1558  timerConversion.ticks());
1559  covariance_ = cv::Mat();
1560 }
1561 
1562 void CoreWrapper::commonLaserScanCallback(
1563  const nav_msgs::OdometryConstPtr & odomMsg,
1564  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
1565  const sensor_msgs::LaserScan& scan2dMsg,
1566  const sensor_msgs::PointCloud2& scan3dMsg,
1567  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg,
1568  const rtabmap_msgs::GlobalDescriptor & globalDescriptor)
1569 {
1570  UTimer timerConversion;
1571  std::string odomFrameId = odomFrameId_;
1572  if(odomMsg.get())
1573  {
1574  odomFrameId = odomMsg->header.frame_id;
1575  if(!scan2dMsg.ranges.empty())
1576  {
1577  if(!odomUpdate(odomMsg, scan2dMsg.header.stamp))
1578  {
1579  return;
1580  }
1581  }
1582  else if(!scan3dMsg.data.empty())
1583  {
1584  if(!odomUpdate(odomMsg, scan3dMsg.header.stamp))
1585  {
1586  return;
1587  }
1588  }
1589  else
1590  {
1591  return;
1592  }
1593  }
1594  else if(!scan2dMsg.ranges.empty())
1595  {
1596  if(!odomTFUpdate(scan2dMsg.header.stamp))
1597  {
1598  return;
1599  }
1600  }
1601  else if(!scan3dMsg.data.empty())
1602  {
1603  if(!odomTFUpdate(scan3dMsg.header.stamp))
1604  {
1605  return;
1606  }
1607  }
1608  else
1609  {
1610  return;
1611  }
1612 
1613  LaserScan scan;
1614  if(!scan2dMsg.ranges.empty())
1615  {
1616  if(!rtabmap_conversions::convertScanMsg(
1617  scan2dMsg,
1618  frameId_,
1619  odomSensorSync_?odomFrameId:"",
1620  lastPoseStamp_,
1621  scan,
1622  tfListener_,
1623  waitForTransform_?waitForTransformDuration_:0,
1624  // backward compatibility, project 2D scan in /base_link frame
1625  rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
1626  {
1627  NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
1628  return;
1629  }
1630  }
1631  else if(!scan3dMsg.data.empty())
1632  {
1633  if(!rtabmap_conversions::convertScan3dMsg(
1634  scan3dMsg,
1635  frameId_,
1636  odomSensorSync_?odomFrameId:"",
1637  lastPoseStamp_,
1638  scan,
1639  tfListener_,
1640  waitForTransform_?waitForTransformDuration_:0,
1641  scanCloudMaxPoints_,
1642  0,
1643  scanCloudIs2d_))
1644  {
1645  NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
1646  return;
1647  }
1648  }
1649 
1650  cv::Mat userData;
1651  if(userDataMsg.get())
1652  {
1653  userData = rtabmap_conversions::userDataFromROS(*userDataMsg);
1654  UScopeMutex lock(userDataMutex_);
1655  if(!userData_.empty())
1656  {
1657  NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
1658  userData_ = cv::Mat();
1659  }
1660  }
1661  else
1662  {
1663  UScopeMutex lock(userDataMutex_);
1664  userData = userData_;
1665  userData_ = cv::Mat();
1666  }
1667 
1668  SensorData data(
1669  scan,
1670  cv::Mat(),
1671  cv::Mat(),
1672  rtabmap::CameraModel(),
1673  lastPoseIntermediate_?-1:!scan2dMsg.ranges.empty()?scan2dMsg.header.seq:scan3dMsg.header.seq,
1674  rtabmap_conversions::timestampFromROS(lastPoseStamp_),
1675  userData);
1676 
1677  OdometryInfo odomInfo;
1678  if(odomInfoMsg.get())
1679  {
1680  odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg);
1681  }
1682 
1683  if(!globalDescriptor.data.empty())
1684  {
1685  data.addGlobalDescriptor(rtabmap_conversions::globalDescriptorFromROS(globalDescriptor));
1686  }
1687 
1688  process(lastPoseStamp_,
1689  data,
1690  lastPose_,
1691  lastPoseVelocity_,
1692  odomFrameId,
1693  covariance_,
1694  odomInfo,
1695  timerConversion.ticks());
1696 
1697  covariance_ = cv::Mat();
1698 }
1699 
1700 void CoreWrapper::commonOdomCallback(
1701  const nav_msgs::OdometryConstPtr & odomMsg,
1702  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
1703  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
1704 {
1705  UTimer timerConversion;
1706  UASSERT(odomMsg.get());
1707  std::string odomFrameId = odomFrameId_;
1708 
1709  odomFrameId = odomMsg->header.frame_id;
1710  if(!odomUpdate(odomMsg, odomMsg->header.stamp))
1711  {
1712  return;
1713  }
1714 
1715  cv::Mat userData;
1716  if(userDataMsg.get())
1717  {
1718  userData = rtabmap_conversions::userDataFromROS(*userDataMsg);
1719  UScopeMutex lock(userDataMutex_);
1720  if(!userData_.empty())
1721  {
1722  NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
1723  userData_ = cv::Mat();
1724  }
1725  }
1726  else
1727  {
1728  UScopeMutex lock(userDataMutex_);
1729  userData = userData_;
1730  userData_ = cv::Mat();
1731  }
1732 
1733  SensorData data(
1734  cv::Mat(),
1735  cv::Mat(),
1736  rtabmap::CameraModel(),
1737  lastPoseIntermediate_?-1:odomMsg->header.seq,
1738  rtabmap_conversions::timestampFromROS(lastPoseStamp_),
1739  userData);
1740 
1741  OdometryInfo odomInfo;
1742  if(odomInfoMsg.get())
1743  {
1744  odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg);
1745  }
1746 
1747  process(lastPoseStamp_,
1748  data,
1749  lastPose_,
1750  lastPoseVelocity_,
1751  odomFrameId,
1752  covariance_,
1753  odomInfo,
1754  timerConversion.ticks());
1755 
1756  covariance_ = cv::Mat();
1757 }
1758 
1759 void CoreWrapper::commonSensorDataCallback(
1760  const rtabmap_msgs::SensorDataConstPtr & sensorDataMsg,
1761  const nav_msgs::OdometryConstPtr & odomMsg,
1762  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
1763 {
1764  UTimer timerConversion;
1765  UASSERT(sensorDataMsg.get());
1766  std::string odomFrameId = odomFrameId_;
1767  if(odomMsg.get())
1768  {
1769  odomFrameId = odomMsg->header.frame_id;
1770  if(!odomUpdate(odomMsg, sensorDataMsg->header.stamp))
1771  {
1772  return;
1773  }
1774  }
1775  else if(!odomTFUpdate(sensorDataMsg->header.stamp))
1776  {
1777  return;
1778  }
1779 
1780  SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg);
1781  data.setId(lastPoseIntermediate_?-1:0);
1782 
1783  OdometryInfo odomInfo;
1784  if(odomInfoMsg.get())
1785  {
1786  odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg);
1787  }
1788 
1789  process(lastPoseStamp_,
1790  data,
1791  lastPose_,
1792  lastPoseVelocity_,
1793  odomFrameId,
1794  covariance_,
1795  odomInfo,
1796  timerConversion.ticks());
1797 
1798  covariance_ = cv::Mat();
1799 }
1800 
1801 void CoreWrapper::process(
1802  const ros::Time & stamp,
1803  SensorData & data,
1804  const Transform & odom,
1805  const std::vector<float> & odomVelocityIn,
1806  const std::string & odomFrameId,
1807  const cv::Mat & odomCovariance,
1808  const OdometryInfo & odomInfo,
1809  double timeMsgConversion)
1810 {
1811  UTimer timer;
1812  if(rtabmap_.isIDsGenerated() || data.id() > 0)
1813  {
1814  // Add intermediate nodes?
1815  for(std::list<std::pair<nav_msgs::Odometry, rtabmap_msgs::OdomInfo> >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
1816  {
1817  if(iter->first.header.stamp < lastPoseStamp_)
1818  {
1819  Transform interOdom;
1820  if(!rtabmap_.getLocalOptimizedPoses().empty())
1821  {
1822  // add intermediate poses only if the current local graph is not empty
1823  interOdom = rtabmap_conversions::transformFromPoseMsg(iter->first.pose.pose);
1824  }
1825  if(!interOdom.isNull())
1826  {
1827  cv::Mat covariance;
1828  double variance = iter->first.twist.covariance[0];
1829  if(variance == BAD_COVARIANCE || variance <= 0.0f)
1830  {
1831  //use the one of the pose
1832  covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.pose.covariance.data()).clone();
1833  covariance /= 2.0;
1834  }
1835  else
1836  {
1837  covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.twist.covariance.data()).clone();
1838  }
1839  if(!uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
1840  {
1841  covariance = cv::Mat::eye(6,6,CV_64FC1);
1842  if(odomDefaultLinVariance_ > 0.0f)
1843  {
1844  covariance.at<double>(0,0) = odomDefaultLinVariance_;
1845  covariance.at<double>(1,1) = odomDefaultLinVariance_;
1846  covariance.at<double>(2,2) = odomDefaultLinVariance_;
1847  }
1848  if(odomDefaultAngVariance_ > 0.0f)
1849  {
1850  covariance.at<double>(3,3) = odomDefaultAngVariance_;
1851  covariance.at<double>(4,4) = odomDefaultAngVariance_;
1852  covariance.at<double>(5,5) = odomDefaultAngVariance_;
1853  }
1854  }
1855  else if(twoDMapping_)
1856  {
1857  // If 2d mapping, make sure all diagonal values of the covariance that even not used are not null.
1858  covariance.at<double>(2,2) = uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
1859  covariance.at<double>(3,3) = uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
1860  covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
1861  }
1862 
1863  SensorData interData(cv::Mat(), cv::Mat(), rtabmap::CameraModel(), -1, rtabmap_conversions::timestampFromROS(iter->first.header.stamp));
1864  Transform gt;
1865  if(!groundTruthFrameId_.empty())
1866  {
1867  gt = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->first.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
1868  }
1869  interData.setGroundTruth(gt);
1870 
1871  std::map<std::string, float> externalStats;
1872  std::vector<float> odomVelocity;
1873  if(iter->second.timeEstimation != 0.0f)
1874  {
1875  OdometryInfo info = rtabmap_conversions::odomInfoFromROS(iter->second, true);
1876  externalStats = rtabmap_conversions::odomInfoToStatistics(info);
1877 
1878  if(info.interval>0.0)
1879  {
1880  odomVelocity.resize(6);
1881  float x,y,z,roll,pitch,yaw;
1882  info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1883  odomVelocity[0] = x/info.interval;
1884  odomVelocity[1] = y/info.interval;
1885  odomVelocity[2] = z/info.interval;
1886  odomVelocity[3] = roll/info.interval;
1887  odomVelocity[4] = pitch/info.interval;
1888  odomVelocity[5] = yaw/info.interval;
1889  }
1890  }
1891  if(odomVelocity.empty())
1892  {
1893  odomVelocity.resize(6);
1894  odomVelocity[0] = iter->first.twist.twist.linear.x;
1895  odomVelocity[1] = iter->first.twist.twist.linear.y;
1896  odomVelocity[2] = iter->first.twist.twist.linear.z;
1897  odomVelocity[3] = iter->first.twist.twist.angular.x;
1898  odomVelocity[4] = iter->first.twist.twist.angular.y;
1899  odomVelocity[5] = iter->first.twist.twist.angular.z;
1900  }
1901 
1902  rtabmap_.process(interData, interOdom, covariance, odomVelocity, externalStats);
1903  }
1904  interOdoms_.erase(iter++);
1905  }
1906  else if(iter->first.header.stamp == lastPoseStamp_)
1907  {
1908  interOdoms_.erase(iter++);
1909  break;
1910  }
1911  else
1912  {
1913  break;
1914  }
1915  }
1916 
1917  //Add async stuff
1918  Transform groundTruthPose;
1919  if(!groundTruthFrameId_.empty())
1920  {
1921  groundTruthPose = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
1922  }
1923  data.setGroundTruth(groundTruthPose);
1924 
1925  //global pose
1926  if(!globalPose_.header.stamp.isZero())
1927  {
1928  // assume sensor is fixed
1929  Transform sensorToBase = rtabmap_conversions::getTransform(
1930  globalPose_.header.frame_id,
1931  frameId_,
1932  lastPoseStamp_,
1933  tfListener_,
1934  waitForTransform_?waitForTransformDuration_:0.0);
1935  if(!sensorToBase.isNull())
1936  {
1937  Transform globalPose = rtabmap_conversions::transformFromPoseMsg(globalPose_.pose.pose);
1938  globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame
1939 
1940  // Correction of the global pose accounting the odometry movement since we received it
1941  Transform correction = rtabmap_conversions::getMovingTransform(
1942  frameId_,
1943  odomFrameId,
1944  lastPoseStamp_,
1945  globalPose_.header.stamp,
1946  tfListener_,
1947  waitForTransform_?waitForTransformDuration_:0.0);
1948  if(!correction.isNull())
1949  {
1950  globalPose *= correction;
1951  }
1952  else
1953  {
1954  NODELET_WARN("Could not adjust global pose accordingly to latest odometry pose. "
1955  "If odometry is small since it received the global pose and "
1956  "covariance is large, this should not be a problem.");
1957  }
1958  cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone();
1959  data.setGlobalPose(globalPose, globalPoseCovariance);
1960  }
1961  }
1962  globalPose_.header.stamp = ros::Time(0);
1963 
1964  if(gps_.stamp() > 0.0)
1965  {
1966  data.setGPS(gps_);
1967  }
1968  gps_ = rtabmap::GPS();
1969 
1970  //tag detections
1971  Landmarks landmarks = rtabmap_conversions::landmarksFromROS(
1972  tags_,
1973  frameId_,
1974  odomFrameId,
1975  lastPoseStamp_,
1976  tfListener_,
1977  waitForTransform_?waitForTransformDuration_:0,
1978  landmarkDefaultLinVariance_,
1979  landmarkDefaultAngVariance_);
1980  tags_.clear();
1981  if(!landmarks.empty())
1982  {
1983  data.setLandmarks(landmarks);
1984  }
1985 
1986  // IMU
1987  if(!imus_.empty())
1988  {
1989  Transform t = Transform::getTransform(imus_, data.stamp());
1990  if(!t.isNull())
1991  {
1992  // get local transform
1993  rtabmap::Transform localTransform;
1994  if(frameId_.compare(imuFrameId_) != 0)
1995  {
1996  localTransform = rtabmap_conversions::getTransform(frameId_, imuFrameId_, ros::Time(data.stamp()), tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
1997  }
1998  else
1999  {
2000  localTransform = rtabmap::Transform::getIdentity();
2001  }
2002 
2003  if(!localTransform.isNull())
2004  {
2005  Eigen::Quaterniond q = t.getQuaterniond();
2006  data.setIMU(IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
2007  cv::Vec3d(), cv::Mat(),
2008  cv::Vec3d(), cv::Mat(),
2009  localTransform));
2010  }
2011  }
2012  else
2013  {
2014  ROS_WARN("We are receiving imu data (buffer=%d), but cannot interpolate "
2015  "imu transform at time %f. IMU won't be added to graph.",
2016  (int)imus_.size(), data.stamp());
2017  }
2018  }
2019 
2020  double timeRtabmap = 0.0;
2021  double timeUpdateMaps = 0.0;
2022  double timePublishMaps = 0.0;
2023 
2024  cv::Mat covariance = odomCovariance;
2025  if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
2026  {
2027  covariance = cv::Mat::eye(6,6,CV_64FC1);
2028  if(odomDefaultLinVariance_ > 0.0f)
2029  {
2030  covariance.at<double>(0,0) = odomDefaultLinVariance_;
2031  covariance.at<double>(1,1) = odomDefaultLinVariance_;
2032  covariance.at<double>(2,2) = odomDefaultLinVariance_;
2033  }
2034  if(odomDefaultAngVariance_ > 0.0f)
2035  {
2036  covariance.at<double>(3,3) = odomDefaultAngVariance_;
2037  covariance.at<double>(4,4) = odomDefaultAngVariance_;
2038  covariance.at<double>(5,5) = odomDefaultAngVariance_;
2039  }
2040  }
2041  else if(twoDMapping_)
2042  {
2043  // If 2d mapping, make sure all diagonal values of the covariance that even not used are not null.
2044  covariance.at<double>(2,2) = uIsFinite(covariance.at<double>(2,2)) && covariance.at<double>(2,2)!=0?covariance.at<double>(2,2):1;
2045  covariance.at<double>(3,3) = uIsFinite(covariance.at<double>(3,3)) && covariance.at<double>(3,3)!=0?covariance.at<double>(3,3):1;
2046  covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
2047  }
2048 
2049  std::map<std::string, float> externalStats;
2050  std::vector<float> odomVelocity;
2051  if(odomInfo.timeEstimation != 0.0f)
2052  {
2053  externalStats = rtabmap_conversions::odomInfoToStatistics(odomInfo);
2054 
2055  if(odomInfo.interval>0.0)
2056  {
2057  odomVelocity.resize(6);
2058  float x,y,z,roll,pitch,yaw;
2059  odomInfo.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
2060  odomVelocity[0] = x/odomInfo.interval;
2061  odomVelocity[1] = y/odomInfo.interval;
2062  odomVelocity[2] = z/odomInfo.interval;
2063  odomVelocity[3] = roll/odomInfo.interval;
2064  odomVelocity[4] = pitch/odomInfo.interval;
2065  odomVelocity[5] = yaw/odomInfo.interval;
2066  }
2067  }
2068  if(odomVelocity.empty())
2069  {
2070  odomVelocity = odomVelocityIn;
2071  }
2072  if(rtabmapROSStats_.size())
2073  {
2074  externalStats.insert(rtabmapROSStats_.begin(), rtabmapROSStats_.end());
2075  rtabmapROSStats_.clear();
2076  }
2077 
2078  timeMsgConversion += timer.ticks();
2079  if(rtabmap_.process(data, odom, covariance, odomVelocity, externalStats))
2080  {
2081  timeRtabmap = timer.ticks();
2082  mapToOdomMutex_.lock();
2083  mapToOdom_ = rtabmap_.getMapCorrection();
2084 
2085  if(!odomFrameId.empty() && !odomFrameId_.empty() && odomFrameId_.compare(odomFrameId)!=0)
2086  {
2087  ROS_ERROR("Odometry received doesn't have same frame_id "
2088  "than the one previously set (old=%s, new=%s). "
2089  "Are there multiple nodes publishing on same odometry topic name? "
2090  "The new frame_id is now used.", odomFrameId_.c_str(), odomFrameId.c_str());
2091  }
2092 
2093  odomFrameId_ = odomFrameId;
2094  mapToOdomMutex_.unlock();
2095 
2096  if(data.id() < 0)
2097  {
2098  NODELET_INFO("Intermediate node added");
2099  }
2100  else
2101  {
2102  if(localizationPosePub_.getNumSubscribers())
2103  {
2104  bool localized = rtabmap_.getStatistics().loopClosureId()!=0 ||
2105  rtabmap_.getStatistics().proximityDetectionId()!=0 ||
2106  static_cast<int>(uValue(rtabmap_.getStatistics().data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f))!=0;
2107 
2108  if(localized || !pubLocPoseOnlyWhenLocalizing_)
2109  {
2110  geometry_msgs::PoseWithCovarianceStamped poseMsg;
2111  poseMsg.header.frame_id = mapFrameId_;
2112  poseMsg.header.stamp = stamp;
2113  rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose);
2114  if(!rtabmap_.getStatistics().localizationCovariance().empty())
2115  {
2116  const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance();
2117  memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double));
2118  }
2119  else
2120  {
2121  // Not yet localized, publish large covariance
2122  poseMsg.pose.covariance.data()[0] = 9999;
2123  poseMsg.pose.covariance.data()[7] = 9999;
2124  poseMsg.pose.covariance.data()[14] = twoDMapping_?rtabmap::Registration::COVARIANCE_LINEAR_EPSILON:9999;
2125  poseMsg.pose.covariance.data()[21] = twoDMapping_?rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON:9999;
2126  poseMsg.pose.covariance.data()[28] = twoDMapping_?rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON:9999;
2127  poseMsg.pose.covariance.data()[35] = 9999;
2128  }
2129  localizationPosePub_.publish(poseMsg);
2130  }
2131  }
2132  std::map<int, rtabmap::Transform> filteredPoses(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end());
2133 
2134  // create a tmp signature with latest sensory data if latest signature was ignored
2135  std::map<int, rtabmap::Signature> tmpSignature;
2136  if(rtabmap_.getMemory() == 0 ||
2137  filteredPoses.size() == 0 ||
2138  rtabmap_.getMemory()->getLastSignatureId() != filteredPoses.rbegin()->first ||
2139  rtabmap_.getMemory()->getLastWorkingSignature() == 0 ||
2140  rtabmap_.getMemory()->getLastWorkingSignature()->sensorData().gridCellSize() == 0 ||
2141  (!mapsManager_.getLocalMapMaker()->isGridFromDepth() && data.laserScanRaw().is2d())) // 2d laser scan would fill empty space for latest data
2142  {
2143  SensorData tmpData = data;
2144  tmpData.setId(0);
2145  tmpSignature.insert(std::make_pair(0, Signature(0, -1, 0, data.stamp(), "", odom, Transform(), tmpData)));
2146  filteredPoses.insert(std::make_pair(0, mapToOdom_*odom));
2147  }
2148 
2149  if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && filteredPoses.size()>1)
2150  {
2151  std::map<int, Transform> nearestPoses = filterNodesToAssemble(filteredPoses, mapToOdom_*odom);
2152 
2153  //add latest/zero and make sure those on a planned path are not filtered
2154  std::set<int> onPath;
2155  if(rtabmap_.getPath().size())
2156  {
2157  std::vector<int> nextNodes = rtabmap_.getPathNextNodes();
2158  onPath.insert(nextNodes.begin(), nextNodes.end());
2159  }
2160  for(std::map<int, Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
2161  {
2162  if(iter->first == 0 || onPath.find(iter->first) != onPath.end())
2163  {
2164  nearestPoses.insert(*iter);
2165  }
2166  else if(onPath.empty())
2167  {
2168  break;
2169  }
2170  }
2171 
2172  filteredPoses = nearestPoses;
2173  }
2174 
2175  // Update maps
2176  filteredPoses = mapsManager_.updateMapCaches(
2177  filteredPoses,
2178  rtabmap_.getMemory(),
2179  false,
2180  false,
2181  tmpSignature);
2182 
2183  timeUpdateMaps = timer.ticks();
2184 
2185  mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_);
2186 
2187  // Publish local graph, info
2188  this->publishStats(stamp);
2189 
2190  // update goal if planning is enabled
2191  if(!currentMetricGoal_.isNull())
2192  {
2193  if(rtabmap_.getPath().size() == 0)
2194  {
2195  // Don't send status yet if move_base actionlib is used unless it failed,
2196  // let move_base finish reaching the goal
2197  if(mbClient_ == 0 || rtabmap_.getPathStatus() <= 0)
2198  {
2199  if(rtabmap_.getPathStatus() > 0)
2200  {
2201  // Goal reached
2202  NODELET_INFO("Planning: Publishing goal reached!");
2203  }
2204  else if(rtabmap_.getPathStatus() <= 0)
2205  {
2206  NODELET_WARN("Planning: Plan failed!");
2207  if(mbClient_ && mbClient_->isServerConnected())
2208  {
2209  mbClient_->cancelGoal();
2210  }
2211  }
2212 
2213  if(goalReachedPub_.getNumSubscribers())
2214  {
2215  std_msgs::Bool result;
2216  result.data = rtabmap_.getPathStatus() > 0;
2217  goalReachedPub_.publish(result);
2218  }
2219  currentMetricGoal_.setNull();
2220  lastPublishedMetricGoal_.setNull();
2221  goalFrameId_.clear();
2222  latestNodeWasReached_ = false;
2223  }
2224  }
2225  else
2226  {
2227  currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
2228  if(!currentMetricGoal_.isNull())
2229  {
2230  // Adjust the target pose relative to last node
2231  if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
2232  {
2233  if(latestNodeWasReached_ ||
2234  rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius())
2235  {
2236  latestNodeWasReached_ = true;
2237  Transform goalLocalTransform = Transform::getIdentity();
2238  if(!goalFrameId_.empty() && goalFrameId_.compare(frameId_) != 0)
2239  {
2240  Transform localT = rtabmap_conversions::getTransform(frameId_, goalFrameId_, ros::Time::now(), tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
2241  if(!localT.isNull())
2242  {
2243  goalLocalTransform = localT.inverse().to3DoF();
2244  }
2245  }
2246  currentMetricGoal_ *= rtabmap_.getPathTransformToGoal()*goalLocalTransform;
2247  }
2248  }
2249 
2250  // publish next goal with updated currentMetricGoal_
2251  publishCurrentGoal(stamp);
2252 
2253  // publish local path
2254  publishLocalPath(stamp);
2255 
2256  // publish global path
2257  publishGlobalPath(stamp);
2258  }
2259  else
2260  {
2261  NODELET_ERROR("Planning: Local map broken, current goal id=%d (the robot may have moved to far from planned nodes)",
2262  rtabmap_.getPathCurrentGoalId());
2263  rtabmap_.clearPath(-1);
2264  if(goalReachedPub_.getNumSubscribers())
2265  {
2266  std_msgs::Bool result;
2267  result.data = false;
2268  goalReachedPub_.publish(result);
2269  }
2270  currentMetricGoal_.setNull();
2271  lastPublishedMetricGoal_.setNull();
2272  goalFrameId_.clear();
2273  latestNodeWasReached_ = false;
2274  }
2275  }
2276  }
2277 
2278  timePublishMaps = timer.ticks();
2279  }
2280 
2281  // If not intermediate node
2282  if(data.id() >= 0)
2283  {
2284  localizationDiagnostic_.updateStatus(rtabmap_.getStatistics().localizationCovariance(), twoDMapping_);
2285  tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps));
2286  }
2287  }
2288  else
2289  {
2290  timeRtabmap = timer.ticks();
2291  }
2292  NODELET_INFO("rtabmap (%d): Rate=%.2fs, Limit=%.3fs, Conversion=%.4fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)",
2293  rtabmap_.getLastLocationId(),
2294  rate_>0?1.0f/rate_:0,
2295  rtabmap_.getTimeThreshold()/1000.0f,
2296  timeMsgConversion,
2297  timeRtabmap,
2298  timeUpdateMaps,
2299  timePublishMaps,
2300  (int)rtabmap_.getLocalOptimizedPoses().size(),
2301  rtabmap_.getWMSize()+rtabmap_.getSTMSize());
2302  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0));
2303  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeMsgConversion/ms"), timeMsgConversion*1000.0f));
2304  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeRtabmap/ms"), timeRtabmap*1000.0f));
2305  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeUpdatingMaps/ms"), timeUpdateMaps*1000.0f));
2306  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimePublishing/ms"), timePublishMaps*1000.0f));
2307  rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/TimeTotal/ms"), (timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps)*1000.0f));
2308  }
2309  else if(!rtabmap_.isIDsGenerated())
2310  {
2311  NODELET_WARN("Ignoring received image because its sequence ID=0. Please "
2312  "set \"Mem/GenerateIds\"=\"true\" to ignore ros generated sequence id. "
2313  "Use only \"Mem/GenerateIds\"=\"false\" for once-time run of RTAB-Map and "
2314  "when you need to have IDs output of RTAB-map synchronized with the source "
2315  "image sequence ID.");
2316  }
2317 }
2318 
2319 std::map<int, Transform> CoreWrapper::filterNodesToAssemble(
2320  const std::map<int, Transform> & nodes,
2321  const Transform & currentPose)
2322 {
2323  std::map<int, Transform> output;
2324  if(mappingMaxNodes_ > 0)
2325  {
2326  std::map<int, float> nodesDist = graph::findNearestNodes(currentPose, nodes, 0, 0, mappingMaxNodes_);
2327  for(std::map<int, float>::iterator iter=nodesDist.begin(); iter!=nodesDist.end(); ++iter)
2328  {
2329  if(mappingAltitudeDelta_<=0.0 ||
2330  fabs(nodes.at(iter->first).z()-currentPose.z())<mappingAltitudeDelta_)
2331  {
2332  output.insert(*nodes.find(iter->first));
2333  }
2334  }
2335  }
2336  else // mappingAltitudeDelta_>0.0
2337  {
2338  for(std::map<int, Transform>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
2339  {
2340  if(fabs(iter->second.z()-currentPose.z())<mappingAltitudeDelta_)
2341  {
2342  output.insert(*iter);
2343  }
2344  }
2345  }
2346  return output;
2347 }
2348 
2349 void CoreWrapper::userDataAsyncCallback(const rtabmap_msgs::UserDataConstPtr & dataMsg)
2350 {
2351  if(!paused_)
2352  {
2353  UScopeMutex lock(userDataMutex_);
2354  static bool warningShow = false;
2355  if(!userData_.empty() && !warningShow)
2356  {
2357  ROS_WARN("Overwriting previous user data set. When asynchronous user "
2358  "data input topic rate is higher than "
2359  "map update rate (current %s=%f), only latest data is saved "
2360  "in the next node created. This message will is shown only once.",
2361  Parameters::kRtabmapDetectionRate().c_str(), rate_);
2362  warningShow = true;
2363  }
2364  userData_ = rtabmap_conversions::userDataFromROS(*dataMsg);
2365  }
2366 }
2367 
2368 void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg)
2369 {
2370  if(!paused_)
2371  {
2372  globalPose_ = *globalPoseMsg;
2373  }
2374 }
2375 
2376 void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg)
2377 {
2378  if(!paused_)
2379  {
2380  double error = 10.0;
2381  if(gpsFixMsg->position_covariance_type != sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN)
2382  {
2383  double variance = uMax3(gpsFixMsg->position_covariance.at(0), gpsFixMsg->position_covariance.at(4), gpsFixMsg->position_covariance.at(8));
2384  if(variance>0.0)
2385  {
2386  error = sqrt(variance);
2387  }
2388  }
2389  gps_ = rtabmap::GPS(
2390  gpsFixMsg->header.stamp.toSec(),
2391  gpsFixMsg->longitude,
2392  gpsFixMsg->latitude,
2393  gpsFixMsg->altitude,
2394  error,
2395  0);
2396  }
2397 }
2398 
2399 #ifdef WITH_APRILTAG_ROS
2400 void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections)
2401 {
2402  if(!paused_)
2403  {
2404  for(unsigned int i=0; i<tagDetections.detections.size(); ++i)
2405  {
2406  if(tagDetections.detections[i].id.size() >= 1)
2407  {
2408  geometry_msgs::PoseWithCovarianceStamped p = tagDetections.detections[i].pose;
2409  p.header = tagDetections.header;
2410  if(!tagDetections.detections[i].pose.header.frame_id.empty())
2411  {
2412  p.header.frame_id = tagDetections.detections[i].pose.header.frame_id;
2413 
2414  static bool warned = false;
2415  if(!warned &&
2416  !tagDetections.header.frame_id.empty() &&
2417  tagDetections.detections[i].pose.header.frame_id.compare(tagDetections.header.frame_id)!=0)
2418  {
2419  NODELET_WARN("frame_id set for individual tag detections (%s) doesn't match the frame_id of the message (%s), "
2420  "the resulting pose of the tag may be wrong. This message is only printed once.",
2421  tagDetections.detections[i].pose.header.frame_id.c_str(), tagDetections.header.frame_id.c_str());
2422  warned = true;
2423  }
2424  }
2425  if(!tagDetections.detections[i].pose.header.stamp.isZero())
2426  {
2427  p.header.stamp = tagDetections.detections[i].pose.header.stamp;
2428 
2429  static bool warned = false;
2430  if(!warned &&
2431  !tagDetections.header.stamp.isZero() &&
2432  tagDetections.detections[i].pose.header.stamp != tagDetections.header.stamp)
2433  {
2434  NODELET_WARN("stamp set for individual tag detections (%f) doesn't match the stamp of the message (%f), "
2435  "the resulting pose of the tag may be wrongly interpolated. This message is only printed once.",
2436  tagDetections.detections[i].pose.header.stamp.toSec(), tagDetections.header.stamp.toSec());
2437  warned = true;
2438  }
2439  }
2440  uInsert(tags_,
2441  std::make_pair(tagDetections.detections[i].id[0],
2442  std::make_pair(p, tagDetections.detections[i].size.size()==1?(float)tagDetections.detections[i].size[0]:0.0f)));
2443  }
2444  }
2445  }
2446 }
2447 #endif
2448 
2449 #ifdef WITH_FIDUCIAL_MSGS
2450 void CoreWrapper::fiducialDetectionsAsyncCallback(const fiducial_msgs::FiducialTransformArray & fiducialDetections)
2451 {
2452  if(!paused_)
2453  {
2454  for(unsigned int i=0; i<fiducialDetections.transforms.size(); ++i)
2455  {
2456  geometry_msgs::PoseWithCovarianceStamped p;
2457  p.pose.pose.orientation = fiducialDetections.transforms[i].transform.rotation;
2458  p.pose.pose.position.x = fiducialDetections.transforms[i].transform.translation.x;
2459  p.pose.pose.position.y = fiducialDetections.transforms[i].transform.translation.y;
2460  p.pose.pose.position.z = fiducialDetections.transforms[i].transform.translation.z;
2461  p.header = fiducialDetections.header;
2462  uInsert(tags_,
2463  std::make_pair(fiducialDetections.transforms[i].fiducial_id,
2464  std::make_pair(p, 0.0f)));
2465  }
2466  }
2467 }
2468 #endif
2469 
2470 void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
2471 {
2472  if(!paused_)
2473  {
2474  if(msg->orientation.x == 0 && msg->orientation.y == 0 && msg->orientation.z == 0 && msg->orientation.w == 0)
2475  {
2476  UERROR("IMU received doesn't have orientation set, it is ignored.");
2477  }
2478  else
2479  {
2480  Transform orientation(0,0,0, msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
2481  imus_.insert(std::make_pair(msg->header.stamp.toSec(), orientation));
2482  if(imus_.size() > 1000)
2483  {
2484  imus_.erase(imus_.begin());
2485  }
2486  if(!imuFrameId_.empty() && imuFrameId_.compare(msg->header.frame_id) != 0)
2487  {
2488  ROS_ERROR("IMU frame_id has changed from %s to %s! Are "
2489  "multiple nodes publishing "
2490  "on same topic %s? IMU buffer is cleared!",
2491  imuFrameId_.c_str(),
2492  msg->header.frame_id.c_str(),
2493  imuSub_.getTopic().c_str());
2494  imus_.clear();
2495  imuFrameId_.clear();
2496  }
2497  else
2498  {
2499  imuFrameId_ = msg->header.frame_id;
2500  }
2501  }
2502  }
2503 }
2504 
2505 void CoreWrapper::republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr& msg)
2506 {
2507  rtabmap_.addNodesToRepublish(msg->data);
2508 }
2509 
2510 void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
2511 {
2512  if(!paused_)
2513  {
2514  interOdoms_.push_back(std::make_pair(*msg, rtabmap_msgs::OdomInfo()));
2515  }
2516 }
2517 
2518 void CoreWrapper::interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_msgs::OdomInfoConstPtr & msg2)
2519 {
2520  if(!paused_)
2521  {
2522  interOdoms_.push_back(std::make_pair(*msg1, *msg2));
2523  }
2524 }
2525 
2526 
2527 void CoreWrapper::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg)
2528 {
2529  Transform mapToPose = Transform::getIdentity();
2530  if(msg->header.frame_id.empty())
2531  {
2532  NODELET_WARN("Received initialpose doesn't have frame_id set, assuming it is in %s frame.", mapFrameId_.c_str());
2533  }
2534  else if(msg->header.frame_id != mapFrameId_)
2535  {
2536  mapToPose = rtabmap_conversions::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
2537  if(mapToPose.isNull())
2538  {
2539  NODELET_ERROR("Failed to transform initialpose from frame %s to map frame %s", msg->header.frame_id.c_str(), mapFrameId_.c_str());
2540  return;
2541  }
2542  }
2543 
2544  Transform initialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
2545  if(initialPose.isNull())
2546  {
2547  NODELET_ERROR("initialpose received is null!");
2548  return;
2549  }
2550  if(mapToPose.isIdentity())
2551  {
2552  NODELET_INFO("initialpose received: %s", initialPose.prettyPrint().c_str());
2553  rtabmap_.setInitialPose(initialPose);
2554  }
2555  else
2556  {
2557  NODELET_INFO("initialpose received: %s in %s frame, transformed to %s in %s frame.",
2558  initialPose.prettyPrint().c_str(),
2559  msg->header.frame_id.c_str(),
2560  (mapToPose * initialPose).prettyPrint().c_str(),
2561  mapFrameId_.c_str());
2562  rtabmap_.setInitialPose(mapToPose*initialPose);
2563  }
2564 }
2565 
2566 void CoreWrapper::goalCommonCallback(
2567  int id,
2568  const std::string & label,
2569  const std::string & frameId,
2570  const Transform & pose,
2571  const ros::Time & stamp,
2572  double * planningTime)
2573 {
2574  UTimer timer;
2575 
2576  if(id == 0 && !label.empty() && rtabmap_.getMemory())
2577  {
2578  id = rtabmap_.getMemory()->getSignatureIdByLabel(label);
2579  }
2580 
2581  if(id > 0)
2582  {
2583  NODELET_INFO("Planning: set goal to node %d", id);
2584  }
2585  else if(id < 0)
2586  {
2587  NODELET_INFO("Planning: set goal to landmark %d", id);
2588  }
2589  else if(!pose.isNull())
2590  {
2591  NODELET_INFO("Planning: set goal %s", pose.prettyPrint().c_str());
2592  }
2593 
2594  if(planningTime)
2595  {
2596  *planningTime = 0.0;
2597  }
2598 
2599  bool success = false;
2600  if((id != 0 && rtabmap_.computePath(id, true)) ||
2601  (!pose.isNull() && rtabmap_.computePath(pose)))
2602  {
2603  if(planningTime)
2604  {
2605  *planningTime = timer.elapsed();
2606  }
2607  NODELET_INFO("Planning: Time computing path = %f s", timer.ticks());
2608  const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
2609 
2610  currentMetricGoal_.setNull();
2611  lastPublishedMetricGoal_.setNull();
2612  goalFrameId_.clear();
2613  latestNodeWasReached_ = false;
2614  if(poses.size() == 0)
2615  {
2616  NODELET_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).",
2617  rtabmap_.getGoalReachedRadius());
2618  rtabmap_.clearPath(1);
2619  if(goalReachedPub_.getNumSubscribers())
2620  {
2621  std_msgs::Bool result;
2622  result.data = true;
2623  goalReachedPub_.publish(result);
2624  }
2625  success = true;
2626  }
2627  else
2628  {
2629  currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
2630  if(!currentMetricGoal_.isNull())
2631  {
2632  NODELET_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
2633  goalFrameId_ = frameId;
2634 
2635  // Adjust the target pose relative to last node
2636  if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
2637  {
2638  if(rtabmap_.getLastLocalizationPose().getDistance(currentMetricGoal_) < rtabmap_.getLocalRadius())
2639  {
2640  latestNodeWasReached_ = true;
2641  Transform goalLocalTransform = Transform::getIdentity();
2642  if(!goalFrameId_.empty() && goalFrameId_.compare(frameId_) != 0)
2643  {
2644  Transform localT = rtabmap_conversions::getTransform(frameId_, goalFrameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
2645  if(!localT.isNull())
2646  {
2647  goalLocalTransform = localT.inverse().to3DoF();
2648  }
2649  }
2650  currentMetricGoal_ *= rtabmap_.getPathTransformToGoal() * goalLocalTransform;
2651  }
2652  }
2653 
2654  publishCurrentGoal(stamp);
2655  publishLocalPath(stamp);
2656  publishGlobalPath(stamp);
2657 
2658  // Just output the path on screen
2659  std::stringstream stream;
2660  for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2661  {
2662  if(iter != poses.begin())
2663  {
2664  stream << " ";
2665  }
2666  stream << iter->first;
2667  }
2668  NODELET_INFO("Global path: [%s]", stream.str().c_str());
2669  success=true;
2670  }
2671  else
2672  {
2673  NODELET_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
2674  }
2675  }
2676  }
2677  else if(!label.empty())
2678  {
2679  NODELET_ERROR("Planning: Node with label \"%s\" not found!", label.c_str());
2680  }
2681  else if(pose.isNull())
2682  {
2683  if(id > 0)
2684  {
2685  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);
2686  }
2687  else if(id < 0)
2688  {
2689  NODELET_ERROR("Planning: Could not plan to landmark %d! The landmark is not in map's graph (look for warnings before this message for more details).", id);
2690  }
2691  else
2692  {
2693  NODELET_ERROR("Planning: Node id should be > 0 !");
2694  }
2695  }
2696  else
2697  {
2698  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());
2699  }
2700 
2701  if(!success)
2702  {
2703  rtabmap_.clearPath(-1);
2704  if(goalReachedPub_.getNumSubscribers())
2705  {
2706  std_msgs::Bool result;
2707  result.data = false;
2708  goalReachedPub_.publish(result);
2709  }
2710  }
2711 }
2712 
2713 void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
2714 {
2715  Transform targetPose = rtabmap_conversions::transformFromPoseMsg(msg->pose, true);
2716 
2717  // transform goal in /map frame
2718  if(!msg->header.frame_id.empty() && mapFrameId_.compare(msg->header.frame_id) != 0)
2719  {
2720  Transform t = rtabmap_conversions::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
2721  if(t.isNull())
2722  {
2723  NODELET_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",
2724  msg->header.frame_id.c_str(), mapFrameId_.c_str());
2725  if(goalReachedPub_.getNumSubscribers())
2726  {
2727  std_msgs::Bool result;
2728  result.data = false;
2729  goalReachedPub_.publish(result);
2730  }
2731  return;
2732  }
2733  targetPose = t * targetPose;
2734  }
2735  // else assume map frame if not set
2736 
2737  goalCommonCallback(0, "", "", targetPose, msg->header.stamp);
2738 }
2739 
2740 void CoreWrapper::goalNodeCallback(const rtabmap_msgs::GoalConstPtr & msg)
2741 {
2742  if(msg->node_id == 0 && msg->node_label.empty())
2743  {
2744  NODELET_ERROR("Node id or label should be set!");
2745  if(goalReachedPub_.getNumSubscribers())
2746  {
2747  std_msgs::Bool result;
2748  result.data = false;
2749  goalReachedPub_.publish(result);
2750  }
2751  return;
2752  }
2753  goalCommonCallback(msg->node_id, msg->node_label, msg->frame_id, Transform(), msg->header.stamp);
2754 }
2755 
2756 bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2757 {
2758  ros::NodeHandle & pnh = getPrivateNodeHandle();
2759  for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
2760  {
2761  std::string vStr;
2762  bool vBool;
2763  int vInt;
2764  double vDouble;
2765  if(pnh.getParam(iter->first, vStr))
2766  {
2767  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
2768  iter->second = vStr;
2769  }
2770  else if(pnh.getParam(iter->first, vBool))
2771  {
2772  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
2773  iter->second = uBool2Str(vBool);
2774  }
2775  else if(pnh.getParam(iter->first, vInt))
2776  {
2777  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
2778  iter->second = uNumber2Str(vInt).c_str();
2779  }
2780  else if(pnh.getParam(iter->first, vDouble))
2781  {
2782  NODELET_INFO("Setting RTAB-Map parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
2783  iter->second = uNumber2Str(vDouble).c_str();
2784  }
2785  }
2786  NODELET_INFO("rtabmap: Updating parameters");
2787  if(parameters_.find(Parameters::kRtabmapDetectionRate()) != parameters_.end())
2788  {
2789  rate_ = uStr2Float(parameters_.at(Parameters::kRtabmapDetectionRate()));
2790  NODELET_INFO("RTAB-Map rate detection = %f Hz", rate_);
2791  }
2792  if(parameters_.find(Parameters::kRtabmapCreateIntermediateNodes()) != parameters_.end())
2793  {
2794  createIntermediateNodes_ = uStr2Bool(parameters_.at(Parameters::kRtabmapCreateIntermediateNodes()));
2795  NODELET_INFO("Create intermediate nodes = %s", createIntermediateNodes_?"true":"false");
2796  }
2797  if(parameters_.find(Parameters::kGridGlobalMaxNodes()) != parameters_.end())
2798  {
2799  mappingMaxNodes_ = uStr2Int(parameters_.at(Parameters::kGridGlobalMaxNodes()));
2800  NODELET_INFO("Max mapping nodes = %d", mappingMaxNodes_);
2801  }
2802  if(parameters_.find(Parameters::kGridGlobalAltitudeDelta()) != parameters_.end())
2803  {
2804  mappingAltitudeDelta_ = uStr2Float(parameters_.at(Parameters::kGridGlobalAltitudeDelta()));
2805  NODELET_INFO("Mapping altitude delta = %f", mappingAltitudeDelta_);
2806  }
2807  if(parameters_.find(Parameters::kRtabmapImagesAlreadyRectified()) != parameters_.end())
2808  {
2809  alreadyRectifiedImages_ = uStr2Bool(parameters_.at(Parameters::kRtabmapImagesAlreadyRectified()));
2810  NODELET_INFO("Already rectified images = %s", alreadyRectifiedImages_?"true":"false");
2811  }
2812  if(parameters_.find(Parameters::kRegForce3DoF()) != parameters_.end())
2813  {
2814  twoDMapping_= uStr2Bool(parameters_.at(Parameters::kRegForce3DoF()));
2815  NODELET_INFO("2D mapping = %s", twoDMapping_?"true":"false");
2816  }
2817  rtabmap_.parseParameters(parameters_);
2818  // Don't reset map in localization mode
2819  if(rtabmap_.getMemory()->isIncremental()) {
2820  mapsManager_.setParameters(parameters_);
2821  }
2822  return true;
2823 }
2824 
2825 bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2826 {
2827  NODELET_INFO("rtabmap: Reset");
2828  rtabmap_.resetMemory();
2829  covariance_ = cv::Mat();
2830  lastPose_.setIdentity();
2831  lastPoseVelocity_.clear();
2832  lastPoseIntermediate_ = false;
2833  currentMetricGoal_.setNull();
2834  lastPublishedMetricGoal_.setNull();
2835  goalFrameId_.clear();
2836  latestNodeWasReached_ = false;
2837  graphLatched_ = false;
2838  mapsManager_.clear();
2839  previousStamp_ = ros::Time(0);
2840  globalPose_.header.stamp = ros::Time(0);
2841  gps_ = rtabmap::GPS();
2842  tags_.clear();
2843  userDataMutex_.lock();
2844  userData_ = cv::Mat();
2845  userDataMutex_.unlock();
2846  imus_.clear();
2847  imuFrameId_.clear();
2848  interOdoms_.clear();
2849  mapToOdomMutex_.lock();
2850  mapToOdom_.setIdentity();
2851  mapToOdomMutex_.unlock();
2852 
2853  return true;
2854 }
2855 
2856 bool CoreWrapper::pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2857 {
2858  if(paused_)
2859  {
2860  NODELET_WARN("rtabmap: Already paused!");
2861  }
2862  else
2863  {
2864  paused_ = true;
2865  NODELET_INFO("rtabmap: paused!");
2866  getPrivateNodeHandle().setParam("is_rtabmap_paused", true);
2867  }
2868  return true;
2869 }
2870 
2871 bool CoreWrapper::resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
2872 {
2873  if(!paused_)
2874  {
2875  NODELET_WARN("rtabmap: Already running!");
2876  }
2877  else
2878  {
2879  paused_ = false;
2880  NODELET_INFO("rtabmap: resumed!");
2881  getPrivateNodeHandle().setParam("is_rtabmap_paused", false);
2882  }
2883  return true;
2884 }
2885 
2886 bool CoreWrapper::loadDatabaseCallback(rtabmap_msgs::LoadDatabase::Request& req, rtabmap_msgs::LoadDatabase::Response&)
2887 {
2888  NODELET_INFO("LoadDatabase: Loading database (%s, clear=%s)...", req.database_path.c_str(), req.clear?"true":"false");
2889  std::string newDatabasePath = uReplaceChar(req.database_path, '~', UDirectory::homeDir());
2890  std::string dir = UDirectory::getDir(newDatabasePath);
2891  if(!UDirectory::exists(dir))
2892  {
2893  ROS_ERROR("Directory %s doesn't exist! Cannot load database \"%s\"", newDatabasePath.c_str(), dir.c_str());
2894  return false;
2895  }
2896 
2897  if(UFile::exists(newDatabasePath) && req.clear)
2898  {
2899  UFile::erase(newDatabasePath);
2900  }
2901 
2902  // Close old database
2903  NODELET_INFO("LoadDatabase: Saving current map (%s)...", databasePath_.c_str());
2904  if(rtabmap_.getMemory())
2905  {
2906  // save the grid map
2907  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
2908  cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
2909  if(!pixels.empty())
2910  {
2911  printf("rtabmap: 2D occupancy grid map saved.\n");
2912  rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
2913  }
2914  }
2915  rtabmap_.close();
2916  NODELET_INFO("LoadDatabase: Saving current map (%s, %ld MB)... done!", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
2917 
2918  covariance_ = cv::Mat();
2919  lastPose_.setIdentity();
2920  lastPoseVelocity_.clear();
2921  lastPoseIntermediate_ = false;
2922  currentMetricGoal_.setNull();
2923  lastPublishedMetricGoal_.setNull();
2924  goalFrameId_.clear();
2925  latestNodeWasReached_ = false;
2926  graphLatched_ = false;
2927  mapsManager_.clear();
2928  previousStamp_ = ros::Time(0);
2929  globalPose_.header.stamp = ros::Time(0);
2930  gps_ = rtabmap::GPS();
2931  tags_.clear();
2932  userDataMutex_.lock();
2933  userData_ = cv::Mat();
2934  userDataMutex_.unlock();
2935  imus_.clear();
2936  imuFrameId_.clear();
2937  interOdoms_.clear();
2938  mapToOdomMutex_.lock();
2939  mapToOdom_.setIdentity();
2940  mapToOdomMutex_.unlock();
2941 
2942  // Open new database
2943  databasePath_ = newDatabasePath;
2944 
2945  // modify default parameters with those in the database
2946  if(!req.clear && UFile::exists(databasePath_))
2947  {
2948  ParametersMap dbParameters;
2949  rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
2950  if(driver->openConnection(databasePath_))
2951  {
2952  dbParameters = driver->getLastParameters(); // parameter migration is already done
2953  }
2954  delete driver;
2955  for(ParametersMap::iterator iter=dbParameters.begin(); iter!=dbParameters.end(); ++iter)
2956  {
2957  if(iter->first.compare(Parameters::kRtabmapWorkingDirectory()) == 0)
2958  {
2959  // ignore working directory
2960  continue;
2961  }
2962  if(parameters_.find(iter->first) == parameters_.end() &&
2963  parameters_.find(iter->first)->second.compare(iter->second) !=0)
2964  {
2965  NODELET_WARN("RTAB-Map parameter \"%s\" from database (%s) is different "
2966  "from the current used one (%s). We still keep the "
2967  "current parameter value (%s). If you want to switch between databases "
2968  "with different configurations, restart rtabmap node instead of using this service.",
2969  iter->first.c_str(), iter->second.c_str(),
2970  parameters_.find(iter->first)->second.c_str(),
2971  parameters_.find(iter->first)->second.c_str());
2972  }
2973  }
2974  }
2975 
2976  NODELET_INFO("LoadDatabase: Loading database...");
2977  rtabmap_.init(parameters_, databasePath_);
2978  NODELET_INFO("LoadDatabase: Loading database... done!");
2979 
2980  if(rtabmap_.getMemory())
2981  {
2982  if(useSavedMap_ && !rtabmap_.getMemory()->isIncremental())
2983  {
2984  float xMin, yMin, gridCellSize;
2985  cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
2986  if(!map.empty())
2987  {
2988  NODELET_INFO("LoadDatabase: 2D occupancy grid map loaded (%dx%d).", map.cols, map.rows);
2989  mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
2990  }
2991  }
2992 
2993  if(rtabmap_.getMemory()->getWorkingMem().size()>1)
2994  {
2995  NODELET_INFO("LoadDatabase: Working Memory = %d, Local map = %d.",
2996  (int)rtabmap_.getMemory()->getWorkingMem().size()-1,
2997  (int)rtabmap_.getLocalOptimizedPoses().size());
2998  }
2999 
3000  if(databasePath_.size())
3001  {
3002  NODELET_INFO("LoadDatabase: Database version = \"%s\".", rtabmap_.getMemory()->getDatabaseVersion().c_str());
3003  }
3004 
3005  if(rtabmap_.getMemory()->isIncremental())
3006  {
3007  NODELET_INFO("LoadDatabase: SLAM mode (%s=true)", Parameters::kMemIncrementalMemory().c_str());
3008  }
3009  else
3010  {
3011  NODELET_INFO("LoadDatabase: Localization mode (%s=false)", Parameters::kMemIncrementalMemory().c_str());
3012  }
3013 
3014  return true;
3015  }
3016 
3017  return false;
3018 }
3019 
3020 bool CoreWrapper::triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
3021 {
3022  NODELET_INFO("rtabmap: Trigger new map");
3023  rtabmap_.triggerNewMap();
3024  return true;
3025 }
3026 
3027 bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
3028 {
3029  NODELET_INFO("Backup: Saving memory...");
3030  if(rtabmap_.getMemory())
3031  {
3032  // save the grid map
3033  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
3034  cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
3035  if(!pixels.empty())
3036  {
3037  printf("rtabmap: 2D occupancy grid map saved.\n");
3038  rtabmap_.getMemory()->save2DMap(pixels, xMin, yMin, gridCellSize);
3039  }
3040  }
3041  rtabmap_.close();
3042  NODELET_INFO("Backup: Saving memory... done!");
3043 
3044  covariance_ = cv::Mat();
3045  lastPose_.setIdentity();
3046  lastPoseVelocity_.clear();
3047  currentMetricGoal_.setNull();
3048  lastPublishedMetricGoal_.setNull();
3049  goalFrameId_.clear();
3050  latestNodeWasReached_ = false;
3051  graphLatched_ = false;
3052  userDataMutex_.lock();
3053  userData_ = cv::Mat();
3054  userDataMutex_.unlock();
3055  globalPose_.header.stamp = ros::Time(0);
3056  gps_ = rtabmap::GPS();
3057  tags_.clear();
3058 
3059  NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());
3060  UFile::copy(databasePath_, databasePath_+".back");
3061  NODELET_INFO("Backup: Saving \"%s\" to \"%s\"... done!", databasePath_.c_str(), (databasePath_+".back").c_str());
3062 
3063  NODELET_INFO("Backup: Reloading memory...");
3064  rtabmap_.init(parameters_, databasePath_);
3065  NODELET_INFO("Backup: Reloading memory... done!");
3066 
3067  return true;
3068 }
3069 
3070 void CoreWrapper::republishMaps()
3071 {
3072  ros::Time stamp = ros::Time::now();
3073  mapsManager_.publishMaps(rtabmap_.getLocalOptimizedPoses(), stamp, mapFrameId_);
3074 
3075  if(mapDataPub_.getNumSubscribers())
3076  {
3077  rtabmap_msgs::MapDataPtr msg(new rtabmap_msgs::MapData);
3078  msg->header.stamp = stamp;
3079  msg->header.frame_id = mapFrameId_;
3080 
3081  rtabmap_conversions::mapDataToROS(
3082  rtabmap_.getLocalOptimizedPoses(),
3083  rtabmap_.getLocalConstraints(),
3084  std::map<int, Signature>(),
3085  rtabmap_.getMapCorrection(),
3086  *msg);
3087 
3088  mapDataPub_.publish(msg);
3089  }
3090 
3091  if(mapGraphPub_.getNumSubscribers())
3092  {
3093  rtabmap_msgs::MapGraphPtr msg(new rtabmap_msgs::MapGraph);
3094  msg->header.stamp = stamp;
3095  msg->header.frame_id = mapFrameId_;
3096 
3097  rtabmap_conversions::mapGraphToROS(
3098  rtabmap_.getLocalOptimizedPoses(),
3099  rtabmap_.getLocalConstraints(),
3100  rtabmap_.getMapCorrection(),
3101  *msg);
3102 
3103  mapGraphPub_.publish(msg);
3104  }
3105 }
3106 
3107 bool CoreWrapper::detectMoreLoopClosuresCallback(rtabmap_msgs::DetectMoreLoopClosures::Request& req, rtabmap_msgs::DetectMoreLoopClosures::Response& res)
3108 {
3109  NODELET_WARN("Detect more loop closures service called");
3110 
3111  UTimer timer;
3112  float clusterRadiusMax = 1;
3113  float clusterRadiusMin = 0;
3114  float clusterAngle = 0;
3115  int iterations = 1;
3116  bool intraSession = true;
3117  bool interSession = true;
3118  if(req.cluster_radius_max > 0.0f)
3119  {
3120  clusterRadiusMax = req.cluster_radius_max;
3121  }
3122  if(req.cluster_radius_min >= 0.0f)
3123  {
3124  clusterRadiusMin = req.cluster_radius_min;
3125  }
3126  if(req.cluster_angle >= 0.0f)
3127  {
3128  clusterAngle = req.cluster_angle;
3129  }
3130  if(req.iterations >= 1.0f)
3131  {
3132  iterations = (int)req.iterations;
3133  }
3134  if(req.intra_only)
3135  {
3136  interSession = false;
3137  }
3138  else if(req.inter_only)
3139  {
3140  intraSession = false;
3141  }
3142  NODELET_WARN("Post-Processing service called: Detecting more loop closures "
3143  "(max radius=%f, min radius=%f, angle=%f, iterations=%d, intra=%s, inter=%s)...",
3144  clusterRadiusMax,
3145  clusterRadiusMin,
3146  clusterAngle,
3147  iterations,
3148  intraSession?"true":"false",
3149  interSession?"true":"false");
3150  res.detected = rtabmap_.detectMoreLoopClosures(
3151  clusterRadiusMax,
3152  clusterAngle*M_PI/180.0,
3153  iterations,
3154  intraSession,
3155  interSession,
3156  0,
3157  clusterRadiusMin);
3158  if(res.detected<0)
3159  {
3160  NODELET_ERROR("Post-Processing: Detecting more loop closures failed!");
3161  }
3162  else
3163  {
3164  NODELET_WARN("Post-Processing: Detected %d loop closures! (%fs)", res.detected, timer.ticks());
3165 
3166  if(res.detected>0)
3167  {
3168  republishMaps();
3169  }
3170  return true;
3171  }
3172  return false;
3173 }
3174 
3175 bool CoreWrapper::cleanupLocalGridsCallback(rtabmap_msgs::CleanupLocalGrids::Request& req, rtabmap_msgs::CleanupLocalGrids::Response& res)
3176 {
3177  NODELET_WARN("Cleanup local grids service called");
3178  UTimer timer;
3179  int radius = 1;
3180  bool filterScans = false;
3181  if(req.radius > 1.0f)
3182  {
3183  radius = (int)req.radius;
3184  }
3185  filterScans = req.filter_scans;
3186  float xMin, yMin, gridCellSize;
3187  cv::Mat map = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
3188  if(map.empty())
3189  {
3190  NODELET_ERROR("Post-Processing: Cleanup local grids failed! There is no optimized map.");
3191  return false;
3192  }
3193  std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
3194  NODELET_WARN("Post-Processing: Cleanup local grids... (radius=%d, filter scans=%s)",
3195  radius,
3196  filterScans?"true":"false");
3197  res.modified = rtabmap_.cleanupLocalGrids(poses, map, xMin, yMin, gridCellSize, radius, filterScans);
3198  if(res.modified<0)
3199  {
3200  NODELET_ERROR("Post-Processing: Cleanup local grids failed!");
3201  }
3202  else
3203  {
3204  if(filterScans)
3205  {
3206  NODELET_WARN("Post-Processing: %d grids and scans modified! (%fs)", res.modified, timer.ticks());
3207  }
3208  else
3209  {
3210  NODELET_WARN("Post-Processing: %d grids modified! (%fs)", res.modified, timer.ticks());
3211  }
3212  if(res.modified > 0)
3213  {
3214  // We should update MapsManager's cache with the modifications
3215  mapsManager_.clear();
3216  mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
3217 
3218  republishMaps();
3219  }
3220  return true;
3221  }
3222 
3223  return false;
3224 }
3225 bool CoreWrapper::globalBundleAdjustmentCallback(rtabmap_msgs::GlobalBundleAdjustment::Request& req, rtabmap_msgs::GlobalBundleAdjustment::Response& res)
3226 {
3227  NODELET_WARN("Global bundle adjustment service called");
3228 
3229  UTimer timer;
3230  int optimizer = (int)Optimizer::kTypeG2O; // g2o
3231  int iterations = Parameters::defaultOptimizerIterations();
3232  float pixelVariance = Parameters::defaultg2oPixelVariance();
3233  bool rematchFeatures = true;
3234  Parameters::parse(parameters_, Parameters::kOptimizerIterations(), iterations);
3235  Parameters::parse(parameters_, Parameters::kg2oPixelVariance(), pixelVariance);
3236  if(req.type == 1.0f)
3237  {
3238  optimizer = (int)Optimizer::kTypeCVSBA;
3239  }
3240  if(req.iterations >= 1.0f)
3241  {
3242  iterations = req.iterations;
3243  }
3244  if(req.pixel_variance > 0.0f)
3245  {
3246  pixelVariance = req.pixel_variance;
3247  }
3248  rematchFeatures = !req.voc_matches;
3249 
3250  NODELET_WARN("Post-Processing: Global Bundle Adjustment... "
3251  "(Optimizer=%s, iterations=%d, pixel variance=%f, rematch=%s)...",
3252  optimizer==Optimizer::kTypeG2O?"g2o":"cvsba",
3253  iterations,
3254  pixelVariance,
3255  rematchFeatures?"true":"false");
3256  bool success = rtabmap_.globalBundleAdjustment((Optimizer::Type)optimizer, rematchFeatures, iterations, pixelVariance);
3257  if(!success)
3258  {
3259  NODELET_ERROR("Post-Processing: Global Bundle Adjustment failed!");
3260  }
3261  else
3262  {
3263  NODELET_WARN("Post-Processing: Global Bundle Adjustment... done! (%fs)", timer.ticks());
3264  republishMaps();
3265  return true;
3266  }
3267 
3268  return false;
3269 }
3270 
3271 bool CoreWrapper::setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
3272 {
3273  NODELET_INFO("rtabmap: Set localization mode");
3274  rtabmap::ParametersMap parameters;
3275  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
3276  ros::NodeHandle & nh = getNodeHandle();
3277  nh.setParam(rtabmap::Parameters::kMemIncrementalMemory(), "false");
3278  rtabmap_.parseParameters(parameters);
3279  NODELET_INFO("rtabmap: Localization mode enabled!");
3280  return true;
3281 }
3282 
3283 bool CoreWrapper::setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
3284 {
3285  NODELET_INFO("rtabmap: Set mapping mode");
3286  rtabmap::ParametersMap parameters;
3287  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "true"));
3288  ros::NodeHandle & nh = getNodeHandle();
3289  nh.setParam(rtabmap::Parameters::kMemIncrementalMemory(), "true");
3290  rtabmap_.parseParameters(parameters);
3291  NODELET_INFO("rtabmap: Mapping mode enabled!");
3292  return true;
3293 }
3294 
3295 bool CoreWrapper::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
3296 {
3297  NODELET_INFO("rtabmap: Set log level to Debug");
3298  ULogger::setLevel(ULogger::kDebug);
3299  return true;
3300 }
3301 bool CoreWrapper::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
3302 {
3303  NODELET_INFO("rtabmap: Set log level to Info");
3304  ULogger::setLevel(ULogger::kInfo);
3305  return true;
3306 }
3307 bool CoreWrapper::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
3308 {
3309  NODELET_INFO("rtabmap: Set log level to Warning");
3310  ULogger::setLevel(ULogger::kWarning);
3311  return true;
3312 }
3313 bool CoreWrapper::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
3314 {
3315  NODELET_INFO("rtabmap: Set log level to Error");
3316  ULogger::setLevel(ULogger::kError);
3317  return true;
3318 }
3319 
3320 bool CoreWrapper::getNodeDataCallback(rtabmap_msgs::GetNodeData::Request& req, rtabmap_msgs::GetNodeData::Response& res)
3321 {
3322  NODELET_INFO("rtabmap: Getting node data (%d node(s), images=%s scan=%s grid=%s user_data=%s)...",
3323  (int)req.ids.size(),
3324  req.images?"true":"false",
3325  req.scan?"true":"false",
3326  req.grid?"true":"false",
3327  req.user_data?"true":"false");
3328 
3329  if(req.ids.empty() && rtabmap_.getMemory() && rtabmap_.getMemory()->getLastWorkingSignature())
3330  {
3331  req.ids.push_back(rtabmap_.getMemory()->getLastWorkingSignature()->id());
3332  }
3333  for(size_t i=0; i<req.ids.size(); ++i)
3334  {
3335  int id = req.ids[i];
3336  Signature s = rtabmap_.getSignatureCopy(id, req.images, req.scan, req.user_data, req.grid, true, true);
3337 
3338  if(s.id()>0)
3339  {
3340  rtabmap_msgs::Node msg;
3341  rtabmap_conversions::nodeToROS(s, msg);
3342  res.data.push_back(msg);
3343  }
3344  }
3345 
3346  return !res.data.empty();
3347 }
3348 
3349 bool CoreWrapper::getMapDataCallback(rtabmap_msgs::GetMap::Request& req, rtabmap_msgs::GetMap::Response& res)
3350 {
3351  NODELET_INFO("rtabmap: Getting map (global=%s optimized=%s graphOnly=%s)...",
3352  req.global?"true":"false",
3353  req.optimized?"true":"false",
3354  req.graphOnly?"true":"false");
3355  std::map<int, Signature> signatures;
3356  std::map<int, Transform> poses;
3357  std::multimap<int, rtabmap::Link> constraints;
3358 
3359  rtabmap_.getGraph(
3360  poses,
3361  constraints,
3362  req.optimized,
3363  req.global,
3364  &signatures,
3365  !req.graphOnly,
3366  !req.graphOnly,
3367  !req.graphOnly,
3368  !req.graphOnly);
3369 
3370  //RGB-D SLAM data
3371  rtabmap_conversions::mapDataToROS(poses,
3372  constraints,
3373  signatures,
3374  mapToOdom_,
3375  res.data);
3376 
3377  res.data.header.stamp = ros::Time::now();
3378  res.data.header.frame_id = mapFrameId_;
3379 
3380  return true;
3381 }
3382 
3383 bool CoreWrapper::getMapData2Callback(rtabmap_msgs::GetMap2::Request& req, rtabmap_msgs::GetMap2::Response& res)
3384 {
3385  NODELET_INFO("rtabmap: Getting map (global=%s optimized=%s with_images=%s with_scans=%s with_user_data=%s with_grids=%s)...",
3386  req.global?"true":"false",
3387  req.optimized?"true":"false",
3388  req.with_images?"true":"false",
3389  req.with_scans?"true":"false",
3390  req.with_user_data?"true":"false",
3391  req.with_grids?"true":"false");
3392  std::map<int, Signature> signatures;
3393  std::map<int, Transform> poses;
3394  std::multimap<int, rtabmap::Link> constraints;
3395 
3396  rtabmap_.getGraph(
3397  poses,
3398  constraints,
3399  req.optimized,
3400  req.global,
3401  &signatures,
3402  req.with_images,
3403  req.with_scans,
3404  req.with_user_data,
3405  req.with_grids,
3406  req.with_words,
3407  req.with_global_descriptors);
3408 
3409  //RGB-D SLAM data
3410  rtabmap_conversions::mapDataToROS(poses,
3411  constraints,
3412  signatures,
3413  mapToOdom_,
3414  res.data);
3415 
3416  res.data.header.stamp = ros::Time::now();
3417  res.data.header.frame_id = mapFrameId_;
3418 
3419  return true;
3420 }
3421 
3422 bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
3423 {
3424  if(parameters_.find(Parameters::kGridSensor()) != parameters_.end() &&
3425  uStr2Int(parameters_.at(Parameters::kGridSensor()))==0)
3426  {
3427  NODELET_WARN("/get_proj_map service is deprecated! Call /get_grid_map service "
3428  "instead with <param name=\"%s\" type=\"string\" value=\"1\"/>. "
3429  "Do \"$ rosrun rtabmap_slam rtabmap --params | grep Grid\" to see "
3430  "all occupancy grid parameters.",
3431  Parameters::kGridSensor().c_str());
3432  }
3433  else
3434  {
3435  NODELET_WARN("/get_proj_map service is deprecated! Call /get_grid_map service instead.");
3436  }
3437  return getGridMapCallback(req, res);
3438 }
3439 
3440 bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
3441 {
3442  NODELET_WARN("/get_grid_map service is deprecated! Call /get_map service instead.");
3443  return getMapCallback(req, res);
3444 }
3445 
3446 bool CoreWrapper::getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
3447 {
3448  // Make sure grid map cache is up to date (in case there is no subscriber on map topics)
3449  std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
3450  mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false);
3451 
3452  // create the grid map
3453  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
3454  cv::Mat pixels = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
3455 
3456  if(!pixels.empty())
3457  {
3458  //init
3459  res.map.info.resolution = gridCellSize;
3460  res.map.info.origin.position.x = 0.0;
3461  res.map.info.origin.position.y = 0.0;
3462  res.map.info.origin.position.z = 0.0;
3463  res.map.info.origin.orientation.x = 0.0;
3464  res.map.info.origin.orientation.y = 0.0;
3465  res.map.info.origin.orientation.z = 0.0;
3466  res.map.info.origin.orientation.w = 1.0;
3467 
3468  res.map.info.width = pixels.cols;
3469  res.map.info.height = pixels.rows;
3470  res.map.info.origin.position.x = xMin;
3471  res.map.info.origin.position.y = yMin;
3472  res.map.data.resize(res.map.info.width * res.map.info.height);
3473 
3474  memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
3475 
3476  res.map.header.frame_id = mapFrameId_;
3477  res.map.header.stamp = ros::Time::now();
3478  return true;
3479  }
3480  else
3481  {
3482  NODELET_WARN("rtabmap: The map is empty!");
3483  }
3484  return false;
3485 }
3486 
3487 bool CoreWrapper::getProbMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
3488 {
3489  // Make sure grid map cache is up to date (in case there is no subscriber on map topics)
3490  std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
3491  mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false);
3492 
3493  // create the grid map
3494  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
3495  cv::Mat pixels = mapsManager_.getGridProbMap(xMin, yMin, gridCellSize);
3496 
3497  if(!pixels.empty())
3498  {
3499  //init
3500  res.map.info.resolution = gridCellSize;
3501  res.map.info.origin.position.x = 0.0;
3502  res.map.info.origin.position.y = 0.0;
3503  res.map.info.origin.position.z = 0.0;
3504  res.map.info.origin.orientation.x = 0.0;
3505  res.map.info.origin.orientation.y = 0.0;
3506  res.map.info.origin.orientation.z = 0.0;
3507  res.map.info.origin.orientation.w = 1.0;
3508 
3509  res.map.info.width = pixels.cols;
3510  res.map.info.height = pixels.rows;
3511  res.map.info.origin.position.x = xMin;
3512  res.map.info.origin.position.y = yMin;
3513  res.map.data.resize(res.map.info.width * res.map.info.height);
3514 
3515  memcpy(res.map.data.data(), pixels.data, res.map.info.width * res.map.info.height);
3516 
3517  res.map.header.frame_id = mapFrameId_;
3518  res.map.header.stamp = ros::Time::now();
3519  return true;
3520  }
3521  else
3522  {
3523  NODELET_WARN("rtabmap: The map is empty!");
3524  }
3525  return false;
3526 }
3527 
3528 bool CoreWrapper::publishMapCallback(rtabmap_msgs::PublishMap::Request& req, rtabmap_msgs::PublishMap::Response& res)
3529 {
3530  NODELET_INFO("rtabmap: Publishing map...");
3531 
3532  ros::Time now = ros::Time::now();
3533 
3534  if(mapDataPub_.getNumSubscribers() ||
3535  (!req.graphOnly && mapsManager_.hasSubscribers()) ||
3536  (req.graphOnly && (labelsPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers() || mapPathPub_.getNumSubscribers())))
3537  {
3538  std::map<int, Transform> poses;
3539  std::multimap<int, rtabmap::Link> constraints;
3540  std::map<int, Signature > signatures;
3541 
3542  rtabmap_.getGraph(
3543  poses,
3544  constraints,
3545  req.optimized,
3546  req.global,
3547  &signatures,
3548  !req.graphOnly,
3549  !req.graphOnly,
3550  !req.graphOnly,
3551  !req.graphOnly);
3552 
3553  if(mapDataPub_.getNumSubscribers())
3554  {
3555  rtabmap_msgs::MapDataPtr msg(new rtabmap_msgs::MapData);
3556  msg->header.stamp = now;
3557  msg->header.frame_id = mapFrameId_;
3558 
3559  rtabmap_conversions::mapDataToROS(poses,
3560  constraints,
3561  signatures,
3562  mapToOdom_,
3563  *msg);
3564 
3565  mapDataPub_.publish(msg);
3566  }
3567 
3568  if(mapGraphPub_.getNumSubscribers())
3569  {
3570  rtabmap_msgs::MapGraphPtr msg(new rtabmap_msgs::MapGraph);
3571  msg->header.stamp = now;
3572  msg->header.frame_id = mapFrameId_;
3573 
3574  rtabmap_conversions::mapGraphToROS(poses,
3575  constraints,
3576  mapToOdom_,
3577  *msg);
3578 
3579  mapGraphPub_.publish(msg);
3580  }
3581 
3582  bool pubLabels = labelsPub_.getNumSubscribers();
3583  visualization_msgs::MarkerArray markers;
3584  if((landmarksPub_.getNumSubscribers() || pubLabels) && !poses.empty() && poses.begin()->first < 0)
3585  {
3586  geometry_msgs::PoseArrayPtr msg(new geometry_msgs::PoseArray);
3587  msg->header.stamp = now;
3588  msg->header.frame_id = mapFrameId_;
3589  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end() && iter->first<0; ++iter)
3590  {
3591  geometry_msgs::Pose p;
3592  rtabmap_conversions::transformToPoseMsg(iter->second, p);
3593  msg->poses.push_back(p);
3594 
3595  if(pubLabels)
3596  {
3597  // Add landmark ids
3598  visualization_msgs::Marker marker;
3599  marker.header.frame_id = mapFrameId_;
3600  marker.header.stamp = now;
3601  marker.ns = "landmarks";
3602  marker.id = iter->first;
3603  marker.action = visualization_msgs::Marker::ADD;
3604  marker.pose.position.x = iter->second.x();
3605  marker.pose.position.y = iter->second.y();
3606  marker.pose.position.z = iter->second.z();
3607  marker.pose.orientation.x = 0.0;
3608  marker.pose.orientation.y = 0.0;
3609  marker.pose.orientation.z = 0.0;
3610  marker.pose.orientation.w = 1.0;
3611  marker.scale.x = 1;
3612  marker.scale.y = 1;
3613  marker.scale.z = 0.35;
3614  marker.color.a = 0.5;
3615  marker.color.r = 1.0;
3616  marker.color.g = 1.0;
3617  marker.color.b = 0.0;
3618  marker.lifetime = ros::Duration(2.0f/rate_);
3619 
3620  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
3621  marker.text = uNumber2Str(iter->first);
3622 
3623  markers.markers.push_back(marker);
3624  }
3625  }
3626 
3627  landmarksPub_.publish(msg);
3628  }
3629 
3630  if(!req.graphOnly)
3631  {
3632  if(mapsManager_.hasSubscribers())
3633  {
3634  std::map<int, Transform> filteredPoses(poses.lower_bound(1), poses.end());
3635  if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && filteredPoses.size()>1)
3636  {
3637  std::map<int, Transform> nearestPoses = filterNodesToAssemble(filteredPoses, filteredPoses.rbegin()->second);
3638  }
3639  if(signatures.size())
3640  {
3641  filteredPoses = mapsManager_.updateMapCaches(
3642  filteredPoses,
3643  rtabmap_.getMemory(),
3644  false,
3645  false,
3646  signatures);
3647  }
3648  else
3649  {
3650  filteredPoses = mapsManager_.getFilteredPoses(filteredPoses);
3651  }
3652  mapsManager_.publishMaps(filteredPoses, now, mapFrameId_);
3653  }
3654  else
3655  {
3656  // this will cleanup the cache if there are no subscribers
3657  mapsManager_.publishMaps(std::map<int, Transform>(), now, mapFrameId_);
3658  }
3659  }
3660 
3661  bool pubPath = mapPathPub_.getNumSubscribers();
3662  if(pubLabels || pubPath)
3663  {
3664  if(poses.size() && signatures.size())
3665  {
3666  nav_msgs::Path path;
3667  if(pubPath)
3668  {
3669  path.poses.resize(poses.size());
3670  }
3671  int oi=0;
3672  for(std::map<int, Signature>::const_iterator iter=signatures.begin();
3673  iter!=signatures.end();
3674  ++iter)
3675  {
3676  std::map<int, Transform>::const_iterator poseIter= poses.find(iter->first);
3677  if(poseIter!=poses.end())
3678  {
3679  if(pubLabels)
3680  {
3681  // Add labels
3682  if(!iter->second.getLabel().empty())
3683  {
3684  visualization_msgs::Marker marker;
3685  marker.header.frame_id = mapFrameId_;
3686  marker.header.stamp = now;
3687  marker.ns = "labels";
3688  marker.id = iter->first;
3689  marker.action = visualization_msgs::Marker::ADD;
3690  marker.pose.position.x = poseIter->second.x();
3691  marker.pose.position.y = poseIter->second.y();
3692  marker.pose.position.z = poseIter->second.z();
3693  marker.pose.orientation.x = 0.0;
3694  marker.pose.orientation.y = 0.0;
3695  marker.pose.orientation.z = 0.0;
3696  marker.pose.orientation.w = 1.0;
3697  marker.scale.x = 1;
3698  marker.scale.y = 1;
3699  marker.scale.z = 0.5;
3700  marker.color.a = 0.7;
3701  marker.color.r = 1.0;
3702  marker.color.g = 0.0;
3703  marker.color.b = 0.0;
3704 
3705  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
3706  marker.text = iter->second.getLabel();
3707 
3708  markers.markers.push_back(marker);
3709  }
3710  // Add node ids
3711  visualization_msgs::Marker marker;
3712  marker.header.frame_id = mapFrameId_;
3713  marker.header.stamp = now;
3714  marker.ns = "ids";
3715  marker.id = iter->first;
3716  marker.action = visualization_msgs::Marker::ADD;
3717  marker.pose.position.x = poseIter->second.x();
3718  marker.pose.position.y = poseIter->second.y();
3719  marker.pose.position.z = poseIter->second.z();
3720  marker.pose.orientation.x = 0.0;
3721  marker.pose.orientation.y = 0.0;
3722  marker.pose.orientation.z = 0.0;
3723  marker.pose.orientation.w = 1.0;
3724  marker.scale.x = 1;
3725  marker.scale.y = 1;
3726  marker.scale.z = 0.2;
3727  marker.color.a = 0.5;
3728  marker.color.r = 1.0;
3729  marker.color.g = 1.0;
3730  marker.color.b = 1.0;
3731  marker.lifetime = ros::Duration(2.0f/rate_);
3732 
3733  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
3734  marker.text = uNumber2Str(iter->first);
3735 
3736  markers.markers.push_back(marker);
3737  }
3738  if(pubPath)
3739  {
3740  rtabmap_conversions::transformToPoseMsg(poseIter->second, path.poses.at(oi).pose);
3741  path.poses.at(oi).header.frame_id = mapFrameId_;
3742  path.poses.at(oi).header.stamp = ros::Time(iter->second.getStamp());
3743  ++oi;
3744  }
3745  }
3746  }
3747 
3748  if(pubLabels && markers.markers.size())
3749  {
3750  labelsPub_.publish(markers);
3751  }
3752  if(pubPath && oi)
3753  {
3754  path.header.frame_id = mapFrameId_;
3755  path.header.stamp = now;
3756  path.poses.resize(oi);
3757  mapPathPub_.publish(path);
3758  }
3759  }
3760  }
3761  }
3762  else
3763  {
3764  UWARN("No subscribers, don't need to publish!");
3765  if(!req.graphOnly)
3766  {
3767  // this will cleanup the cache if there are no subscribers
3768  mapsManager_.publishMaps(std::map<int, Transform>(), now, mapFrameId_);
3769  }
3770  }
3771 
3772  return true;
3773 }
3774 
3775 bool CoreWrapper::getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &res)
3776 {
3777  Transform pose = rtabmap_conversions::transformFromPoseMsg(req.goal.pose, true);
3778  UTimer timer;
3779  if(!pose.isNull())
3780  {
3781  // transform goal in /map frame
3782  Transform coordinateTransform = Transform::getIdentity();
3783  if(!req.goal.header.frame_id.empty() && mapFrameId_.compare(req.goal.header.frame_id) != 0)
3784  {
3785  coordinateTransform = rtabmap_conversions::getTransform(mapFrameId_, req.goal.header.frame_id, req.goal.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
3786  if(coordinateTransform.isNull())
3787  {
3788  NODELET_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",
3789  req.goal.header.frame_id.c_str(), mapFrameId_.c_str());
3790  return false;
3791  }
3792  pose = coordinateTransform * pose;
3793  }
3794  //else assume map frame if not set
3795 
3796  // To convert back the poses in goal frame
3797  coordinateTransform = coordinateTransform.inverse();
3798 
3799  if(rtabmap_.computePath(pose, req.tolerance))
3800  {
3801  NODELET_INFO("Planning: Time computing path = %f s", timer.ticks());
3802  const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
3803  res.plan.header.frame_id = req.goal.header.frame_id;
3804  res.plan.header.stamp = req.goal.header.stamp;
3805  if(poses.size() == 0)
3806  {
3807  NODELET_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).",
3808  rtabmap_.getGoalReachedRadius());
3809  // just set the goal directly
3810  res.plan.poses.resize(1);
3811  rtabmap_conversions::transformToPoseMsg(coordinateTransform*pose, res.plan.poses[0].pose);
3812  }
3813  else
3814  {
3815  res.plan.poses.resize(poses.size());
3816  int oi = 0;
3817  for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
3818  {
3819  res.plan.poses[oi].header = res.plan.header;
3820  rtabmap_conversions::transformToPoseMsg(coordinateTransform*iter->second, res.plan.poses[oi].pose);
3821  ++oi;
3822  }
3823  if(!rtabmap_.getPathTransformToGoal().isIdentity())
3824  {
3825  res.plan.poses.resize(res.plan.poses.size()+1);
3826  res.plan.poses[res.plan.poses.size()-1].header = res.plan.header;
3827  Transform p = rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal();
3828  rtabmap_conversions::transformToPoseMsg(coordinateTransform*p, res.plan.poses[res.plan.poses.size()-1].pose);
3829  }
3830 
3831  // Just output the path on screen
3832  std::stringstream stream;
3833  for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
3834  {
3835  if(iter != poses.begin())
3836  {
3837  stream << " ";
3838  }
3839  stream << iter->first;
3840  }
3841  NODELET_INFO("Planned path: [%s]", stream.str().c_str());
3842  }
3843  }
3844  rtabmap_.clearPath(0);
3845  }
3846  return true;
3847 }
3848 
3849 bool CoreWrapper::getPlanNodesCallback(rtabmap_msgs::GetPlan::Request &req, rtabmap_msgs::GetPlan::Response &res)
3850 {
3851  Transform pose;
3852  if(req.goal_node <= 0)
3853  {
3854  pose = rtabmap_conversions::transformFromPoseMsg(req.goal.pose, true);
3855  }
3856  UTimer timer;
3857  if(req.goal_node > 0 || !pose.isNull())
3858  {
3859  Transform coordinateTransform = Transform::getIdentity();
3860  // transform goal in /map frame
3861  if(!pose.isNull() && !req.goal.header.frame_id.empty() && mapFrameId_.compare(req.goal.header.frame_id) != 0)
3862  {
3863  coordinateTransform = rtabmap_conversions::getTransform(mapFrameId_, req.goal.header.frame_id, req.goal.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
3864  if(coordinateTransform.isNull())
3865  {
3866  NODELET_ERROR("Cannot transform goal pose from \"%s\" frame to \"%s\" frame!",
3867  req.goal.header.frame_id.c_str(), mapFrameId_.c_str());
3868  return false;
3869  }
3870  if(!pose.isNull())
3871  {
3872  pose = coordinateTransform * pose;
3873  }
3874  }
3875  //else assume map frame if not set
3876 
3877  // To convert back the poses in goal frame
3878  coordinateTransform = coordinateTransform.inverse();
3879 
3880  if((req.goal_node > 0 && rtabmap_.computePath(req.goal_node, req.tolerance)) ||
3881  (req.goal_node <= 0 && rtabmap_.computePath(pose, req.tolerance)))
3882  {
3883  NODELET_INFO("Planning: Time computing path = %f s", timer.ticks());
3884  const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
3885  res.plan.header.frame_id = mapFrameId_;
3886  res.plan.header.stamp = req.goal_node > 0?ros::Time::now():req.goal.header.stamp;
3887  if(poses.size() == 0)
3888  {
3889  NODELET_WARN("Planning: Goal already reached (RGBD/GoalReachedRadius=%fm).",
3890  rtabmap_.getGoalReachedRadius());
3891  if(!pose.isNull())
3892  {
3893  // just set the goal directly
3894  res.plan.poses.resize(1);
3895  res.plan.nodeIds.resize(1);
3896  rtabmap_conversions::transformToPoseMsg(coordinateTransform*pose, res.plan.poses[0]);
3897  res.plan.nodeIds[0] = 0;
3898  }
3899  }
3900  else
3901  {
3902  res.plan.poses.resize(poses.size());
3903  res.plan.nodeIds.resize(poses.size());
3904  int oi = 0;
3905  for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
3906  {
3907  rtabmap_conversions::transformToPoseMsg(coordinateTransform*iter->second, res.plan.poses[oi]);
3908  res.plan.nodeIds[oi] = iter->first;
3909  ++oi;
3910  }
3911  if(!rtabmap_.getPathTransformToGoal().isIdentity())
3912  {
3913  res.plan.poses.resize(res.plan.poses.size()+1);
3914  res.plan.nodeIds.resize(res.plan.nodeIds.size()+1);
3915  Transform p = rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal();
3916  rtabmap_conversions::transformToPoseMsg(coordinateTransform*p, res.plan.poses[res.plan.poses.size()-1]);
3917  res.plan.nodeIds[res.plan.nodeIds.size()-1] = 0;
3918  }
3919 
3920  // Just output the path on screen
3921  std::stringstream stream;
3922  for(std::vector<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
3923  {
3924  if(iter != poses.begin())
3925  {
3926  stream << " ";
3927  }
3928  stream << iter->first;
3929  }
3930  NODELET_INFO("Planned path: [%s]", stream.str().c_str());
3931  }
3932  }
3933  rtabmap_.clearPath(0);
3934  }
3935  return true;
3936 }
3937 
3938 bool CoreWrapper::setGoalCallback(rtabmap_msgs::SetGoal::Request& req, rtabmap_msgs::SetGoal::Response& res)
3939 {
3940  double planningTime = 0.0;
3941  goalCommonCallback(req.node_id, req.node_label, req.frame_id, Transform(), ros::Time::now(), &planningTime);
3942  const std::vector<std::pair<int, Transform> > & path = rtabmap_.getPath();
3943  res.path_ids.resize(path.size());
3944  res.path_poses.resize(path.size());
3945  res.planning_time = planningTime;
3946  for(unsigned int i=0; i<path.size(); ++i)
3947  {
3948  res.path_ids[i] = path[i].first;
3949  rtabmap_conversions::transformToPoseMsg(path[i].second, res.path_poses[i]);
3950  }
3951  return true;
3952 }
3953 
3954 bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
3955 {
3956  if(rtabmap_.getPath().size())
3957  {
3958  NODELET_WARN("Goal cancelled!");
3959  rtabmap_.clearPath(0);
3960  currentMetricGoal_.setNull();
3961  lastPublishedMetricGoal_.setNull();
3962  goalFrameId_.clear();
3963  latestNodeWasReached_ = false;
3964  if(goalReachedPub_.getNumSubscribers())
3965  {
3966  std_msgs::Bool result;
3967  result.data = false;
3968  goalReachedPub_.publish(result);
3969  }
3970  }
3971  if(mbClient_ && mbClient_->isServerConnected())
3972  {
3973  mbClient_->cancelGoal();
3974  }
3975 
3976  return true;
3977 }
3978 
3979 bool CoreWrapper::setLabelCallback(rtabmap_msgs::SetLabel::Request& req, rtabmap_msgs::SetLabel::Response& res)
3980 {
3981  if(rtabmap_.labelLocation(req.node_id, req.node_label))
3982  {
3983  if(req.node_id > 0)
3984  {
3985  NODELET_INFO("Set label \"%s\" to node %d", req.node_label.c_str(), req.node_id);
3986  }
3987  else
3988  {
3989  NODELET_INFO("Set label \"%s\" to last node", req.node_label.c_str());
3990  }
3991  }
3992  else
3993  {
3994  if(req.node_id > 0)
3995  {
3996  NODELET_ERROR("Could not set label \"%s\" to node %d", req.node_label.c_str(), req.node_id);
3997  }
3998  else
3999  {
4000  NODELET_ERROR("Could not set label \"%s\" to last node", req.node_label.c_str());
4001  }
4002  }
4003  return true;
4004 }
4005 
4006 bool CoreWrapper::listLabelsCallback(rtabmap_msgs::ListLabels::Request& req, rtabmap_msgs::ListLabels::Response& res)
4007 {
4008  if(rtabmap_.getMemory())
4009  {
4010  std::map<int, std::string> labels = rtabmap_.getMemory()->getAllLabels();
4011  res.ids = uKeys(labels);
4012  res.labels = uValues(labels);
4013  NODELET_INFO("List labels service: %d labels found.", (int)res.labels.size());
4014  }
4015  return true;
4016 }
4017 
4018 bool CoreWrapper::removeLabelCallback(rtabmap_msgs::RemoveLabel::Request& req, rtabmap_msgs::RemoveLabel::Response& res)
4019 {
4020  if(rtabmap_.getMemory())
4021  {
4022  int id = rtabmap_.getMemory()->getSignatureIdByLabel(req.label, true);
4023  if(id == 0)
4024  {
4025  NODELET_WARN("Label \"%s\" not found in the map, cannot remove it!", req.label.c_str());
4026  }
4027  else if(!rtabmap_.labelLocation(id, ""))
4028  {
4029  NODELET_ERROR("Failed removing label \"%s\".", req.label.c_str());
4030  }
4031  else
4032  {
4033  NODELET_INFO("Removed label \"%s\".", req.label.c_str());
4034  }
4035  }
4036  return true;
4037 }
4038 
4039 bool CoreWrapper::addLinkCallback(rtabmap_msgs::AddLink::Request& req, rtabmap_msgs::AddLink::Response&)
4040 {
4041  if(rtabmap_.getMemory())
4042  {
4043  ROS_INFO("Adding external link %d -> %d", req.link.fromId, req.link.toId);
4044  rtabmap_.addLink(rtabmap_conversions::linkFromROS(req.link));
4045  return true;
4046  }
4047  return false;
4048 }
4049 
4050 bool CoreWrapper::getNodesInRadiusCallback(rtabmap_msgs::GetNodesInRadius::Request& req, rtabmap_msgs::GetNodesInRadius::Response& res)
4051 {
4052  ROS_INFO("Get nodes in radius (%f): node_id=%d pose=(%f,%f,%f)", req.radius, req.node_id, req.x, req.y, req.z);
4053  std::map<int, Transform> poses;
4054  std::map<int, float> dists;
4055  if(req.node_id != 0 || (req.x == 0.0f && req.y == 0.0f && req.z == 0.0f))
4056  {
4057  poses = rtabmap_.getNodesInRadius(req.node_id, req.radius, req.k, &dists);
4058  }
4059  else
4060  {
4061  poses = rtabmap_.getNodesInRadius(Transform(req.x, req.y, req.z, 0,0,0), req.radius, req.k, &dists);
4062  }
4063 
4064  //Optimized graph
4065  res.ids.resize(poses.size());
4066  res.poses.resize(poses.size());
4067  res.distsSqr.resize(poses.size());
4068  int index = 0;
4069  for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin();
4070  iter != poses.end();
4071  ++iter)
4072  {
4073  res.ids[index] = iter->first;
4074  rtabmap_conversions::transformToPoseMsg(iter->second, res.poses[index]);
4075  UASSERT(dists.find(iter->first) != dists.end());
4076  res.distsSqr[index] = dists.at(iter->first);
4077  ++index;
4078  }
4079 
4080  return true;
4081 }
4082 
4083 void CoreWrapper::publishStats(const ros::Time & stamp)
4084 {
4085  UDEBUG("Publishing stats...");
4086  const rtabmap::Statistics & stats = rtabmap_.getStatistics();
4087 
4088  if(infoPub_.getNumSubscribers())
4089  {
4090  //NODELET_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId());
4091  rtabmap_msgs::InfoPtr msg(new rtabmap_msgs::Info);
4092  msg->header.stamp = stamp;
4093  msg->header.frame_id = mapFrameId_;
4094 
4095  rtabmap_conversions::infoToROS(stats, *msg);
4096  infoPub_.publish(msg);
4097  }
4098 
4099  if(mapDataPub_.getNumSubscribers())
4100  {
4101  rtabmap_msgs::MapDataPtr msg(new rtabmap_msgs::MapData);
4102  msg->header.stamp = stamp;
4103  msg->header.frame_id = mapFrameId_;
4104 
4105  rtabmap_conversions::mapDataToROS(
4106  stats.poses(),
4107  stats.constraints(),
4108  stats.getSignaturesData(),
4109  stats.mapCorrection(),
4110  *msg);
4111 
4112  mapDataPub_.publish(msg);
4113  }
4114 
4115  if(mapGraphPub_.getNumSubscribers())
4116  {
4117  if(mapsManager_.isMapUpdated())
4118  {
4119  graphLatched_ = false;
4120  }
4121  if(!(mapsManager_.isLatching() && graphLatched_))
4122  {
4123  rtabmap_msgs::MapGraphPtr msg(new rtabmap_msgs::MapGraph);
4124  msg->header.stamp = stamp;
4125  msg->header.frame_id = mapFrameId_;
4126 
4127  rtabmap_conversions::mapGraphToROS(
4128  stats.poses(),
4129  stats.constraints(),
4130  stats.mapCorrection(),
4131  *msg);
4132 
4133  mapGraphPub_.publish(msg);
4134  graphLatched_ = mapsManager_.isLatching();
4135  }
4136  // else we already published the latched graph
4137  }
4138  else
4139  {
4140  graphLatched_ = false;
4141  }
4142 
4143  if(odomCachePub_.getNumSubscribers())
4144  {
4145  rtabmap_msgs::MapGraphPtr msg(new rtabmap_msgs::MapGraph);
4146  msg->header.stamp = stamp;
4147  msg->header.frame_id = mapFrameId_;
4148 
4149  // For visualization of the constraints (MapGraph rviz plugin), we should include target nodes from the map
4150  std::map<int, Transform> poses = stats.odomCachePoses();
4151  // transform in map frame
4152  for(std::map<int, Transform>::iterator iter=poses.begin();
4153  iter!=poses.end();
4154  ++iter)
4155  {
4156  iter->second = stats.mapCorrection() * iter->second;
4157  }
4158  for(std::multimap<int, rtabmap::Link>::const_iterator iter=stats.odomCacheConstraints().begin();
4159  iter!=stats.odomCacheConstraints().end();
4160  ++iter)
4161  {
4162  std::map<int, Transform>::const_iterator pter = stats.poses().find(iter->second.to());
4163  if(pter != stats.poses().end())
4164  {
4165  poses.insert(*pter);
4166  }
4167  }
4168  rtabmap_conversions::mapGraphToROS(
4169  poses,
4170  stats.odomCacheConstraints(),
4171  stats.mapCorrection(),
4172  *msg);
4173 
4174  odomCachePub_.publish(msg);
4175  }
4176 
4177  if(localGridObstacle_.getNumSubscribers() && !stats.getLastSignatureData().sensorData().gridObstacleCellsRaw().empty())
4178  {
4179  pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridObstacleCellsRaw()));
4180  sensor_msgs::PointCloud2 msg;
4181  pcl_conversions::moveFromPCL(*cloud, msg);
4182  msg.header.stamp = stamp;
4183  msg.header.frame_id = frameId_;
4184  localGridObstacle_.publish(msg);
4185  }
4186  if(localGridEmpty_.getNumSubscribers() && !stats.getLastSignatureData().sensorData().gridEmptyCellsRaw().empty())
4187  {
4188  pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridEmptyCellsRaw()));
4189  sensor_msgs::PointCloud2 msg;
4190  pcl_conversions::moveFromPCL(*cloud, msg);
4191  msg.header.stamp = stamp;
4192  msg.header.frame_id = frameId_;
4193  localGridEmpty_.publish(msg);
4194  }
4195  if(localGridGround_.getNumSubscribers() && !stats.getLastSignatureData().sensorData().gridGroundCellsRaw().empty())
4196  {
4197  pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(stats.getLastSignatureData().sensorData().gridGroundCellsRaw()));
4198  sensor_msgs::PointCloud2 msg;
4199  pcl_conversions::moveFromPCL(*cloud, msg);
4200  msg.header.stamp = stamp;
4201  msg.header.frame_id = frameId_;
4202  localGridGround_.publish(msg);
4203  }
4204 
4205  bool pubLabels = labelsPub_.getNumSubscribers();
4206  visualization_msgs::MarkerArray markers;
4207  if((landmarksPub_.getNumSubscribers() || pubLabels) && !stats.poses().empty() && stats.poses().begin()->first < 0)
4208  {
4209  geometry_msgs::PoseArrayPtr msg(new geometry_msgs::PoseArray);
4210  msg->header.stamp = stamp;
4211  msg->header.frame_id = mapFrameId_;
4212  for(std::map<int, Transform>::const_iterator iter=stats.poses().begin(); iter!=stats.poses().end() && iter->first<0; ++iter)
4213  {
4214  geometry_msgs::Pose p;
4215  rtabmap_conversions::transformToPoseMsg(iter->second, p);
4216  msg->poses.push_back(p);
4217 
4218  if(pubLabels)
4219  {
4220  // Add landmark ids
4221  visualization_msgs::Marker marker;
4222  marker.header.frame_id = mapFrameId_;
4223  marker.header.stamp = stamp;
4224  marker.ns = "landmarks";
4225  marker.id = iter->first;
4226  marker.action = visualization_msgs::Marker::ADD;
4227  marker.pose.position.x = iter->second.x();
4228  marker.pose.position.y = iter->second.y();
4229  marker.pose.position.z = iter->second.z();
4230  marker.pose.orientation.x = 0.0;
4231  marker.pose.orientation.y = 0.0;
4232  marker.pose.orientation.z = 0.0;
4233  marker.pose.orientation.w = 1.0;
4234  marker.scale.x = 1;
4235  marker.scale.y = 1;
4236  marker.scale.z = 0.35;
4237  marker.color.a = 0.7;
4238  marker.color.r = 0.0;
4239  marker.color.g = 1.0;
4240  marker.color.b = 0.0;
4241  marker.lifetime = ros::Duration(2.0f/rate_);
4242 
4243  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
4244  marker.text = uNumber2Str(iter->first);
4245 
4246  markers.markers.push_back(marker);
4247  }
4248  }
4249 
4250  landmarksPub_.publish(msg);
4251  }
4252 
4253  bool pubPath = mapPathPub_.getNumSubscribers();
4254  if(pubLabels || pubPath)
4255  {
4256  if(stats.poses().size())
4257  {
4258  nav_msgs::Path path;
4259  if(pubPath)
4260  {
4261  // Ignore pose of current location in Localization mode
4262  path.poses.resize(stats.poses().size()-(rtabmap_.getMemory()->isIncremental()?0:1));
4263  }
4264  int oi = 0;
4265  for(std::map<int, Transform>::const_iterator poseIter=stats.poses().begin();
4266  poseIter!=stats.poses().end();
4267  ++poseIter)
4268  {
4269  if(pubLabels && rtabmap_.getMemory())
4270  {
4271  // Add labels
4272  std::map<int, std::string>::const_iterator lter = rtabmap_.getMemory()->getAllLabels().find(poseIter->first);
4273  if(lter != rtabmap_.getMemory()->getAllLabels().end() && !lter->second.empty())
4274  {
4275  visualization_msgs::Marker marker;
4276  marker.header.frame_id = mapFrameId_;
4277  marker.header.stamp = stamp;
4278  marker.ns = "labels";
4279  marker.id = -poseIter->first;
4280  marker.action = visualization_msgs::Marker::ADD;
4281  marker.pose.position.x = poseIter->second.x();
4282  marker.pose.position.y = poseIter->second.y();
4283  marker.pose.position.z = poseIter->second.z();
4284  marker.pose.orientation.x = 0.0;
4285  marker.pose.orientation.y = 0.0;
4286  marker.pose.orientation.z = 0.0;
4287  marker.pose.orientation.w = 1.0;
4288  marker.scale.x = 1;
4289  marker.scale.y = 1;
4290  marker.scale.z = 0.5;
4291  marker.color.a = 0.7;
4292  marker.color.r = 1.0;
4293  marker.color.g = 0.0;
4294  marker.color.b = 0.0;
4295 
4296  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
4297  marker.text = lter->second;
4298 
4299  markers.markers.push_back(marker);
4300  }
4301 
4302  // Add node ids
4303  visualization_msgs::Marker marker;
4304  marker.header.frame_id = mapFrameId_;
4305  marker.header.stamp = stamp;
4306  marker.ns = "ids";
4307  marker.id = poseIter->first;
4308  marker.action = visualization_msgs::Marker::ADD;
4309  marker.pose.position.x = poseIter->second.x();
4310  marker.pose.position.y = poseIter->second.y();
4311  marker.pose.position.z = poseIter->second.z();
4312  marker.pose.orientation.x = 0.0;
4313  marker.pose.orientation.y = 0.0;
4314  marker.pose.orientation.z = 0.0;
4315  marker.pose.orientation.w = 1.0;
4316  marker.scale.x = 1;
4317  marker.scale.y = 1;
4318  marker.scale.z = 0.2;
4319  marker.color.a = 0.5;
4320  marker.color.r = 1.0;
4321  marker.color.g = 1.0;
4322  marker.color.b = 1.0;
4323  marker.lifetime = ros::Duration(2.0f/rate_);
4324 
4325  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
4326  marker.text = uNumber2Str(poseIter->first);
4327 
4328  markers.markers.push_back(marker);
4329  }
4330  if(pubPath && (rtabmap_.getMemory()->isIncremental() || poseIter->first != stats.poses().rbegin()->first))
4331  {
4332  rtabmap_conversions::transformToPoseMsg(poseIter->second, path.poses.at(oi).pose);
4333  path.poses.at(oi).header.frame_id = mapFrameId_;
4334  path.poses.at(oi).header.stamp = stamp;
4335  ++oi;
4336  }
4337  }
4338 
4339  if(pubLabels && markers.markers.size())
4340  {
4341  labelsPub_.publish(markers);
4342  }
4343  if(pubPath && oi)
4344  {
4345  path.header.frame_id = mapFrameId_;
4346  path.header.stamp = stamp;
4347  path.poses.resize(oi);
4348  mapPathPub_.publish(path);
4349  }
4350  }
4351  }
4352 }
4353 
4354 void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
4355 {
4356  if(!currentMetricGoal_.isNull() && currentMetricGoal_ != lastPublishedMetricGoal_)
4357  {
4358  NODELET_INFO("Publishing next goal: %d -> %s",
4359  rtabmap_.getPathCurrentGoalId(), currentMetricGoal_.prettyPrint().c_str());
4360 
4361  geometry_msgs::PoseStamped poseMsg;
4362  poseMsg.header.frame_id = mapFrameId_;
4363  poseMsg.header.stamp = stamp;
4364  rtabmap_conversions::transformToPoseMsg(currentMetricGoal_, poseMsg.pose);
4365 
4366  if(useActionForGoal_)
4367  {
4368  if(mbClient_ == 0 || !mbClient_->isServerConnected())
4369  {
4370  NODELET_INFO("Connecting to move_base action server...");
4371  if(mbClient_ == 0)
4372  {
4373  mbClient_ = new MoveBaseClient("move_base", true);
4374  }
4375  mbClient_->waitForServer(ros::Duration(5.0));
4376  }
4377  if(mbClient_ && mbClient_->isServerConnected())
4378  {
4379  move_base_msgs::MoveBaseGoal goal;
4380  goal.target_pose = poseMsg;
4381 
4382  mbClient_->sendGoal(goal,
4383  boost::bind(&CoreWrapper::goalDoneCb, this, boost::placeholders::_1, boost::placeholders::_2),
4384  boost::bind(&CoreWrapper::goalActiveCb, this),
4385  boost::bind(&CoreWrapper::goalFeedbackCb, this, boost::placeholders::_1));
4386  lastPublishedMetricGoal_ = currentMetricGoal_;
4387  }
4388  else
4389  {
4390  NODELET_ERROR("Cannot connect to move_base action server (called \"%s\")!", this->getNodeHandle().resolveName("move_base").c_str());
4391  }
4392  }
4393  if(nextMetricGoalPub_.getNumSubscribers())
4394  {
4395  nextMetricGoalPub_.publish(poseMsg);
4396  if(!useActionForGoal_)
4397  {
4398  lastPublishedMetricGoal_ = currentMetricGoal_;
4399  }
4400  }
4401  }
4402 }
4403 
4404 // Called once when the goal completes
4405 void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
4406  const move_base_msgs::MoveBaseResultConstPtr& result)
4407 {
4408  bool ignore = false;
4409  if(!currentMetricGoal_.isNull())
4410  {
4411  if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
4412  {
4413  if(rtabmap_.getPath().size() &&
4414  rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first &&
4415  (!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first) || !latestNodeWasReached_))
4416  {
4417  NODELET_WARN("Planning: move_base reached current goal but it is not "
4418  "the last one planned by rtabmap. A new goal should be sent when "
4419  "rtabmap will be able to retrieve next locations on the path.");
4420  ignore = true;
4421  }
4422  else
4423  {
4424  NODELET_INFO("Planning: move_base success!");
4425  }
4426  }
4427  else
4428  {
4429  NODELET_ERROR("Planning: move_base failed for some reason. Aborting the plan...");
4430  }
4431 
4432  if(!ignore && goalReachedPub_.getNumSubscribers())
4433  {
4434  std_msgs::Bool result;
4435  result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED;
4436  goalReachedPub_.publish(result);
4437  }
4438  }
4439 
4440  if(!ignore)
4441  {
4442  rtabmap_.clearPath(1);
4443  currentMetricGoal_.setNull();
4444  lastPublishedMetricGoal_.setNull();
4445  goalFrameId_.clear();
4446  latestNodeWasReached_ = false;
4447  }
4448 }
4449 
4450 // Called once when the goal becomes active
4451 void CoreWrapper::goalActiveCb()
4452 {
4453  //NODELET_INFO("Planning: Goal just went active");
4454 }
4455 
4456 // Called every time feedback is received for the goal
4457 void CoreWrapper::goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
4458 {
4459  //Transform basePosition = rtabmap_conversions::transformFromPoseMsg(feedback->base_position.pose);
4460  //NODELET_INFO("Planning: feedback base_position = %s", basePosition.prettyPrint().c_str());
4461 }
4462 
4463 void CoreWrapper::publishLocalPath(const ros::Time & stamp)
4464 {
4465  if(rtabmap_.getPath().size())
4466  {
4467  std::vector<std::pair<int, Transform> > poses = rtabmap_.getPathNextPoses();
4468  if(poses.size())
4469  {
4470  if(localPathPub_.getNumSubscribers() || localPathNodesPub_.getNumSubscribers())
4471  {
4472  nav_msgs::Path path;
4473  rtabmap_msgs::Path pathNodes;
4474  path.header.frame_id = pathNodes.header.frame_id = mapFrameId_;
4475  path.header.stamp = pathNodes.header.stamp = stamp;
4476  path.poses.resize(poses.size());
4477  pathNodes.nodeIds.resize(poses.size());
4478  pathNodes.poses.resize(poses.size());
4479  int oi = 0;
4480  for(std::vector<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
4481  {
4482  path.poses[oi].header = path.header;
4483  rtabmap_conversions::transformToPoseMsg(iter->second, path.poses[oi].pose);
4484  pathNodes.poses[oi] = path.poses[oi].pose;
4485  pathNodes.nodeIds[oi] = iter->first;
4486  ++oi;
4487  }
4488  if(localPathPub_.getNumSubscribers())
4489  {
4490  localPathPub_.publish(path);
4491  }
4492  if(localPathNodesPub_.getNumSubscribers())
4493  {
4494  localPathNodesPub_.publish(pathNodes);
4495  }
4496  }
4497  }
4498  }
4499 }
4500 
4501 void CoreWrapper::publishGlobalPath(const ros::Time & stamp)
4502 {
4503  if((globalPathPub_.getNumSubscribers() || globalPathNodesPub_.getNumSubscribers()) && rtabmap_.getPath().size())
4504  {
4505  Transform pose = uValue(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPathCurrentGoalId(), Transform());
4506  if(!pose.isNull() && rtabmap_.getPathCurrentGoalIndex() < rtabmap_.getPath().size())
4507  {
4508  // transform the global path in the goal referential
4509  Transform t = pose * rtabmap_.getPath().at(rtabmap_.getPathCurrentGoalIndex()).second.inverse();
4510 
4511  nav_msgs::Path path;
4512  rtabmap_msgs::Path pathNodes;
4513  path.header.frame_id = pathNodes.header.frame_id = mapFrameId_;
4514  path.header.stamp = pathNodes.header.stamp = stamp;
4515  path.poses.resize(rtabmap_.getPath().size());
4516  pathNodes.nodeIds.resize(rtabmap_.getPath().size());
4517  pathNodes.poses.resize(rtabmap_.getPath().size());
4518  int oi = 0;
4519  for(std::vector<std::pair<int, Transform> >::const_iterator iter=rtabmap_.getPath().begin(); iter!=rtabmap_.getPath().end(); ++iter)
4520  {
4521  path.poses[oi].header = path.header;
4522  rtabmap_conversions::transformToPoseMsg(t*iter->second, path.poses[oi].pose);
4523  pathNodes.poses[oi] = path.poses[oi].pose;
4524  pathNodes.nodeIds[oi] = iter->first;
4525  ++oi;
4526  }
4527  Transform goalLocalTransform = Transform::getIdentity();
4528  if(!goalFrameId_.empty() && goalFrameId_.compare(frameId_) != 0)
4529  {
4530  Transform localT = rtabmap_conversions::getTransform(frameId_, goalFrameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
4531  if(!localT.isNull())
4532  {
4533  goalLocalTransform = localT.inverse().to3DoF();
4534  }
4535  }
4536 
4537  if(!rtabmap_.getPathTransformToGoal().isIdentity() || !goalLocalTransform.isIdentity())
4538  {
4539  path.poses.resize(path.poses.size()+1);
4540  path.poses[path.poses.size()-1].header = path.header;
4541  pathNodes.nodeIds.resize(pathNodes.nodeIds.size()+1);
4542  pathNodes.poses.resize(pathNodes.poses.size()+1);
4543  Transform p = t * rtabmap_.getPath().back().second*rtabmap_.getPathTransformToGoal() * goalLocalTransform;
4544  rtabmap_conversions::transformToPoseMsg(p, path.poses[path.poses.size()-1].pose);
4545  pathNodes.poses[pathNodes.poses.size()-1] = path.poses[path.poses.size()-1].pose;
4546  pathNodes.nodeIds[pathNodes.nodeIds.size()-1] = 0;
4547  }
4548  if(globalPathPub_.getNumSubscribers())
4549  {
4550  globalPathPub_.publish(path);
4551  }
4552  if(globalPathNodesPub_.getNumSubscribers())
4553  {
4554  globalPathNodesPub_.publish(pathNodes);
4555  }
4556  }
4557  }
4558 }
4559 
4560 CoreWrapper::LocalizationStatusTask::LocalizationStatusTask() :
4561  diagnostic_updater::DiagnosticTask("Localization status"),
4562  localizationThreshold_(0.0),
4563  localizationError_(9999)
4564 {}
4565 
4566 void CoreWrapper::LocalizationStatusTask::setLocalizationThreshold(double value)
4567 {
4568  localizationThreshold_ = value;
4569 }
4570 
4571 void CoreWrapper::LocalizationStatusTask::updateStatus(const cv::Mat & cov, bool twoDMapping)
4572 {
4573  if(localizationThreshold_ > 0.0 && !cov.empty())
4574  {
4575  if(cov.at<double>(0,0) >= 9999.0)
4576  {
4577  localizationError_ = 9999.0;
4578  }
4579  else
4580  {
4581  localizationError_ = sqrt(uMax3(cov.at<double>(0,0), cov.at<double>(1,1), twoDMapping?0.0:cov.at<double>(2,2)));
4582  }
4583  }
4584 }
4585 
4586 void CoreWrapper::LocalizationStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
4587 {
4588  if(localizationError_>=9999)
4589  {
4590  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not localized!");
4591  }
4592  else if(localizationError_ > localizationThreshold_)
4593  {
4594  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Localization error is high!");
4595  }
4596  else
4597  {
4598  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Localized.");
4599  }
4600  stat.add("Localization error (m)", localizationError_);
4601  stat.add("loc_thr (m)", localizationThreshold_);
4602 }
4603 
4604 #ifdef WITH_OCTOMAP_MSGS
4605 #ifdef RTABMAP_OCTOMAP
4606 bool CoreWrapper::octomapBinaryCallback(
4607  octomap_msgs::GetOctomap::Request &req,
4608  octomap_msgs::GetOctomap::Response &res)
4609 {
4610  NODELET_INFO("Sending binary map data on service request");
4611  res.map.header.frame_id = mapFrameId_;
4612  res.map.header.stamp = ros::Time::now();
4613 
4614  std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
4615  if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && poses.size()>1)
4616  {
4617  poses = filterNodesToAssemble(poses, poses.rbegin()->second);
4618  }
4619 
4620  mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
4621 
4622  const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
4623  bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
4624  return success;
4625 }
4626 
4627 bool CoreWrapper::octomapFullCallback(
4628  octomap_msgs::GetOctomap::Request &req,
4629  octomap_msgs::GetOctomap::Response &res)
4630 {
4631  NODELET_INFO("Sending full map data on service request");
4632  res.map.header.frame_id = mapFrameId_;
4633  res.map.header.stamp = ros::Time::now();
4634 
4635  std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
4636  if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && poses.size()>1)
4637  {
4638  poses = filterNodesToAssemble(poses, poses.rbegin()->second);
4639  }
4640 
4641  mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, true);
4642 
4643  const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
4644  bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
4645  return success;
4646 }
4647 #endif
4648 #endif
4649 
4650 PLUGINLIB_EXPORT_CLASS(rtabmap_slam::CoreWrapper, nodelet::Nodelet);
4651 
4652 }
rtabmap_slam::CoreWrapper::loadParameters
void loadParameters(const std::string &configFile, rtabmap::ParametersMap &parameters)
Definition: CoreWrapper.cpp:899
rtabmap::SensorData
rtabmap_slam::CoreWrapper::publishLocalPath
void publishLocalPath(const ros::Time &stamp)
Definition: CoreWrapper.cpp:4463
rtabmap_conversions::convertScanMsg
bool convertScanMsg(const sensor_msgs::LaserScan &scan2dMsg, const std::string &frameId, const std::string &odomFrameId, const ros::Time &odomStamp, rtabmap::LaserScan &scan, tf::TransformListener &listener, double waitForTransform, bool outputInFrameId=false)
ParametersPair
std::pair< std::string, std::string > ParametersPair
int
int
rtabmap::LaserScan::localTransform
Transform localTransform() const
UMutex::lock
int lock() const
rtabmap::Statistics::loopClosureId
int loopClosureId() const
rtabmap_slam::CoreWrapper::LocalizationStatusTask::updateStatus
void updateStatus(const cv::Mat &covariance, bool twoDMapping)
Definition: CoreWrapper.cpp:4571
rtabmap_slam::CoreWrapper::waitForTransformDuration_
double waitForTransformDuration_
Definition: CoreWrapper.h:280
rtabmap_util::MapsManager::getGridProbMap
cv::Mat getGridProbMap(float &xMin, float &yMin, float &gridCellSize)
rtabmap_slam::CoreWrapper::commonLaserScanCallback
virtual void commonLaserScanCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_msgs::UserDataConstPtr &userDataMsg, const sensor_msgs::LaserScan &scanMsg, const sensor_msgs::PointCloud2 &scan3dMsg, const rtabmap_msgs::OdomInfoConstPtr &odomInfoMsg, const rtabmap_msgs::GlobalDescriptor &globalDescriptor=rtabmap_msgs::GlobalDescriptor())
Definition: CoreWrapper.cpp:1562
rtabmap_slam::CoreWrapper::setLogError
bool setLogError(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:3313
rtabmap::Rtabmap::getPathNextPoses
std::vector< std::pair< int, Transform > > getPathNextPoses() const
rtabmap_slam::CoreWrapper::mapPathPub_
ros::Publisher mapPathPub_
Definition: CoreWrapper.h:305
ros::NodeHandle::setParam
void setParam(const std::string &key, bool b) const
rtabmap_slam::CoreWrapper::latestNodeWasReached_
bool latestNodeWasReached_
Definition: CoreWrapper.h:262
rtabmap::Rtabmap::cleanupLocalGrids
int cleanupLocalGrids(const std::map< int, Transform > &mapPoses, const cv::Mat &map, float xMin, float yMin, float cellSize, int cropRadius=1, bool filterScans=false)
rtabmap::Memory::isIncremental
bool isIncremental() const
rtabmap_slam::CoreWrapper::goalNodeSub_
ros::Subscriber goalNodeSub_
Definition: CoreWrapper.h:314
OctoMap.h
NODELET_ERROR
#define NODELET_ERROR(...)
rtabmap::Rtabmap::getSTMSize
int getSTMSize() const
rtabmap_slam::CoreWrapper::addLinkCallback
bool addLinkCallback(rtabmap_msgs::AddLink::Request &, rtabmap_msgs::AddLink::Response &)
Definition: CoreWrapper.cpp:4039
rtabmap_slam::CoreWrapper::gpsFixAsyncCallback
void gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr &gpsFixMsg)
Definition: CoreWrapper.cpp:2376
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
rtabmap_conversions::mapGraphToROS
void mapGraphToROS(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, rtabmap::Link > &links, const rtabmap::Transform &mapToOdom, rtabmap_msgs::MapGraph &msg)
rtabmap::Statistics
rtabmap_conversions::globalDescriptorsFromROS
std::vector< rtabmap::GlobalDescriptor > globalDescriptorsFromROS(const std::vector< rtabmap_msgs::GlobalDescriptor > &msg)
UFile::erase
static int erase(const std::string &filePath)
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
msg
msg
octomap_msgs::fullMapToMsg
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
ros::Publisher
rtabmap_conversions::globalDescriptorFromROS
rtabmap::GlobalDescriptor globalDescriptorFromROS(const rtabmap_msgs::GlobalDescriptor &msg)
rtabmap_slam::CoreWrapper::~CoreWrapper
virtual ~CoreWrapper()
Definition: CoreWrapper.cpp:868
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
rtabmap_slam::CoreWrapper::commonSensorDataCallback
virtual void commonSensorDataCallback(const rtabmap_msgs::SensorDataConstPtr &sensorDataMsg, const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_msgs::OdomInfoConstPtr &odomInfoMsg)
Definition: CoreWrapper.cpp:1759
yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
image_encodings.h
rtabmap_slam::CoreWrapper::globalPose_
geometry_msgs::PoseWithCovarianceStamped globalPose_
Definition: CoreWrapper.h:377
rtabmap_slam::CoreWrapper::setGoalSrv_
ros::ServiceServer setGoalSrv_
Definition: CoreWrapper.h:352
rtabmap_slam::CoreWrapper::landmarksPub_
ros::Publisher landmarksPub_
Definition: CoreWrapper.h:303
UFile::copy
void copy(const std::string &to)
image_transport::ImageTransport
rtabmap_slam::CoreWrapper::genDepthFillHolesSize_
int genDepthFillHolesSize_
Definition: CoreWrapper.h:288
ULogger::kError
kError
rtabmap_slam::CoreWrapper::process
void process(const ros::Time &stamp, rtabmap::SensorData &data, const rtabmap::Transform &odom=rtabmap::Transform(), const std::vector< float > &odomVelocity=std::vector< float >(), const std::string &odomFrameId="", const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const rtabmap::OdometryInfo &odomInfo=rtabmap::OdometryInfo(), double timeMsgConversion=0.0)
Definition: CoreWrapper.cpp:1801
rtabmap_slam::CoreWrapper::nextMetricGoalPub_
ros::Publisher nextMetricGoalPub_
Definition: CoreWrapper.h:315
cameras
std::array< PinholeCamera< Cal3_S2 >, 3 > cameras
message_filters::Synchronizer
rtabmap_slam::CoreWrapper::labelsPub_
ros::Publisher labelsPub_
Definition: CoreWrapper.h:304
boost::shared_ptr
timer
rtabmap_conversions::getTransform
rtabmap::Transform getTransform(const std::string &fromFrameId, const std::string &toFrameId, const ros::Time &stamp, tf::TransformListener &listener, double waitForTransform)
rtabmap_conversions::transformFromPoseMsg
rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose &msg, bool ignoreRotationIfNotSet=false)
rtabmap_slam::CoreWrapper::globalPathNodesPub_
ros::Publisher globalPathNodesPub_
Definition: CoreWrapper.h:319
s
RealScalar s
rtabmap_slam::CoreWrapper::mappingMaxNodes_
int mappingMaxNodes_
Definition: CoreWrapper.h:399
rtabmap_slam::CoreWrapper::interOdomInfoSyncSub_
message_filters::Subscriber< rtabmap_msgs::OdomInfo > interOdomInfoSyncSub_
Definition: CoreWrapper.h:391
rtabmap::SensorData::gridCellSize
float gridCellSize() const
diagnostic_updater::DiagnosticStatusWrapper::add
void add(const std::string &key, const bool &b)
rtabmap_conversions::transformToPoseMsg
void transformToPoseMsg(const rtabmap::Transform &transform, geometry_msgs::Pose &msg)
rtabmap_slam::CoreWrapper::parameters_
rtabmap::ParametersMap parameters_
Definition: CoreWrapper.h:265
rtabmap::Statistics::localizationCovariance
const cv::Mat & localizationCovariance() const
rtabmap_slam::CoreWrapper::MyExactInterOdomSyncPolicy
message_filters::sync_policies::ExactTime< nav_msgs::Odometry, rtabmap_msgs::OdomInfo > MyExactInterOdomSyncPolicy
Definition: CoreWrapper.h:392
rtabmap_slam::CoreWrapper::frameId_
std::string frameId_
Definition: CoreWrapper.h:268
rtabmap_slam::CoreWrapper::localPathNodesPub_
ros::Publisher localPathNodesPub_
Definition: CoreWrapper.h:320
rtabmap_slam::CoreWrapper::getProbMapCallback
bool getProbMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: CoreWrapper.cpp:3487
rtabmap_slam::CoreWrapper::resumeSrv_
ros::ServiceServer resumeSrv_
Definition: CoreWrapper.h:329
rtabmap::Rtabmap::getMemory
const Memory * getMemory() const
rtabmap_util::MapsManager::isMapUpdated
bool isMapUpdated() const
rtabmap_slam::CoreWrapper::goalNodeCallback
void goalNodeCallback(const rtabmap_msgs::GoalConstPtr &msg)
Definition: CoreWrapper.cpp:2740
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
rtabmap_slam::CoreWrapper::removeLabelSrv_
ros::ServiceServer removeLabelSrv_
Definition: CoreWrapper.h:356
rtabmap_slam::CoreWrapper::setLabelSrv_
ros::ServiceServer setLabelSrv_
Definition: CoreWrapper.h:354
stream
stream
rtabmap_slam::CoreWrapper::genScanMaxDepth_
double genScanMaxDepth_
Definition: CoreWrapper.h:284
UFile::length
long length()
UMutex::unlock
int unlock() const
fx
const double fx
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
rtabmap_sync::CommonDataSubscriber::isSubscribedToRGBD
bool isSubscribedToRGBD() const
rtabmap_slam::CoreWrapper::interOdoms_
std::list< std::pair< nav_msgs::Odometry, rtabmap_msgs::OdomInfo > > interOdoms_
Definition: CoreWrapper.h:389
rtabmap::Rtabmap::getLocalOptimizedPoses
const std::map< int, Transform > & getLocalOptimizedPoses() const
rtabmap_slam::CoreWrapper::genDepth_
bool genDepth_
Definition: CoreWrapper.h:286
rtabmap_slam::CoreWrapper::filterNodesToAssemble
std::map< int, rtabmap::Transform > filterNodesToAssemble(const std::map< int, rtabmap::Transform > &nodes, const rtabmap::Transform &currentPose)
Definition: CoreWrapper.cpp:2319
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
rtabmap::OdometryInfo::timeEstimation
float timeEstimation
rtabmap_slam::CoreWrapper::scanCloudIs2d_
bool scanCloudIs2d_
Definition: CoreWrapper.h:292
ros.h
rtabmap::Transform::getIdentity
static Transform getIdentity()
rtabmap_slam::CoreWrapper::userDataMutex_
UMutex userDataMutex_
Definition: CoreWrapper.h:374
rtabmap_slam::CoreWrapper::LocalizationStatusTask::run
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: CoreWrapper.cpp:4586
rtabmap::CameraModel
actionlib::SimpleActionClient::waitForServer
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
ULogger::setLevel
static void setLevel(ULogger::Level level)
rtabmap::LaserScan::data
const cv::Mat & data() const
rtabmap_slam::CoreWrapper::publishCurrentGoal
void publishCurrentGoal(const ros::Time &stamp)
Definition: CoreWrapper.cpp:4354
rtabmap_slam::CoreWrapper::commonMultiCameraCallback
virtual void commonMultiCameraCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_msgs::UserDataConstPtr &userDataMsg, const std::vector< cv_bridge::CvImageConstPtr > &imageMsgs, const std::vector< cv_bridge::CvImageConstPtr > &depthMsgs, const std::vector< sensor_msgs::CameraInfo > &cameraInfoMsgs, const std::vector< sensor_msgs::CameraInfo > &depthCameraInfoMsgs, const sensor_msgs::LaserScan &scanMsg, const sensor_msgs::PointCloud2 &scan3dMsg, const rtabmap_msgs::OdomInfoConstPtr &odomInfoMsg, const std::vector< rtabmap_msgs::GlobalDescriptor > &globalDescriptorMsgs=std::vector< rtabmap_msgs::GlobalDescriptor >(), const std::vector< std::vector< rtabmap_msgs::KeyPoint > > &localKeyPoints=std::vector< std::vector< rtabmap_msgs::KeyPoint > >(), const std::vector< std::vector< rtabmap_msgs::Point3f > > &localPoints3d=std::vector< std::vector< rtabmap_msgs::Point3f > >(), const std::vector< cv::Mat > &localDescriptors=std::vector< cv::Mat >())
Definition: CoreWrapper.cpp:1190
rtabmap_slam::CoreWrapper::lastPoseVelocity_
std::vector< float > lastPoseVelocity_
Definition: CoreWrapper.h:257
rtabmap_slam::CoreWrapper::localPathPub_
ros::Publisher localPathPub_
Definition: CoreWrapper.h:318
rtabmap_slam::CoreWrapper::globalBundleAdjustmentCallback
bool globalBundleAdjustmentCallback(rtabmap_msgs::GlobalBundleAdjustment::Request &, rtabmap_msgs::GlobalBundleAdjustment::Response &)
Definition: CoreWrapper.cpp:3225
rtabmap::Rtabmap::getGoalReachedRadius
float getGoalReachedRadius() const
actionlib::SimpleClientGoalState
rtabmap_slam::CoreWrapper::commonMultiCameraCallbackImpl
void commonMultiCameraCallbackImpl(const std::string &odomFrameId, const rtabmap_msgs::UserDataConstPtr &userDataMsg, const std::vector< cv_bridge::CvImageConstPtr > &imageMsgs, const std::vector< cv_bridge::CvImageConstPtr > &depthMsgs, const std::vector< sensor_msgs::CameraInfo > &cameraInfoMsgs, const std::vector< sensor_msgs::CameraInfo > &depthCameraInfoMsgs, const sensor_msgs::LaserScan &scan2dMsg, const sensor_msgs::PointCloud2 &scan3dMsg, const rtabmap_msgs::OdomInfoConstPtr &odomInfoMsg, const std::vector< rtabmap_msgs::GlobalDescriptor > &globalDescriptorMsgs, const std::vector< std::vector< rtabmap_msgs::KeyPoint > > &localKeyPoints, const std::vector< std::vector< rtabmap_msgs::Point3f > > &localPoints3d, const std::vector< cv::Mat > &localDescriptors)
Definition: CoreWrapper.cpp:1262
rtabmap_slam::CoreWrapper::genDepthFillHolesError_
double genDepthFillHolesError_
Definition: CoreWrapper.h:290
rtabmap_slam::CoreWrapper::republishNodeDataCallback
void republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr &msg)
Definition: CoreWrapper.cpp:2505
rtabmap_slam::CoreWrapper::mbClient_
MoveBaseClient * mbClient_
Definition: CoreWrapper.h:364
ros::Subscriber::getTopic
std::string getTopic() const
rtabmap_slam::CoreWrapper::setLogWarnSrv_
ros::ServiceServer setLogWarnSrv_
Definition: CoreWrapper.h:340
rtabmap_slam::CoreWrapper::imuFrameId_
std::string imuFrameId_
Definition: CoreWrapper.h:385
rtabmap_slam::CoreWrapper::getNodesInRadiusSrv_
ros::ServiceServer getNodesInRadiusSrv_
Definition: CoreWrapper.h:358
rtabmap::GPS
ULogger::kDebug
kDebug
rtabmap_sync::CommonDataSubscriber::isSubscribedToRGB
bool isSubscribedToRGB() const
frameId
string frameId
rtabmap_slam::CoreWrapper::republishNodeDataSub_
ros::Subscriber republishNodeDataSub_
Definition: CoreWrapper.h:386
rtabmap_slam::CoreWrapper::LocalizationStatusTask::LocalizationStatusTask
LocalizationStatusTask()
Definition: CoreWrapper.cpp:4560
rtabmap_slam::CoreWrapper::globalPoseAsyncSub_
ros::Subscriber globalPoseAsyncSub_
Definition: CoreWrapper.h:376
ros::TransportHints
rtabmap::Memory::getLastSignatureId
int getLastSignatureId() const
rtabmap_slam::CoreWrapper::configPath_
std::string configPath_
Definition: CoreWrapper.h:273
rtabmap_slam::CoreWrapper::globalBundleAdjustmentSrv_
ros::ServiceServer globalBundleAdjustmentSrv_
Definition: CoreWrapper.h:334
UStl.h
rtabmap::Memory::getSignatureIdByLabel
int getSignatureIdByLabel(const std::string &label, bool lookInDatabase=true) const
rtabmap::Rtabmap::globalBundleAdjustment
bool globalBundleAdjustment(int optimizerType=1, bool rematchFeatures=true, int iterations=0, float pixelVariance=0.0f)
TimeBase< Time, Duration >::toSec
double toSec() const
NODELET_WARN
#define NODELET_WARN(...)
rtabmap_util::MapsManager::getOctomap
const rtabmap::OctoMap * getOctomap() const
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, bool withImages=false, bool withScan=false, bool withUserData=false, bool withGrid=false, bool withWords=true, bool withGlobalDescriptors=true) const
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
rtabmap_sync::CommonDataSubscriber::isSubscribedToDepth
bool isSubscribedToDepth() const
rtabmap_sync::CommonDataSubscriber::setupCallbacks
void setupCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, const std::string &name, std::vector< diagnostic_updater::DiagnosticTask * > otherTasks=std::vector< diagnostic_updater::DiagnosticTask * >())
y
Matrix3f y
rtabmap_slam::CoreWrapper::getProjMapSrv_
ros::ServiceServer getProjMapSrv_
Definition: CoreWrapper.h:345
rtabmap::OctoMap::octree
const RtabmapColorOcTree * octree() const
rtabmap_slam::CoreWrapper::createIntermediateNodes_
bool createIntermediateNodes_
Definition: CoreWrapper.h:398
rtabmap_slam::CoreWrapper::cleanupLocalGridsCallback
bool cleanupLocalGridsCallback(rtabmap_msgs::CleanupLocalGrids::Request &, rtabmap_msgs::CleanupLocalGrids::Response &)
Definition: CoreWrapper.cpp:3175
true
#define true
rtabmap_slam::CoreWrapper::setModeMappingCallback
bool setModeMappingCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:3283
rtabmap_slam::CoreWrapper::mapToOdom_
rtabmap::Transform mapToOdom_
Definition: CoreWrapper.h:294
rtabmap::Rtabmap::getLastLocalizationPose
const Transform & getLastLocalizationPose() const
iterator
res
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
rtabmap_slam::CoreWrapper::groundTruthFrameId_
std::string groundTruthFrameId_
Definition: CoreWrapper.h:271
rtabmap_slam::CoreWrapper::lastPublishedMetricGoal_
rtabmap::Transform lastPublishedMetricGoal_
Definition: CoreWrapper.h:261
rtabmap_sync::CommonDataSubscriber::isSubscribedToScan3d
bool isSubscribedToScan3d() const
rtabmap_slam::CoreWrapper::setLogErrorSrv_
ros::ServiceServer setLogErrorSrv_
Definition: CoreWrapper.h:341
rtabmap_conversions::odomInfoToStatistics
std::map< std::string, float > odomInfoToStatistics(const rtabmap::OdometryInfo &info)
rtabmap::Transform::setNull
void setNull()
rtabmap_util::MapsManager::isLatching
bool isLatching() const
rtabmap_slam::CoreWrapper::imus_
std::map< double, rtabmap::Transform > imus_
Definition: CoreWrapper.h:384
rtabmap::util2d::disparityFromStereoImages
cv::Mat RTABMAP_CORE_EXPORT disparityFromStereoImages(const cv::Mat &leftImage, const cv::Mat &rightImage, const ParametersMap &parameters=ParametersMap())
rtabmap::LaserScan
rtabmap_slam::CoreWrapper::setModeLocalizationCallback
bool setModeLocalizationCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:3271
sensor_msgs::image_encodings::RGB8
const std::string RGB8
ParametersMap
std::map< std::string, std::string > ParametersMap
rtabmap_slam::CoreWrapper::initialPoseCallback
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
Definition: CoreWrapper.cpp:2527
rtabmap_conversions::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::vector< sensor_msgs::CameraInfo > &depthCameraInfoMsgs, const std::string &frameId, const std::string &odomFrameId, const ros::Time &odomStamp, cv::Mat &rgb, cv::Mat &depth, std::vector< rtabmap::CameraModel > &cameraModels, std::vector< rtabmap::StereoCameraModel > &stereoCameraModels, tf::TransformListener &listener, double waitForTransform, bool alreadRectifiedImages, const std::vector< std::vector< rtabmap_msgs::KeyPoint > > &localKeyPointsMsgs=std::vector< std::vector< rtabmap_msgs::KeyPoint > >(), const std::vector< std::vector< rtabmap_msgs::Point3f > > &localPoints3dMsgs=std::vector< std::vector< rtabmap_msgs::Point3f > >(), const std::vector< cv::Mat > &localDescriptorsMsgs=std::vector< cv::Mat >(), std::vector< cv::KeyPoint > *localKeyPoints=0, std::vector< cv::Point3f > *localPoints3d=0, cv::Mat *localDescriptors=0)
nodelet::Nodelet::getMyArgv
const V_string & getMyArgv() const
rtabmap::Memory::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
rtabmap::Memory::getLastWorkingSignature
const Signature * getLastWorkingSignature() const
f
Vector3 f(-6, 1, 0.5)
Landmarks
std::map< int, Landmark > Landmarks
rtabmap_slam::CoreWrapper::listLabelsSrv_
ros::ServiceServer listLabelsSrv_
Definition: CoreWrapper.h:355
rtabmap::Rtabmap::addNodesToRepublish
void addNodesToRepublish(const std::vector< int > &ids)
uNumber2Str
std::string uNumber2Str(double number, int precision, bool fixed)
rtabmap::Parameters::getDefaultDatabaseName
static std::string getDefaultDatabaseName()
Registration.h
rtabmap_slam::CoreWrapper::resumeRtabmapCallback
bool resumeRtabmapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2871
rtabmap_sync::CommonDataSubscriber::isSubscribedToScan2d
bool isSubscribedToScan2d() const
rtabmap_slam::CoreWrapper::setLogWarn
bool setLogWarn(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:3307
rtabmap::Transform::isNull
bool isNull() const
rtabmap_slam
Definition: CoreWrapper.h:100
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rtabmap_slam::CoreWrapper::triggerNewMapSrv_
ros::ServiceServer triggerNewMapSrv_
Definition: CoreWrapper.h:331
rtabmap::Registration::COVARIANCE_LINEAR_EPSILON
static double COVARIANCE_LINEAR_EPSILON
rtabmap::Statistics::proximityDetectionId
int proximityDetectionId() const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
rtabmap::LaserScan::isEmpty
bool isEmpty() const
rtabmap_slam::CoreWrapper::LocalizationStatusTask::setLocalizationThreshold
void setLocalizationThreshold(double value)
Definition: CoreWrapper.cpp:4566
rtabmap::Rtabmap::getPathTransformToGoal
const Transform & getPathTransformToGoal() const
rtabmap::OdometryInfo::transform
Transform transform
rtabmap_util::MapsManager::clear
void clear()
rtabmap_slam::CoreWrapper::currentMetricGoal_
rtabmap::Transform currentMetricGoal_
Definition: CoreWrapper.h:260
rtabmap_slam::CoreWrapper::interOdomSync_
message_filters::Synchronizer< MyExactInterOdomSyncPolicy > * interOdomSync_
Definition: CoreWrapper.h:393
rtabmap_slam::CoreWrapper::previousStamp_
ros::Time previousStamp_
Definition: CoreWrapper.h:403
data
data
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
rtabmap_conversions::userDataFromROS
cv::Mat userDataFromROS(const rtabmap_msgs::UserData &dataMsg)
rtabmap_slam::CoreWrapper::goalReachedPub_
ros::Publisher goalReachedPub_
Definition: CoreWrapper.h:316
fabs
Real fabs(const Real &a)
rtabmap_slam::CoreWrapper::getNodeDataSrv_
ros::ServiceServer getNodeDataSrv_
Definition: CoreWrapper.h:342
UDirectory::homeDir
static std::string homeDir()
rtabmap_slam::CoreWrapper::paused_
bool paused_
Definition: CoreWrapper.h:254
labels
std::vector< std::string > labels
rtabmap_conversions::odomInfoFromROS
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::OdomInfo &msg, bool ignoreData=false)
rtabmap_slam::CoreWrapper::goalCommonCallback
void goalCommonCallback(int id, const std::string &label, const std::string &frameId, const rtabmap::Transform &pose, const ros::Time &stamp, double *planningTime=0)
Definition: CoreWrapper.cpp:2566
rtabmap::Rtabmap::getStatistics
const Statistics & getStatistics() const
rtabmap_slam::CoreWrapper::detectMoreLoopClosuresCallback
bool detectMoreLoopClosuresCallback(rtabmap_msgs::DetectMoreLoopClosures::Request &, rtabmap_msgs::DetectMoreLoopClosures::Response &)
Definition: CoreWrapper.cpp:3107
rtabmap_conversions::landmarksFromROS
rtabmap::Landmarks landmarksFromROS(const std::map< int, std::pair< geometry_msgs::PoseWithCovarianceStamped, float > > &tags, const std::string &frameId, const std::string &odomFrameId, const ros::Time &odomStamp, tf::TransformListener &listener, double waitForTransform, double defaultLinVariance, double defaultAngVariance)
uReplaceChar
std::string uReplaceChar(const std::string &str, char before, char after)
rtabmap_slam::CoreWrapper::mapFrameId_
std::string mapFrameId_
Definition: CoreWrapper.h:270
rtabmap_slam::CoreWrapper::stereoToDepth_
bool stereoToDepth_
Definition: CoreWrapper.h:395
rtabmap::IMU
rtabmap_slam::CoreWrapper::odomUpdate
bool odomUpdate(const nav_msgs::OdometryConstPtr &odomMsg, ros::Time stamp)
Definition: CoreWrapper.cpp:1022
CoreWrapper.h
uContains
bool uContains(const std::list< V > &list, const V &value)
rtabmap::Transform::z
float & z()
rtabmap_slam::CoreWrapper::twoDMapping_
bool twoDMapping_
Definition: CoreWrapper.h:402
rtabmap::Statistics::data
const std::map< std::string, float > & data() const
UDirectory::currentDir
static std::string currentDir(bool trailingSeparator=false)
rtabmap_slam::CoreWrapper::publishGlobalPath
void publishGlobalPath(const ros::Time &stamp)
Definition: CoreWrapper.cpp:4501
rtabmap_slam::CoreWrapper::initialPoseSub_
ros::Subscriber initialPoseSub_
Definition: CoreWrapper.h:310
rtabmap_slam::CoreWrapper::goalFeedbackCb
void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)
Definition: CoreWrapper.cpp:4457
rtabmap::Rtabmap::getLocalRadius
float getLocalRadius() const
rtabmap_slam::CoreWrapper::scanCloudMaxPoints_
int scanCloudMaxPoints_
Definition: CoreWrapper.h:291
rtabmap_conversions::nodeToROS
void nodeToROS(const rtabmap::Signature &signature, rtabmap_msgs::Node &msg)
rtabmap_slam::CoreWrapper::goalDoneCb
void goalDoneCb(const actionlib::SimpleClientGoalState &state, const move_base_msgs::MoveBaseResultConstPtr &result)
Definition: CoreWrapper.cpp:4405
rtabmap_slam::CoreWrapper::getMapSrv_
ros::ServiceServer getMapSrv_
Definition: CoreWrapper.h:346
ULogger::setEventLevel
static void setEventLevel(ULogger::Level eventSentLevel)
octomap_msgs::binaryMapToMsg
static bool binaryMapToMsg(const OctomapT &octomap, Octomap &msg)
rtabmap_slam::CoreWrapper::updateSrv_
ros::ServiceServer updateSrv_
Definition: CoreWrapper.h:326
uBool2Str
std::string uBool2Str(bool boolean)
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
rtabmap::Rtabmap::getNodesInRadius
std::map< int, Transform > getNodesInRadius(const Transform &pose, float radius, int k=0, std::map< int, float > *distsSqr=0)
rtabmap_slam::CoreWrapper::odomTFUpdate
bool odomTFUpdate(const ros::Time &stamp)
Definition: CoreWrapper.cpp:1133
diagnostic_updater::DiagnosticStatusWrapper::summary
void summary(const diagnostic_msgs::DiagnosticStatus &src)
UScopeMutex
rtabmap_slam::CoreWrapper::loadDatabaseSrv_
ros::ServiceServer loadDatabaseSrv_
Definition: CoreWrapper.h:330
rtabmap_slam::CoreWrapper::getPlanNodesCallback
bool getPlanNodesCallback(rtabmap_msgs::GetPlan::Request &req, rtabmap_msgs::GetPlan::Response &res)
Definition: CoreWrapper.cpp:3849
UDirectory::getDir
static std::string getDir(const std::string &filePath)
rtabmap_util::MapsManager::getFilteredPoses
std::map< int, rtabmap::Transform > getFilteredPoses(const std::map< int, rtabmap::Transform > &poses)
rtabmap_slam::CoreWrapper::updateRtabmapCallback
bool updateRtabmapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2756
rtabmap::Optimizer::Type
Type
MAX
#define MAX(a, b)
uInsert
void uInsert(std::map< K, V > &map, const std::map< K, V > &items)
rtabmap_slam::CoreWrapper::rate_
float rate_
Definition: CoreWrapper.h:397
rtabmap_slam::CoreWrapper::mappingAltitudeDelta_
double mappingAltitudeDelta_
Definition: CoreWrapper.h:400
stats
bool stats
rtabmap::Rtabmap::getTimeThreshold
float getTimeThreshold() const
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
q
EIGEN_DEVICE_FUNC const Scalar & q
rtabmap::Rtabmap::triggerNewMap
int triggerNewMap()
rtabmap_slam::CoreWrapper::getMapData2Srv_
ros::ServiceServer getMapData2Srv_
Definition: CoreWrapper.h:344
sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::Rtabmap::addLink
bool addLink(const Link &link)
rtabmap_slam::CoreWrapper::cancelGoalSrv_
ros::ServiceServer cancelGoalSrv_
Definition: CoreWrapper.h:353
rtabmap_slam::CoreWrapper::mapToOdomMutex_
boost::mutex mapToOdomMutex_
Definition: CoreWrapper.h:295
rtabmap_slam::CoreWrapper::globalPoseAsyncCallback
void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &globalPoseMsg)
Definition: CoreWrapper.cpp:2368
uMax3
T uMax3(const T &a, const T &b, const T &c)
rtabmap_slam::CoreWrapper::fiducialTransfromsSub_
ros::Subscriber fiducialTransfromsSub_
Definition: CoreWrapper.h:381
rtabmap::Rtabmap::clearPath
void clearPath(int status)
rtabmap_slam::CoreWrapper::publishMapDataSrv_
ros::ServiceServer publishMapDataSrv_
Definition: CoreWrapper.h:349
ROS_DEBUG
#define ROS_DEBUG(...)
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())
BAD_COVARIANCE
#define BAD_COVARIANCE
Definition: CoreWrapper.cpp:73
rtabmap_slam::CoreWrapper::graphLatched_
bool graphLatched_
Definition: CoreWrapper.h:264
TimeBase< Time, Duration >::isZero
bool isZero() const
rtabmap_slam::CoreWrapper::landmarkDefaultLinVariance_
double landmarkDefaultLinVariance_
Definition: CoreWrapper.h:278
actionlib::SimpleActionClient::sendGoal
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
rtabmap_slam::CoreWrapper::gpsFixAsyncSub_
ros::Subscriber gpsFixAsyncSub_
Definition: CoreWrapper.h:378
diagnostic_updater
rtabmap_slam::CoreWrapper::getPlanSrv_
ros::ServiceServer getPlanSrv_
Definition: CoreWrapper.h:350
rtabmap_slam::CoreWrapper::odomDefaultLinVariance_
double odomDefaultLinVariance_
Definition: CoreWrapper.h:276
rtabmap_slam::CoreWrapper::resetSrv_
ros::ServiceServer resetSrv_
Definition: CoreWrapper.h:327
rtabmap_slam::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_slam::CoreWrapper, nodelet::Nodelet)
rtabmap_slam::CoreWrapper::getMapCallback
bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: CoreWrapper.cpp:3446
ULogger::kWarning
kWarning
rtabmap_slam::CoreWrapper::transformThread_
boost::thread * transformThread_
Definition: CoreWrapper.h:366
rtabmap_slam::CoreWrapper::tfBroadcaster_
tf2_ros::TransformBroadcaster tfBroadcaster_
Definition: CoreWrapper.h:323
rtabmap::Memory::getAllLabels
const std::map< int, std::string > & getAllLabels() const
rtabmap::Rtabmap::computePath
bool computePath(const Transform &targetPose, float tolerance=-1.0f)
rtabmap_slam::CoreWrapper::genScan_
bool genScan_
Definition: CoreWrapper.h:283
rtabmap_slam::CoreWrapper::userDataAsyncSub_
ros::Subscriber userDataAsyncSub_
Definition: CoreWrapper.h:372
ULogger::kInfo
kInfo
rtabmap::Rtabmap::getSignatureCopy
Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const
rtabmap::Rtabmap::setInitialPose
void setInitialPose(const Transform &initialPose)
rtabmap_slam::CoreWrapper::loadDatabaseCallback
bool loadDatabaseCallback(rtabmap_msgs::LoadDatabase::Request &, rtabmap_msgs::LoadDatabase::Response &)
Definition: CoreWrapper.cpp:2886
orientation
GLM_FUNC_DECL detail::tmat4x4< T, P > orientation(detail::tvec3< T, P > const &Normal, detail::tvec3< T, P > const &Up)
info
else if n * info
rtabmap_slam::CoreWrapper::odomDefaultAngVariance_
double odomDefaultAngVariance_
Definition: CoreWrapper.h:275
rtabmap_slam::CoreWrapper::publishMapCallback
bool publishMapCallback(rtabmap_msgs::PublishMap::Request &, rtabmap_msgs::PublishMap::Response &)
Definition: CoreWrapper.cpp:3528
rtabmap_sync::CommonDataSubscriber::isSubscribedToSensorData
bool isSubscribedToSensorData() const
rtabmap_slam::CoreWrapper::setModeLocalizationSrv_
ros::ServiceServer setModeLocalizationSrv_
Definition: CoreWrapper.h:336
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
rtabmap::SensorData::setId
void setId(int id)
UASSERT
#define UASSERT(condition)
argv
argv
UDirectory.h
rtabmap_slam::CoreWrapper::setLogInfo
bool setLogInfo(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:3301
ROS_WARN
#define ROS_WARN(...)
z
z
uValues
std::vector< V > uValues(const std::map< K, V > &m)
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
rtabmap_slam::CoreWrapper::defaultCallback
void defaultCallback(const sensor_msgs::ImageConstPtr &imageMsg)
Definition: CoreWrapper.cpp:953
x
x
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
error
void error(const char *str)
rtabmap_sync::CommonDataSubscriber::isDataSubscribed
bool isDataSubscribed() const
p
Point3_ p(2)
actionlib::SimpleClientGoalState::SUCCEEDED
SUCCEEDED
rtabmap_slam::CoreWrapper::mapGraphPub_
ros::Publisher mapGraphPub_
Definition: CoreWrapper.h:301
rtabmap_slam::CoreWrapper::localGridObstacle_
ros::Publisher localGridObstacle_
Definition: CoreWrapper.h:306
rtabmap::GPS::stamp
const double & stamp() const
rtabmap_slam::CoreWrapper::republishMaps
void republishMaps()
Definition: CoreWrapper.cpp:3070
rtabmap_slam::CoreWrapper::groundTruthBaseFrameId_
std::string groundTruthBaseFrameId_
Definition: CoreWrapper.h:272
rtabmap::Rtabmap::getLastLocationId
int getLastLocationId() const
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
rtabmap::Parameters
rtabmap::Transform::setIdentity
void setIdentity()
rtabmap_slam::CoreWrapper::interOdomInfoCallback
void interOdomInfoCallback(const nav_msgs::OdometryConstPtr &msg1, const rtabmap_msgs::OdomInfoConstPtr &msg2)
Definition: CoreWrapper.cpp:2518
rtabmap_util::MapsManager::set2DMap
void set2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize, const std::map< int, rtabmap::Transform > &poses, const rtabmap::Memory *memory=0)
rtabmap::util2d::depthFromDisparity
cv::Mat RTABMAP_CORE_EXPORT depthFromDisparity(const cv::Mat &disparity, float fx, float baseline, int type=CV_32FC1)
rtabmap_slam::CoreWrapper::goalActiveCb
void goalActiveCb()
Definition: CoreWrapper.cpp:4451
LocalGridMaker.h
rtabmap_util::MapsManager::getGridMap
cv::Mat getGridMap(float &xMin, float &yMin, float &gridCellSize)
rtabmap_slam::CoreWrapper::setLogDebugSrv_
ros::ServiceServer setLogDebugSrv_
Definition: CoreWrapper.h:338
rtabmap_slam::CoreWrapper::interOdomCallback
void interOdomCallback(const nav_msgs::OdometryConstPtr &msg)
Definition: CoreWrapper.cpp:2510
rtabmap_slam::CoreWrapper::localGridGround_
ros::Publisher localGridGround_
Definition: CoreWrapper.h:308
model
noiseModel::Diagonal::shared_ptr model
pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap_conversions::infoToROS
void infoToROS(const rtabmap::Statistics &stats, rtabmap_msgs::Info &info)
rtabmap_slam::CoreWrapper::mapDataPub_
ros::Publisher mapDataPub_
Definition: CoreWrapper.h:300
rtabmap_slam::CoreWrapper::detectMoreLoopClosuresSrv_
ros::ServiceServer detectMoreLoopClosuresSrv_
Definition: CoreWrapper.h:333
rtabmap::Transform::inverse
Transform inverse() const
ros::Rate::sleep
bool sleep()
rtabmap::Rtabmap::getPathCurrentGoalIndex
unsigned int getPathCurrentGoalIndex() const
rtabmap_slam::CoreWrapper::alreadyRectifiedImages_
bool alreadyRectifiedImages_
Definition: CoreWrapper.h:401
actionlib::SimpleActionClient::isServerConnected
bool isServerConnected() const
path
path
rtabmap_sync::CommonDataSubscriber::tick
void tick(const ros::Time &stamp, double targetFrequency=0)
UWARN
#define UWARN(...)
rtabmap::Rtabmap::init
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
rtabmap_slam::CoreWrapper::lastPose_
rtabmap::Transform lastPose_
Definition: CoreWrapper.h:255
rtabmap::Rtabmap::getPose
Transform getPose(int locationId) const
rtabmap::DBDriver
uStr2Int
int uStr2Int(const std::string &str)
rtabmap_slam::CoreWrapper::publishStats
void publishStats(const ros::Time &stamp)
Definition: CoreWrapper.cpp:4083
rtabmap_slam::CoreWrapper::useSavedMap_
bool useSavedMap_
Definition: CoreWrapper.h:282
rtabmap_slam::CoreWrapper::pubLocPoseOnlyWhenLocalizing_
bool pubLocPoseOnlyWhenLocalizing_
Definition: CoreWrapper.h:263
rtabmap_util::MapsManager::hasSubscribers
bool hasSubscribers() const
rtabmap_slam::CoreWrapper::pauseRtabmapCallback
bool pauseRtabmapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2856
timer::elapsed
double elapsed() const
nodes
KeyVector nodes
rtabmap_util::MapsManager::init
void init(ros::NodeHandle &nh, ros::NodeHandle &pnh, const std::string &name, bool usePublicNamespace)
rtabmap_slam::CoreWrapper::listLabelsCallback
bool listLabelsCallback(rtabmap_msgs::ListLabels::Request &req, rtabmap_msgs::ListLabels::Response &res)
Definition: CoreWrapper.cpp:4006
rtabmap_slam::CoreWrapper::getGridMapCallback
bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: CoreWrapper.cpp:3440
rtabmap::Rtabmap::getWMSize
int getWMSize() const
rtabmap::Rtabmap::getPathStatus
int getPathStatus() const
rtabmap::Memory::getWorkingMem
const std::map< int, double > & getWorkingMem() const
rtabmap_slam::CoreWrapper::userData_
cv::Mat userData_
Definition: CoreWrapper.h:373
message_filters::Subscriber::subscribe
void subscribe()
Eigen::Quaternion
rtabmap::Memory::load2DMap
cv::Mat load2DMap(float &xMin, float &yMin, float &cellSize) const
NODELET_INFO
#define NODELET_INFO(...)
rtabmap_slam::CoreWrapper::imuAsyncCallback
void imuAsyncCallback(const sensor_msgs::ImuConstPtr &tagDetections)
Definition: CoreWrapper.cpp:2470
rtabmap::Rtabmap::getPath
const std::vector< std::pair< int, Transform > > & getPath() const
rtabmap_slam::CoreWrapper::interOdomSub_
ros::Subscriber interOdomSub_
Definition: CoreWrapper.h:388
rtabmap::Transform
rtabmap_slam::CoreWrapper::publishLoop
void publishLoop(double tfDelay, double tfTolerance)
Definition: CoreWrapper.cpp:930
actionlib::SimpleActionClient::cancelGoal
void cancelGoal()
rtabmap_slam::CoreWrapper::infoPub_
ros::Publisher infoPub_
Definition: CoreWrapper.h:299
UTimer::ticks
double ticks()
rtabmap_slam::CoreWrapper::getNodesInRadiusCallback
bool getNodesInRadiusCallback(rtabmap_msgs::GetNodesInRadius::Request &, rtabmap_msgs::GetNodesInRadius::Response &)
Definition: CoreWrapper.cpp:4050
rtabmap_slam::CoreWrapper::localizationDiagnostic_
LocalizationStatusTask localizationDiagnostic_
Definition: CoreWrapper.h:418
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_CORE_EXPORT cvtDepthFromFloat(const cv::Mat &depth32F)
UDirectory::exists
static bool exists(const std::string &dirPath)
rtabmap_slam::CoreWrapper::lastPoseIntermediate_
bool lastPoseIntermediate_
Definition: CoreWrapper.h:258
Optimizer.h
rtabmap_conversions::timestampFromROS
double timestampFromROS(const ros::Time &stamp)
rtabmap_conversions::getMovingTransform
rtabmap::Transform getMovingTransform(const std::string &movingFrame, const std::string &fixedFrame, const ros::Time &stampFrom, const ros::Time &stampTo, tf::TransformListener &listener, double waitForTransform)
rtabmap_slam::CoreWrapper::genDepthDecimation_
int genDepthDecimation_
Definition: CoreWrapper.h:287
rtabmap_slam::CoreWrapper::tags_
std::map< int, std::pair< geometry_msgs::PoseWithCovarianceStamped, float > > tags_
Definition: CoreWrapper.h:382
nodelet::Nodelet
rtabmap_slam::CoreWrapper::getGridMapSrv_
ros::ServiceServer getGridMapSrv_
Definition: CoreWrapper.h:348
rtabmap_slam::CoreWrapper::tfListener_
tf::TransformListener tfListener_
Definition: CoreWrapper.h:324
ros::Time
rtabmap::Signature::id
int id() const
rtabmap::Rtabmap::process
bool process(const cv::Mat &image, int id=0, const std::map< std::string, float > &externalStats=std::map< std::string, float >())
rtabmap_slam::CoreWrapper::odomFrameId_
std::string odomFrameId_
Definition: CoreWrapper.h:269
UFile.h
rtabmap_slam::CoreWrapper::triggerNewMapCallback
bool triggerNewMapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:3020
rtabmap_slam::CoreWrapper::getPlanCallback
bool getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &res)
Definition: CoreWrapper.cpp:3775
iter
iterator iter(handle obj)
rtabmap_slam::CoreWrapper::removeLabelCallback
bool removeLabelCallback(rtabmap_msgs::RemoveLabel::Request &req, rtabmap_msgs::RemoveLabel::Response &res)
Definition: CoreWrapper.cpp:4018
rtabmap::Rtabmap::detectMoreLoopClosures
int detectMoreLoopClosures(float clusterRadiusMax=0.5f, float clusterAngle=M_PI/6.0f, int iterations=1, bool intraSession=true, bool interSession=true, const ProgressState *state=0, float clusterRadiusMin=0.0f)
rtabmap_slam::CoreWrapper::setGoalCallback
bool setGoalCallback(rtabmap_msgs::SetGoal::Request &req, rtabmap_msgs::SetGoal::Response &res)
Definition: CoreWrapper.cpp:3938
rtabmap::Rtabmap::isIDsGenerated
bool isIDsGenerated() const
rtabmap_slam::CoreWrapper::defaultSub_
image_transport::Subscriber defaultSub_
Definition: CoreWrapper.h:370
sensor_msgs::image_encodings::MONO8
const std::string MONO8
rtabmap::Rtabmap::getPathNextNodes
std::vector< int > getPathNextNodes() const
rtabmap_slam::CoreWrapper::mapsManager_
rtabmap_util::MapsManager mapsManager_
Definition: CoreWrapper.h:297
rtabmap_slam::CoreWrapper::odomCachePub_
ros::Publisher odomCachePub_
Definition: CoreWrapper.h:302
rtabmap::Transform::isIdentity
bool isIdentity() const
rtabmap_slam::CoreWrapper::tfThreadRunning_
bool tfThreadRunning_
Definition: CoreWrapper.h:367
rtabmap::Rtabmap::parseParameters
void parseParameters(const ParametersMap &parameters)
rtabmap_slam::CoreWrapper::goalFrameId_
std::string goalFrameId_
Definition: CoreWrapper.h:321
OccupancyGrid.h
rtabmap_slam::CoreWrapper::setLogDebug
bool setLogDebug(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:3295
rtabmap_slam::CoreWrapper::commonOdomCallback
virtual void commonOdomCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_msgs::UserDataConstPtr &userDataMsg, const rtabmap_msgs::OdomInfoConstPtr &odomInfoMsg)
Definition: CoreWrapper.cpp:1700
rtabmap_slam::CoreWrapper::pauseSrv_
ros::ServiceServer pauseSrv_
Definition: CoreWrapper.h:328
c_str
const char * c_str(Args &&...args)
rtabmap_slam::CoreWrapper::genDepthFillIterations_
int genDepthFillIterations_
Definition: CoreWrapper.h:289
cv_bridge.h
rtabmap_slam::CoreWrapper::rtabmapROSStats_
std::map< std::string, float > rtabmapROSStats_
Definition: CoreWrapper.h:266
rtabmap_slam::CoreWrapper::goalSub_
ros::Subscriber goalSub_
Definition: CoreWrapper.h:313
sensor_msgs::image_encodings::MONO16
const std::string MONO16
uStr2Bool
bool uStr2Bool(const char *str)
rtabmap_slam::CoreWrapper::interOdomSyncSub_
message_filters::Subscriber< nav_msgs::Odometry > interOdomSyncSub_
Definition: CoreWrapper.h:390
ROS_ERROR
#define ROS_ERROR(...)
rtabmap::Rtabmap::close
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
rtabmap_slam::CoreWrapper::saveParameters
void saveParameters(const std::string &configFile)
Definition: CoreWrapper.cpp:912
rtabmap_conversions::transformToGeometryMsg
void transformToGeometryMsg(const rtabmap::Transform &transform, geometry_msgs::Transform &msg)
rtabmap::OdometryInfo::interval
double interval
rtabmap_slam::CoreWrapper::getProjMapCallback
bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
Definition: CoreWrapper.cpp:3422
rtabmap_sync::CommonDataSubscriber::isSubscribedToOdom
bool isSubscribedToOdom() const
rtabmap_slam::CoreWrapper::setModeMappingSrv_
ros::ServiceServer setModeMappingSrv_
Definition: CoreWrapper.h:337
class_list_macros.hpp
rtabmap_conversions::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_msgs::MapData &msg)
UDEBUG
#define UDEBUG(...)
DBDriver.h
nodelet::Nodelet::getName
const std::string & getName() const
rtabmap_slam::CoreWrapper::genScanMinDepth_
double genScanMinDepth_
Definition: CoreWrapper.h:285
OdometryEvent.h
UTimer
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
rtabmap::OdometryInfo
uKeys
std::vector< K > uKeys(const std::map< K, V > &m)
M_PI
#define M_PI
diagnostic_updater::DiagnosticStatusWrapper
rtabmap_slam::CoreWrapper::localizationPosePub_
ros::Publisher localizationPosePub_
Definition: CoreWrapper.h:309
rtabmap_slam::CoreWrapper::cancelGoalCallback
bool cancelGoalCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: CoreWrapper.cpp:3954
rtabmap::Rtabmap::getMapCorrection
Transform getMapCorrection() const
scan_step::second
@ second
rtabmap_slam::CoreWrapper::getNodeDataCallback
bool getNodeDataCallback(rtabmap_msgs::GetNodeData::Request &req, rtabmap_msgs::GetNodeData::Response &res)
Definition: CoreWrapper.cpp:3320
rtabmap_slam::CoreWrapper::getMapDataCallback
bool getMapDataCallback(rtabmap_msgs::GetMap::Request &req, rtabmap_msgs::GetMap::Response &res)
Definition: CoreWrapper.cpp:3349
rtabmap_slam::CoreWrapper::globalPathPub_
ros::Publisher globalPathPub_
Definition: CoreWrapper.h:317
rtabmap_slam::CoreWrapper
Definition: CoreWrapper.h:102
rtabmap_slam::CoreWrapper::tagDetectionsSub_
ros::Subscriber tagDetectionsSub_
Definition: CoreWrapper.h:380
ULogger::kFatal
kFatal
conversions.h
ros::Rate
rtabmap::Memory::getDatabaseVersion
std::string getDatabaseVersion() const
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointNormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
sensor_msgs::image_encodings::BGR8
const std::string BGR8
rtabmap::OctoMap
rtabmap_slam::CoreWrapper::onInit
virtual void onInit()
Definition: CoreWrapper.cpp:140
rtabmap_conversions::convertScan3dMsg
bool convertScan3dMsg(const sensor_msgs::PointCloud2 &scan3dMsg, const std::string &frameId, const std::string &odomFrameId, const ros::Time &odomStamp, rtabmap::LaserScan &scan, tf::TransformListener &listener, double waitForTransform, int maxPoints=0, float maxRange=0.0f, bool is2D=false)
rtabmap_slam::CoreWrapper::gps_
rtabmap::GPS gps_
Definition: CoreWrapper.h:379
MoveBaseClient
actionlib::SimpleActionClient< move_base_msgs::MoveBaseAction > MoveBaseClient
Definition: CoreWrapper.h:94
ULogger::Level
Level
rtabmap::Rtabmap::getLocalConstraints
const std::multimap< int, Link > & getLocalConstraints() const
false
#define false
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
rtabmap::LocalGridMaker::isGridFromDepth
bool isGridFromDepth() const
image_transport::TransportHints
util3d_surface.h
rtabmap_slam::CoreWrapper::getMapData2Callback
bool getMapData2Callback(rtabmap_msgs::GetMap2::Request &req, rtabmap_msgs::GetMap2::Response &res)
Definition: CoreWrapper.cpp:3383
rtabmap_util::MapsManager::getLocalMapMaker
const rtabmap::LocalGridMaker * getLocalMapMaker() const
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::Image &source, const boost::shared_ptr< void const > &tracked_object, const std::string &encoding=std::string())
rtabmap::Transform::to3DoF
Transform to3DoF() const
rtabmap_slam::CoreWrapper::databasePath_
std::string databasePath_
Definition: CoreWrapper.h:274
rtabmap_slam::CoreWrapper::goalCallback
void goalCallback(const geometry_msgs::PoseStampedConstPtr &msg)
Definition: CoreWrapper.cpp:2713
rtabmap_slam::CoreWrapper::setLogInfoSrv_
ros::ServiceServer setLogInfoSrv_
Definition: CoreWrapper.h:339
UTimer.h
rtabmap_slam::CoreWrapper::getPlanNodesSrv_
ros::ServiceServer getPlanNodesSrv_
Definition: CoreWrapper.h:351
header
const std::string header
Memory.h
uStr2Float
float uStr2Float(const std::string &str)
t
Point2 t(10, 10)
ROS_INFO
#define ROS_INFO(...)
rtabmap::Signature::sensorData
SensorData & sensorData()
UConversion.h
rtabmap_slam::CoreWrapper::setLabelCallback
bool setLabelCallback(rtabmap_msgs::SetLabel::Request &req, rtabmap_msgs::SetLabel::Response &res)
Definition: CoreWrapper.cpp:3979
MsgConversion.h
rtabmap
rtabmap_slam::CoreWrapper::getMapDataSrv_
ros::ServiceServer getMapDataSrv_
Definition: CoreWrapper.h:343
UFile::exists
bool exists()
rtabmap_util::MapsManager::setParameters
void setParameters(const rtabmap::ParametersMap &parameters)
util3d_transforms.h
rtabmap_slam::CoreWrapper::landmarkDefaultAngVariance_
double landmarkDefaultAngVariance_
Definition: CoreWrapper.h:277
UERROR
#define UERROR(...)
rtabmap::Rtabmap::getPathCurrentGoalId
int getPathCurrentGoalId() const
ros::Duration
rtabmap_slam::CoreWrapper::getProbMapSrv_
ros::ServiceServer getProbMapSrv_
Definition: CoreWrapper.h:347
rtabmap_conversions::linkFromROS
rtabmap::Link linkFromROS(const rtabmap_msgs::Link &msg)
value
value
Graph.h
rtabmap_slam::CoreWrapper::userDataAsyncCallback
void userDataAsyncCallback(const rtabmap_msgs::UserDataConstPtr &dataMsg)
Definition: CoreWrapper.cpp:2349
rtabmap_sync::CommonDataSubscriber::isSubscribedToStereo
bool isSubscribedToStereo() const
i
int i
uIsFinite
bool uIsFinite(const T &value)
rtabmap_slam::CoreWrapper::useActionForGoal_
bool useActionForGoal_
Definition: CoreWrapper.h:281
rtabmap_slam::CoreWrapper::odomSensorSync_
bool odomSensorSync_
Definition: CoreWrapper.h:396
image_transport::Subscriber::getTopic
std::string getTopic() const
rtabmap::Rtabmap::labelLocation
bool labelLocation(int id, const std::string &label)
rtabmap_slam::CoreWrapper::resetRtabmapCallback
bool resetRtabmapCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:2825
result
RESULT & result
rtabmap_slam::CoreWrapper::addLinkSrv_
ros::ServiceServer addLinkSrv_
Definition: CoreWrapper.h:357
rtabmap_slam::CoreWrapper::waitForTransform_
bool waitForTransform_
Definition: CoreWrapper.h:279
rtabmap_util::MapsManager::updateMapCaches
std::map< int, rtabmap::Transform > updateMapCaches(const std::map< int, rtabmap::Transform > &poses, const rtabmap::Memory *memory, bool updateGrid, bool updateOctomap, const std::map< int, rtabmap::Signature > &signatures=std::map< int, rtabmap::Signature >())
rtabmap_slam::CoreWrapper::covariance_
cv::Mat covariance_
Definition: CoreWrapper.h:259
rtabmap::Rtabmap::resetMemory
void resetMemory()
rtabmap_slam::CoreWrapper::cleanupLocalGridsSrv_
ros::ServiceServer cleanupLocalGridsSrv_
Definition: CoreWrapper.h:335
baseline
double baseline
util3d.h
rtabmap_conversions::sensorDataFromROS
rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::SensorData &msg)
roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON
static double COVARIANCE_ANGULAR_EPSILON
pcl_conversions::moveFromPCL
void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
UMath.h
ros::NodeHandle
rtabmap::Signature
rtabmap_slam::CoreWrapper::rtabmap_
rtabmap::Rtabmap rtabmap_
Definition: CoreWrapper.h:253
rtabmap_slam::CoreWrapper::lastPoseStamp_
ros::Time lastPoseStamp_
Definition: CoreWrapper.h:256
rtabmap_util::MapsManager::backwardCompatibilityParameters
void backwardCompatibilityParameters(ros::NodeHandle &pnh, rtabmap::ParametersMap &parameters) const
rtabmap_slam::CoreWrapper::localGridEmpty_
ros::Publisher localGridEmpty_
Definition: CoreWrapper.h:307
rtabmap_util::MapsManager::publishMaps
void publishMaps(const std::map< int, rtabmap::Transform > &poses, const ros::Time &stamp, const std::string &mapFrameId)
util2d.h
state
state
ros::Time::now
static Time now()
rtabmap_slam::CoreWrapper::backupDatabase_
ros::ServiceServer backupDatabase_
Definition: CoreWrapper.h:332
rtabmap_slam::CoreWrapper::imuSub_
ros::Subscriber imuSub_
Definition: CoreWrapper.h:383
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
pcl_conversions.h
rtabmap_slam::CoreWrapper::backupDatabaseCallback
bool backupDatabaseCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CoreWrapper.cpp:3027

rtabmap_slam
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:43:58