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