CommonDataSubscriber.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_sync/CommonDataSubscriber.h>
29 
30 namespace rtabmap_sync {
31 
32 CommonDataSubscriber::CommonDataSubscriber(bool gui) :
33  topicQueueSize_(10),
34  syncQueueSize_(10),
35  approxSync_(true),
36  subscribedToDepth_(!gui),
37  subscribedToStereo_(false),
38  subscribedToRGB_(!gui),
39  subscribedToOdom_(false),
40  subscribedToRGBD_(false),
41  subscribedToSensorData_(false),
42  subscribedToScan2d_(false),
43  subscribedToScan3d_(false),
44  subscribedToScanDescriptor_(false),
45  subscribedToOdomInfo_(false),
46 
47  // RGB + Depth
48  SYNC_INIT(depth),
49  SYNC_INIT(depthScan2d),
50  SYNC_INIT(depthScan3d),
51  SYNC_INIT(depthScanDesc),
52  SYNC_INIT(depthInfo),
53  SYNC_INIT(depthScan2dInfo),
54  SYNC_INIT(depthScan3dInfo),
55  SYNC_INIT(depthScanDescInfo),
56 
57  // RGB + Depth + Odom
58  SYNC_INIT(depthOdom),
59  SYNC_INIT(depthOdomScan2d),
60  SYNC_INIT(depthOdomScan3d),
61  SYNC_INIT(depthOdomScanDesc),
62  SYNC_INIT(depthOdomInfo),
63  SYNC_INIT(depthOdomScan2dInfo),
64  SYNC_INIT(depthOdomScan3dInfo),
65  SYNC_INIT(depthOdomScanDescInfo),
66 
67 #ifdef RTABMAP_SYNC_USER_DATA
68  // RGB + Depth + User Data
69  SYNC_INIT(depthData),
70  SYNC_INIT(depthDataScan2d),
71  SYNC_INIT(depthDataScan3d),
72  SYNC_INIT(depthDataScanDesc),
73  SYNC_INIT(depthDataInfo),
74  SYNC_INIT(depthDataScan2dInfo),
75  SYNC_INIT(depthDataScan3dInfo),
76  SYNC_INIT(depthDataScanDescInfo),
77 
78  // RGB + Depth + Odom + User Data
79  SYNC_INIT(depthOdomData),
80  SYNC_INIT(depthOdomDataScan2d),
81  SYNC_INIT(depthOdomDataScan3d),
82  SYNC_INIT(depthOdomDataScanDesc),
83  SYNC_INIT(depthOdomDataInfo),
84  SYNC_INIT(depthOdomDataScan2dInfo),
85  SYNC_INIT(depthOdomDataScan3dInfo),
86  SYNC_INIT(depthOdomDataScanDescInfo),
87 #endif
88 
89  // Stereo
90  SYNC_INIT(stereo),
91  SYNC_INIT(stereoInfo),
92 
93  // Stereo + Odom
94  SYNC_INIT(stereoOdom),
95  SYNC_INIT(stereoOdomInfo),
96 
97  // RGB-only
98  SYNC_INIT(rgb),
99  SYNC_INIT(rgbScan2d),
100  SYNC_INIT(rgbScan3d),
101  SYNC_INIT(rgbScanDesc),
102  SYNC_INIT(rgbInfo),
103  SYNC_INIT(rgbScan2dInfo),
104  SYNC_INIT(rgbScan3dInfo),
105  SYNC_INIT(rgbScanDescInfo),
106 
107  // RGB-only + Odom
108  SYNC_INIT(rgbOdom),
109  SYNC_INIT(rgbOdomScan2d),
110  SYNC_INIT(rgbOdomScan3d),
111  SYNC_INIT(rgbOdomScanDesc),
112  SYNC_INIT(rgbOdomInfo),
113  SYNC_INIT(rgbOdomScan2dInfo),
114  SYNC_INIT(rgbOdomScan3dInfo),
115  SYNC_INIT(rgbOdomScanDescInfo),
116 
117 #ifdef RTABMAP_SYNC_USER_DATA
118  // RGB-only + User Data
119  SYNC_INIT(rgbData),
120  SYNC_INIT(rgbDataScan2d),
121  SYNC_INIT(rgbDataScan3d),
122  SYNC_INIT(rgbDataScanDesc),
123  SYNC_INIT(rgbDataInfo),
124  SYNC_INIT(rgbDataScan2dInfo),
125  SYNC_INIT(rgbDataScan3dInfo),
126  SYNC_INIT(rgbDataScanDescInfo),
127 
128  // RGB-only + Odom + User Data
129  SYNC_INIT(rgbOdomData),
130  SYNC_INIT(rgbOdomDataScan2d),
131  SYNC_INIT(rgbOdomDataScan3d),
132  SYNC_INIT(rgbOdomDataScanDesc),
133  SYNC_INIT(rgbOdomDataInfo),
134  SYNC_INIT(rgbOdomDataScan2dInfo),
135  SYNC_INIT(rgbOdomDataScan3dInfo),
136  SYNC_INIT(rgbOdomDataScanDescInfo),
137 #endif
138  // 1 RGBD
139  SYNC_INIT(rgbdScan2d),
140  SYNC_INIT(rgbdScan3d),
141  SYNC_INIT(rgbdScanDesc),
142  SYNC_INIT(rgbdInfo),
143 
144  // 1 RGBD + Odom
145  SYNC_INIT(rgbdOdom),
146  SYNC_INIT(rgbdOdomScan2d),
147  SYNC_INIT(rgbdOdomScan3d),
148  SYNC_INIT(rgbdOdomScanDesc),
149  SYNC_INIT(rgbdOdomInfo),
150 
151 #ifdef RTABMAP_SYNC_USER_DATA
152  // 1 RGBD + User Data
153  SYNC_INIT(rgbdData),
154  SYNC_INIT(rgbdDataScan2d),
155  SYNC_INIT(rgbdDataScan3d),
156  SYNC_INIT(rgbdDataScanDesc),
157  SYNC_INIT(rgbdDataInfo),
158 
159  // 1 RGBD + Odom + User Data
160  SYNC_INIT(rgbdOdomData),
161  SYNC_INIT(rgbdOdomDataScan2d),
162  SYNC_INIT(rgbdOdomDataScan3d),
163  SYNC_INIT(rgbdOdomDataScanDesc),
164  SYNC_INIT(rgbdOdomDataInfo),
165 #endif
166  // X RGBD
167  SYNC_INIT(rgbdXScan2d),
168  SYNC_INIT(rgbdXScan3d),
169  SYNC_INIT(rgbdXScanDesc),
170  SYNC_INIT(rgbdXInfo),
171 
172  // X RGBD + Odom
173  SYNC_INIT(rgbdXOdom),
174  SYNC_INIT(rgbdXOdomScan2d),
175  SYNC_INIT(rgbdXOdomScan3d),
176  SYNC_INIT(rgbdXOdomScanDesc),
177  SYNC_INIT(rgbdXOdomInfo),
178 
179 #ifdef RTABMAP_SYNC_USER_DATA
180  // X RGBD + User Data
181  SYNC_INIT(rgbdXData),
182  SYNC_INIT(rgbdXDataScan2d),
183  SYNC_INIT(rgbdXDataScan3d),
184  SYNC_INIT(rgbdXDataScanDesc),
185  SYNC_INIT(rgbdXDataInfo),
186 
187  // X RGBD + Odom + User Data
188  SYNC_INIT(rgbdXOdomData),
189  SYNC_INIT(rgbdXOdomDataScan2d),
190  SYNC_INIT(rgbdXOdomDataScan3d),
191  SYNC_INIT(rgbdXOdomDataScanDesc),
192  SYNC_INIT(rgbdXOdomDataInfo),
193 #endif
194 
195  // SensorData
196  SYNC_INIT(sensorDataInfo),
197  SYNC_INIT(sensorDataOdom),
198  SYNC_INIT(sensorDataOdomInfo),
199 
200 #ifdef RTABMAP_SYNC_MULTI_RGBD
201  // 2 RGBD
202  SYNC_INIT(rgbd2),
203  SYNC_INIT(rgbd2Scan2d),
204  SYNC_INIT(rgbd2Scan3d),
205  SYNC_INIT(rgbd2ScanDesc),
206  SYNC_INIT(rgbd2Info),
207 
208  // 2 RGBD + Odom
209  SYNC_INIT(rgbd2Odom),
210  SYNC_INIT(rgbd2OdomScan2d),
211  SYNC_INIT(rgbd2OdomScan3d),
212  SYNC_INIT(rgbd2OdomScanDesc),
213  SYNC_INIT(rgbd2OdomInfo),
214 
215 #ifdef RTABMAP_SYNC_USER_DATA
216  // 2 RGBD + User Data
217  SYNC_INIT(rgbd2Data),
218  SYNC_INIT(rgbd2DataScan2d),
219  SYNC_INIT(rgbd2DataScan3d),
220  SYNC_INIT(rgbd2DataScanDesc),
221  SYNC_INIT(rgbd2DataInfo),
222 
223  // 2 RGBD + Odom + User Data
224  SYNC_INIT(rgbd2OdomData),
225  SYNC_INIT(rgbd2OdomDataScan2d),
226  SYNC_INIT(rgbd2OdomDataScan3d),
227  SYNC_INIT(rgbd2OdomDataScanDesc),
228  SYNC_INIT(rgbd2OdomDataInfo),
229 #endif
230 
231  // 3 RGBD
232  SYNC_INIT(rgbd3),
233  SYNC_INIT(rgbd3Scan2d),
234  SYNC_INIT(rgbd3Scan3d),
235  SYNC_INIT(rgbd3ScanDesc),
236  SYNC_INIT(rgbd3Info),
237 
238  // 3 RGBD + Odom
239  SYNC_INIT(rgbd3Odom),
240  SYNC_INIT(rgbd3OdomScan2d),
241  SYNC_INIT(rgbd3OdomScan3d),
242  SYNC_INIT(rgbd3OdomScanDesc),
243  SYNC_INIT(rgbd3OdomInfo),
244 
245 #ifdef RTABMAP_SYNC_USER_DATA
246  // 3 RGBD + User Data
247  SYNC_INIT(rgbd3Data),
248  SYNC_INIT(rgbd3DataScan2d),
249  SYNC_INIT(rgbd3DataScan3d),
250  SYNC_INIT(rgbd3DataScanDesc),
251  SYNC_INIT(rgbd3DataInfo),
252 
253  // 3 RGBD + Odom + User Data
254  SYNC_INIT(rgbd3OdomData),
255  SYNC_INIT(rgbd3OdomDataScan2d),
256  SYNC_INIT(rgbd3OdomDataScan3d),
257  SYNC_INIT(rgbd3OdomDataScanDesc),
258  SYNC_INIT(rgbd3OdomDataInfo),
259 #endif
260 
261  // 4 RGBD
262  SYNC_INIT(rgbd4),
263  SYNC_INIT(rgbd4Scan2d),
264  SYNC_INIT(rgbd4Scan3d),
265  SYNC_INIT(rgbd4ScanDesc),
266  SYNC_INIT(rgbd4Info),
267 
268  // 4 RGBD + Odom
269  SYNC_INIT(rgbd4Odom),
270  SYNC_INIT(rgbd4OdomScan2d),
271  SYNC_INIT(rgbd4OdomScan3d),
272  SYNC_INIT(rgbd4OdomScanDesc),
273  SYNC_INIT(rgbd4OdomInfo),
274 
275 #ifdef RTABMAP_SYNC_USER_DATA
276  // 4 RGBD + User Data
277  SYNC_INIT(rgbd4Data),
278  SYNC_INIT(rgbd4DataScan2d),
279  SYNC_INIT(rgbd4DataScan3d),
280  SYNC_INIT(rgbd4DataScanDesc),
281  SYNC_INIT(rgbd4DataInfo),
282 
283  // 4 RGBD + Odom + User Data
284  SYNC_INIT(rgbd4OdomData),
285  SYNC_INIT(rgbd4OdomDataScan2d),
286  SYNC_INIT(rgbd4OdomDataScan3d),
287  SYNC_INIT(rgbd4OdomDataScanDesc),
288  SYNC_INIT(rgbd4OdomDataInfo),
289 #endif
290  // 5 RGBD
291  SYNC_INIT(rgbd5),
292  SYNC_INIT(rgbd5Scan2d),
293  SYNC_INIT(rgbd5Scan3d),
294  SYNC_INIT(rgbd5ScanDesc),
295  SYNC_INIT(rgbd5Info),
296 
297  // 5 RGBD + Odom
298  SYNC_INIT(rgbd5Odom),
299  SYNC_INIT(rgbd5OdomScan2d),
300  SYNC_INIT(rgbd5OdomScan3d),
301  SYNC_INIT(rgbd5OdomScanDesc),
302  SYNC_INIT(rgbd5OdomInfo),
303 
304  // 6 RGBD
305  SYNC_INIT(rgbd6),
306  SYNC_INIT(rgbd6Scan2d),
307  SYNC_INIT(rgbd6Scan3d),
308  SYNC_INIT(rgbd6ScanDesc),
309  SYNC_INIT(rgbd6Info),
310 
311  // 6 RGBD + Odom
312  SYNC_INIT(rgbd6Odom),
313  SYNC_INIT(rgbd6OdomScan2d),
314  SYNC_INIT(rgbd6OdomScan3d),
315  SYNC_INIT(rgbd6OdomScanDesc),
316  SYNC_INIT(rgbd6OdomInfo),
317 #endif // RTABMAP_SYNC_MULTI_RGBD
318 
319  // Scan
320  SYNC_INIT(scan2dInfo),
321  SYNC_INIT(scan3dInfo),
322  SYNC_INIT(scanDescInfo),
323 
324  // Scan + Odom
325  SYNC_INIT(odomScan2d),
326  SYNC_INIT(odomScan3d),
327  SYNC_INIT(odomScanDesc),
328  SYNC_INIT(odomScan2dInfo),
329  SYNC_INIT(odomScan3dInfo),
330  SYNC_INIT(odomScanDescInfo),
331 
332 #ifdef RTABMAP_SYNC_USER_DATA
333  // Scan + User Data
334  SYNC_INIT(dataScan2d),
335  SYNC_INIT(dataScan3d),
336  SYNC_INIT(dataScanDesc),
337  SYNC_INIT(dataScan2dInfo),
338  SYNC_INIT(dataScan3dInfo),
339  SYNC_INIT(dataScanDescInfo),
340 
341  // Scan + Odom + User Data
342  SYNC_INIT(odomDataScan2d),
343  SYNC_INIT(odomDataScan3d),
344  SYNC_INIT(odomDataScanDesc),
345  SYNC_INIT(odomDataScan2dInfo),
346  SYNC_INIT(odomDataScan3dInfo),
347  SYNC_INIT(odomDataScanDescInfo),
348 #endif
349 
350  // Odom
351  SYNC_INIT(odomInfo)
352 #ifdef RTABMAP_SYNC_USER_DATA
353  ,
354  // Odom + User Data
355  SYNC_INIT(odomData),
356  SYNC_INIT(odomDataInfo)
357 #endif
358 
359 {
360 }
361 
362 void CommonDataSubscriber::setupCallbacks(
363  ros::NodeHandle & nh,
364  ros::NodeHandle & pnh,
365  const std::string & name,
366  std::vector<diagnostic_updater::DiagnosticTask*> otherTasks)
367 {
368  bool subscribeScan2d = false;
369  bool subscribeScan3d = false;
370  bool subscribeScanDesc = false;
371  bool subscribeOdomInfo = false;
372  bool subscribeUserData = false;
373  bool subscribeOdom = true;
374  int rgbdCameras = 1;
375  name_ = name;
376 
377  // ROS related parameters (private)
378  pnh.param("subscribe_depth", subscribedToDepth_, subscribedToDepth_);
379  if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
380  {
381  ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
382  }
383  pnh.param("subscribe_rgb", subscribedToRGB_, subscribedToRGB_);
384  pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d);
385  pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
386  pnh.param("subscribe_scan_descriptor", subscribeScanDesc, subscribeScanDesc);
387  pnh.param("subscribe_stereo", subscribedToStereo_, subscribedToStereo_);
388  pnh.param("subscribe_rgbd", subscribedToRGBD_, subscribedToRGBD_);
389  pnh.param("subscribe_sensor_data", subscribedToSensorData_, subscribedToSensorData_);
390  pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
391  pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
392  pnh.param("subscribe_odom", subscribeOdom, subscribeOdom);
393 
394 #ifdef RTABMAP_SYNC_USER_DATA
395  if(subscribeUserData)
396  {
397  ROS_ERROR("subscribe_user_data is true, but rtabmap_ros has been built with RTABMAP_SYNC_USER_DATA. Setting back to false.");
398  subscribeUserData = false;
399  }
400 #endif
401 
402  if(subscribedToDepth_ && subscribedToStereo_)
403  {
404  ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
405  subscribedToDepth_ = false;
406  subscribedToRGB_ = false;
407  }
408  if(subscribedToRGB_ && subscribedToStereo_)
409  {
410  ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgb cannot be true at the same time. Parameter subscribe_rgb is set to false.");
411  subscribedToRGB_ = false;
412  }
413  if(subscribedToRGBD_)
414  {
415  if(subscribedToDepth_)
416  {
417  ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
418  subscribedToDepth_ = false;
419  subscribedToRGB_ = false;
420  }
421  if(subscribedToRGB_)
422  {
423  ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgb is set to false.");
424  subscribedToRGB_ = false;
425  }
426  if(subscribedToStereo_)
427  {
428  ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
429  subscribedToStereo_ = false;
430  }
431  }
432  if(subscribedToSensorData_)
433  {
434  if(!subscribedToRGBD_)
435  {
436  if(subscribedToDepth_)
437  {
438  ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_sensor_data cannot be true at the same time. Parameter subscribe_depth is set to false.");
439  subscribedToDepth_ = false;
440  subscribedToRGB_ = false;
441  }
442  if(subscribedToRGB_)
443  {
444  ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_sensor_data cannot be true at the same time. Parameter subscribe_rgb is set to false.");
445  subscribedToRGB_ = false;
446  }
447  if(subscribedToStereo_)
448  {
449  ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_sensor_data cannot be true at the same time. Parameter subscribe_stereo is set to false.");
450  subscribedToStereo_ = false;
451  }
452  }
453  else
454  {
455  ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgbd is set to false.");
456  subscribedToRGBD_ = false;
457  }
458  }
459  if(subscribeScan2d && subscribeScan3d)
460  {
461  ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
462  subscribeScan3d = false;
463  }
464  if(subscribeScan2d && subscribeScanDesc)
465  {
466  ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan is set to false.");
467  subscribeScan2d = false;
468  }
469  if(subscribeScan3d && subscribeScanDesc)
470  {
471  ROS_WARN("rtabmap: Parameters subscribe_scan_cloud and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
472  subscribeScan3d = false;
473  }
474  if(subscribedToSensorData_ && subscribeScan2d)
475  {
476  ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_scan cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
477  subscribeScan2d = false;
478  }
479  if(subscribedToSensorData_ && subscribeScan3d)
480  {
481  ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
482  subscribeScan3d = false;
483  }
484  if(subscribedToSensorData_ && subscribeScanDesc)
485  {
486  ROS_WARN("rtabmap: Parameters subscribe_sensor_data and subscribe_scan_descriptor cannot be true at the same time. Parameter subscribe_scan_descriptor is set to false.");
487  subscribeScanDesc = false;
488  }
489  if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
490  {
491  if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
492  {
493  approxSync_ = false; // default for scan: exact sync
494  }
495  }
496  if(subscribedToStereo_)
497  {
498  approxSync_ = false; // default for stereo: exact sync
499  }
500 
501  std::string odomFrameId;
502  pnh.getParam("odom_frame_id", odomFrameId);
503  pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
504  if(pnh.hasParam("depth_cameras"))
505  {
506  ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true.");
507  }
508  pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_);
509  if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
510  {
511  pnh.param("queue_size", syncQueueSize_, syncQueueSize_);
512  ROS_WARN("Parameter \"queue_size\" has been renamed "
513  "to \"sync_queue_size\" and will be removed "
514  "in future versions! The value (%d) is copied to "
515  "\"sync_queue_size\".", syncQueueSize_);
516  }
517  else
518  {
519  pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_);
520  }
521  if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
522  {
523  ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
524  "to \"approx_sync\"! Your value is copied to "
525  "corresponding parameter.");
526  pnh.param("stereo_approx_sync", approxSync_, approxSync_);
527  }
528  else
529  {
530  pnh.param("approx_sync", approxSync_, approxSync_);
531  }
532 
533  ROS_INFO("%s: subscribe_depth = %s", name.c_str(), subscribedToDepth_?"true":"false");
534  ROS_INFO("%s: subscribe_rgb = %s", name.c_str(), subscribedToRGB_?"true":"false");
535  ROS_INFO("%s: subscribe_stereo = %s", name.c_str(), subscribedToStereo_?"true":"false");
536  ROS_INFO("%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras);
537  ROS_INFO("%s: subscribe_sensor_data = %s", name.c_str(), subscribedToSensorData_?"true":"false");
538  ROS_INFO("%s: subscribe_odom_info = %s", name.c_str(), subscribeOdomInfo?"true":"false");
539  ROS_INFO("%s: subscribe_user_data = %s", name.c_str(), subscribeUserData?"true":"false");
540  ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false");
541  ROS_INFO("%s: subscribe_scan_cloud = %s", name.c_str(), subscribeScan3d?"true":"false");
542  ROS_INFO("%s: subscribe_scan_descriptor = %s", name.c_str(), subscribeScanDesc?"true":"false");
543  ROS_INFO("%s: topic_queue_size = %d", name.c_str(), topicQueueSize_);
544  ROS_INFO("%s: sync_queue_size = %d", name.c_str(), syncQueueSize_);
545  ROS_INFO("%s: approx_sync = %s", name.c_str(), approxSync_?"true":"false");
546 
547  subscribedToOdom_ = odomFrameId.empty() && subscribeOdom;
548  if(subscribedToDepth_)
549  {
550  setupDepthCallbacks(
551  nh,
552  pnh,
553  subscribedToOdom_,
554  subscribeUserData,
555  subscribeScan2d,
556  subscribeScan3d,
557  subscribeScanDesc,
558  subscribeOdomInfo);
559  }
560  else if(subscribedToStereo_)
561  {
562  setupStereoCallbacks(
563  nh,
564  pnh,
565  subscribedToOdom_,
566  subscribeOdomInfo);
567  }
568  else if(subscribedToRGB_)
569  {
570  setupRGBCallbacks(
571  nh,
572  pnh,
573  subscribedToOdom_,
574  subscribeUserData,
575  subscribeScan2d,
576  subscribeScan3d,
577  subscribeScanDesc,
578  subscribeOdomInfo);
579  }
580  else if(subscribedToRGBD_)
581  {
582 #ifdef RTABMAP_SYNC_MULTI_RGBD
583  if(rgbdCameras >= 6)
584  {
585  if(rgbdCameras > 6)
586  {
587  ROS_ERROR("Cannot synchronize more than 6 rgbd topics (rgbd_cameras is set to %d). Set "
588  "rgbd_cameras=0 to use RGBDImages interface instead, then "
589  "synchronize RGBDImage topics yourself.", rgbdCameras);
590  }
591 
592  setupRGBD6Callbacks(
593  nh,
594  pnh,
595  subscribedToOdom_,
596  subscribeUserData,
597  subscribeScan2d,
598  subscribeScan3d,
599  subscribeScanDesc,
600  subscribeOdomInfo);
601  }
602  else if(rgbdCameras == 5)
603  {
604  setupRGBD5Callbacks(
605  nh,
606  pnh,
607  subscribedToOdom_,
608  subscribeUserData,
609  subscribeScan2d,
610  subscribeScan3d,
611  subscribeScanDesc,
612  subscribeOdomInfo);
613  }
614  else if(rgbdCameras == 4)
615  {
616  setupRGBD4Callbacks(
617  nh,
618  pnh,
619  subscribedToOdom_,
620  subscribeUserData,
621  subscribeScan2d,
622  subscribeScan3d,
623  subscribeScanDesc,
624  subscribeOdomInfo);
625  }
626  else if(rgbdCameras == 3)
627  {
628  setupRGBD3Callbacks(
629  nh,
630  pnh,
631  subscribedToOdom_,
632  subscribeUserData,
633  subscribeScan2d,
634  subscribeScan3d,
635  subscribeScanDesc,
636  subscribeOdomInfo);
637  }
638  else if(rgbdCameras == 2)
639  {
640  setupRGBD2Callbacks(
641  nh,
642  pnh,
643  subscribedToOdom_,
644  subscribeUserData,
645  subscribeScan2d,
646  subscribeScan3d,
647  subscribeScanDesc,
648  subscribeOdomInfo);
649  }
650 #else
651  if(rgbdCameras>1)
652  {
653  ROS_FATAL("Cannot synchronize more than 1 rgbd topic (rtabmap_ros has "
654  "been built without RTABMAP_SYNC_MULTI_RGBD option). Set rgbd_cameras=0 to "
655  "use RGBDImages interface instead without recompiling with RTABMAP_SYNC_MULTI_RGBD, "
656  "but you will have to synchronize RGBDImage topics yourself.");
657  }
658 #endif
659  else if(rgbdCameras == 0)
660  {
661  setupRGBDXCallbacks(
662  nh,
663  pnh,
664  subscribedToOdom_,
665  subscribeUserData,
666  subscribeScan2d,
667  subscribeScan3d,
668  subscribeScanDesc,
669  subscribeOdomInfo);
670  }
671  else
672  {
673  setupRGBDCallbacks(
674  nh,
675  pnh,
676  subscribedToOdom_,
677  subscribeUserData,
678  subscribeScan2d,
679  subscribeScan3d,
680  subscribeScanDesc,
681  subscribeOdomInfo);
682  }
683  }
684  else if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
685  {
686  setupScanCallbacks(
687  nh,
688  pnh,
689  subscribeScan2d,
690  subscribeScanDesc,
691  subscribedToOdom_,
692  subscribeUserData,
693  subscribeOdomInfo);
694  }
695  else if(subscribedToSensorData_)
696  {
697  setupSensorDataCallbacks(
698  nh,
699  pnh,
700  subscribedToOdom_,
701  subscribeOdomInfo);
702  }
703  else if(subscribedToOdom_)
704  {
705  setupOdomCallbacks(
706  nh,
707  pnh,
708  subscribeUserData,
709  subscribeOdomInfo);
710  }
711 
712  if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
713  {
714  ROS_INFO("%s", subscribedTopicsMsg_.c_str());
715  syncDiagnostic_.reset(new SyncDiagnostic(nh, pnh, name, 0.5));
716  syncDiagnostic_->init("",
717  uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
718  "published (\"$ rostopic hz my_topic\") and the timestamps in their "
719  "header are set. If topics are coming from different computers, make sure "
720  "the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
721  name_.c_str(),
722  approxSync_?
723  uFormat("If topics are not published at the same rate, you could increase \"sync_queue_size\" and/or \"topic_queue_size\" parameters (current=%d and %d respectively).", syncQueueSize_, topicQueueSize_).c_str():
724  "Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.",
725  subscribedTopicsMsg_.c_str()),
726  otherTasks);
727 
728  }
729 }
730 
731 CommonDataSubscriber::~CommonDataSubscriber()
732 {
733  // RGB + Depth
734  SYNC_DEL(depth);
735  SYNC_DEL(depthScan2d);
736  SYNC_DEL(depthScan3d);
737  SYNC_DEL(depthScanDesc);
738  SYNC_DEL(depthInfo);
739  SYNC_DEL(depthScan2dInfo);
740  SYNC_DEL(depthScan3dInfo);
741  SYNC_DEL(depthScanDescInfo);
742 
743  // RGB + Depth + Odom
744  SYNC_DEL(depthOdom);
745  SYNC_DEL(depthOdomScan2d);
746  SYNC_DEL(depthOdomScan3d);
747  SYNC_DEL(depthOdomScanDesc);
748  SYNC_DEL(depthOdomInfo);
749  SYNC_DEL(depthOdomScan2dInfo);
750  SYNC_DEL(depthOdomScan3dInfo);
751  SYNC_DEL(depthOdomScanDescInfo);
752 
753 #ifdef RTABMAP_SYNC_USER_DATA
754  // RGB + Depth + User Data
755  SYNC_DEL(depthData);
756  SYNC_DEL(depthDataScan2d);
757  SYNC_DEL(depthDataScan3d);
758  SYNC_DEL(depthDataScanDesc);
759  SYNC_DEL(depthDataInfo);
760  SYNC_DEL(depthDataScan2dInfo);
761  SYNC_DEL(depthDataScan3dInfo);
762  SYNC_DEL(depthDataScanDescInfo);
763 
764  // RGB + Depth + Odom + User Data
765  SYNC_DEL(depthOdomData);
766  SYNC_DEL(depthOdomDataScan2d);
767  SYNC_DEL(depthOdomDataScan3d);
768  SYNC_DEL(depthOdomDataScanDesc);
769  SYNC_DEL(depthOdomDataInfo);
770  SYNC_DEL(depthOdomDataScan2dInfo);
771  SYNC_DEL(depthOdomDataScan3dInfo);
772  SYNC_DEL(depthOdomDataScanDescInfo);
773 #endif
774 
775  // Stereo
776  SYNC_DEL(stereo);
777  SYNC_DEL(stereoInfo);
778 
779  // Stereo + Odom
780  SYNC_DEL(stereoOdom);
781  SYNC_DEL(stereoOdomInfo);
782 
783  // RGB-only
784  SYNC_DEL(rgb);
785  SYNC_DEL(rgbScan2d);
786  SYNC_DEL(rgbScan3d);
787  SYNC_DEL(rgbScanDesc);
788  SYNC_DEL(rgbInfo);
789  SYNC_DEL(rgbScan2dInfo);
790  SYNC_DEL(rgbScan3dInfo);
791  SYNC_DEL(rgbScanDescInfo);
792 
793  // RGB-only + Odom
794  SYNC_DEL(rgbOdom);
795  SYNC_DEL(rgbOdomScan2d);
796  SYNC_DEL(rgbOdomScan3d);
797  SYNC_DEL(rgbOdomScanDesc);
798  SYNC_DEL(rgbOdomInfo);
799  SYNC_DEL(rgbOdomScan2dInfo);
800  SYNC_DEL(rgbOdomScan3dInfo);
801  SYNC_DEL(rgbOdomScanDescInfo);
802 
803 #ifdef RTABMAP_SYNC_USER_DATA
804  // RGB-only + User Data
805  SYNC_DEL(rgbData);
806  SYNC_DEL(rgbDataScan2d);
807  SYNC_DEL(rgbDataScan3d);
808  SYNC_DEL(rgbDataScanDesc);
809  SYNC_DEL(rgbDataInfo);
810  SYNC_DEL(rgbDataScan2dInfo);
811  SYNC_DEL(rgbDataScan3dInfo);
812  SYNC_DEL(rgbDataScanDescInfo);
813 
814  // RGB-only + Odom + User Data
815  SYNC_DEL(rgbOdomData);
816  SYNC_DEL(rgbOdomDataScan2d);
817  SYNC_DEL(rgbOdomDataScan3d);
818  SYNC_DEL(rgbOdomDataScanDesc);
819  SYNC_DEL(rgbOdomDataInfo);
820  SYNC_DEL(rgbOdomDataScan2dInfo);
821  SYNC_DEL(rgbOdomDataScan3dInfo);
822  SYNC_DEL(rgbOdomDataScanDescInfo);
823 #endif
824 
825  // 1 RGBD
826  SYNC_DEL(rgbdScan2d);
827  SYNC_DEL(rgbdScan3d);
828  SYNC_DEL(rgbdScanDesc);
829  SYNC_DEL(rgbdInfo);
830 
831  // 1 RGBD + Odom
832  SYNC_DEL(rgbdOdom);
833  SYNC_DEL(rgbdOdomScan2d);
834  SYNC_DEL(rgbdOdomScan3d);
835  SYNC_DEL(rgbdOdomScanDesc);
836  SYNC_DEL(rgbdOdomInfo);
837 
838 #ifdef RTABMAP_SYNC_USER_DATA
839  // 1 RGBD + User Data
840  SYNC_DEL(rgbdData);
841  SYNC_DEL(rgbdDataScan2d);
842  SYNC_DEL(rgbdDataScan3d);
843  SYNC_DEL(rgbdDataScanDesc);
844  SYNC_DEL(rgbdDataInfo);
845 
846  // 1 RGBD + Odom + User Data
847  SYNC_DEL(rgbdOdomData);
848  SYNC_DEL(rgbdOdomDataScan2d);
849  SYNC_DEL(rgbdOdomDataScan3d);
850  SYNC_DEL(rgbdOdomDataScanDesc);
851  SYNC_DEL(rgbdOdomDataInfo);
852 #endif
853 
854  // X RGBD
855  SYNC_DEL(rgbdXScan2d);
856  SYNC_DEL(rgbdXScan3d);
857  SYNC_DEL(rgbdXScanDesc);
858  SYNC_DEL(rgbdXInfo);
859 
860  // X RGBD + Odom
861  SYNC_DEL(rgbdXOdom);
862  SYNC_DEL(rgbdXOdomScan2d);
863  SYNC_DEL(rgbdXOdomScan3d);
864  SYNC_DEL(rgbdXOdomScanDesc);
865  SYNC_DEL(rgbdXOdomInfo);
866 
867 #ifdef RTABMAP_SYNC_USER_DATA
868  // X RGBD + User Data
869  SYNC_DEL(rgbdXData);
870  SYNC_DEL(rgbdXDataScan2d);
871  SYNC_DEL(rgbdXDataScan3d);
872  SYNC_DEL(rgbdXDataScanDesc);
873  SYNC_DEL(rgbdXDataInfo);
874 
875  // X RGBD + Odom + User Data
876  SYNC_DEL(rgbdXOdomData);
877  SYNC_DEL(rgbdXOdomDataScan2d);
878  SYNC_DEL(rgbdXOdomDataScan3d);
879  SYNC_DEL(rgbdXOdomDataScanDesc);
880  SYNC_DEL(rgbdXOdomDataInfo);
881 #endif
882 
883 #ifdef RTABMAP_SYNC_MULTI_RGBD
884  // 2 RGBD
885  SYNC_DEL(rgbd2);
886  SYNC_DEL(rgbd2Scan2d);
887  SYNC_DEL(rgbd2Scan3d);
888  SYNC_DEL(rgbd2ScanDesc);
889  SYNC_DEL(rgbd2Info);
890 
891  // 2 RGBD + Odom
892  SYNC_DEL(rgbd2Odom);
893  SYNC_DEL(rgbd2OdomScan2d);
894  SYNC_DEL(rgbd2OdomScan3d);
895  SYNC_DEL(rgbd2OdomScanDesc);
896  SYNC_DEL(rgbd2OdomInfo);
897 
898 #ifdef RTABMAP_SYNC_USER_DATA
899  // 2 RGBD + User Data
900  SYNC_DEL(rgbd2Data);
901  SYNC_DEL(rgbd2DataScan2d);
902  SYNC_DEL(rgbd2DataScan3d);
903  SYNC_DEL(rgbd2DataScanDesc);
904  SYNC_DEL(rgbd2DataInfo);
905 
906  // 2 RGBD + Odom + User Data
907  SYNC_DEL(rgbd2OdomData);
908  SYNC_DEL(rgbd2OdomDataScan2d);
909  SYNC_DEL(rgbd2OdomDataScan3d);
910  SYNC_DEL(rgbd2OdomDataScanDesc);
911  SYNC_DEL(rgbd2OdomDataInfo);
912 #endif
913 
914  // 3 RGBD
915  SYNC_DEL(rgbd3);
916  SYNC_DEL(rgbd3Scan2d);
917  SYNC_DEL(rgbd3Scan3d);
918  SYNC_DEL(rgbd3ScanDesc);
919  SYNC_DEL(rgbd3Info);
920 
921  // 3 RGBD + Odom
922  SYNC_DEL(rgbd3Odom);
923  SYNC_DEL(rgbd3OdomScan2d);
924  SYNC_DEL(rgbd3OdomScan3d);
925  SYNC_DEL(rgbd3OdomScanDesc);
926  SYNC_DEL(rgbd3OdomInfo);
927 
928 #ifdef RTABMAP_SYNC_USER_DATA
929  // 3 RGBD + User Data
930  SYNC_DEL(rgbd3Data);
931  SYNC_DEL(rgbd3DataScan2d);
932  SYNC_DEL(rgbd3DataScan3d);
933  SYNC_DEL(rgbd3DataScanDesc);
934  SYNC_DEL(rgbd3DataInfo);
935 
936  // 3 RGBD + Odom + User Data
937  SYNC_DEL(rgbd3OdomData);
938  SYNC_DEL(rgbd3OdomDataScan2d);
939  SYNC_DEL(rgbd3OdomDataScan3d);
940  SYNC_DEL(rgbd3OdomDataScanDesc);
941  SYNC_DEL(rgbd3OdomDataInfo);
942 #endif
943 
944  // 4 RGBD
945  SYNC_DEL(rgbd4);
946  SYNC_DEL(rgbd4Scan2d);
947  SYNC_DEL(rgbd4Scan3d);
948  SYNC_DEL(rgbd4ScanDesc);
949  SYNC_DEL(rgbd4Info);
950 
951  // 4 RGBD + Odom
952  SYNC_DEL(rgbd4Odom);
953  SYNC_DEL(rgbd4OdomScan2d);
954  SYNC_DEL(rgbd4OdomScan3d);
955  SYNC_DEL(rgbd4OdomScanDesc);
956  SYNC_DEL(rgbd4OdomInfo);
957 
958 #ifdef RTABMAP_SYNC_USER_DATA
959  // 4 RGBD + User Data
960  SYNC_DEL(rgbd4Data);
961  SYNC_DEL(rgbd4DataScan2d);
962  SYNC_DEL(rgbd4DataScan3d);
963  SYNC_DEL(rgbd4DataScanDesc);
964  SYNC_DEL(rgbd4DataInfo);
965 
966  // 4 RGBD + Odom + User Data
967  SYNC_DEL(rgbd4OdomData);
968  SYNC_DEL(rgbd4OdomDataScan2d);
969  SYNC_DEL(rgbd4OdomDataScan3d);
970  SYNC_DEL(rgbd4OdomDataScanDesc);
971  SYNC_DEL(rgbd4OdomDataInfo);
972 #endif
973  // 5 RGBD
974  SYNC_DEL(rgbd5);
975  SYNC_DEL(rgbd5Scan2d);
976  SYNC_DEL(rgbd5Scan3d);
977  SYNC_DEL(rgbd5ScanDesc);
978  SYNC_DEL(rgbd5Info);
979 
980  // 5 RGBD + Odom
981  SYNC_DEL(rgbd5Odom);
982  SYNC_DEL(rgbd5OdomScan2d);
983  SYNC_DEL(rgbd5OdomScan3d);
984  SYNC_DEL(rgbd5OdomScanDesc);
985  SYNC_DEL(rgbd5OdomInfo);
986 
987  // 6 RGBD
988  SYNC_DEL(rgbd6);
989  SYNC_DEL(rgbd6Scan2d);
990  SYNC_DEL(rgbd6Scan3d);
991  SYNC_DEL(rgbd6ScanDesc);
992  SYNC_DEL(rgbd6Info);
993 
994  // 6 RGBD + Odom
995  SYNC_DEL(rgbd6Odom);
996  SYNC_DEL(rgbd6OdomScan2d);
997  SYNC_DEL(rgbd6OdomScan3d);
998  SYNC_DEL(rgbd6OdomScanDesc);
999  SYNC_DEL(rgbd6OdomInfo);
1000 #endif //RTABMAP_SYNC_MULTI_RGBD
1001 
1002  // Scan
1003  SYNC_DEL(scan2dInfo);
1004  SYNC_DEL(scan3dInfo);
1005 
1006  // Scan + Odom
1007  SYNC_DEL(odomScan2d);
1008  SYNC_DEL(odomScan3d);
1009  SYNC_DEL(odomScanDesc);
1010  SYNC_DEL(odomScan2dInfo);
1011  SYNC_DEL(odomScan3dInfo);
1012  SYNC_DEL(odomScanDescInfo);
1013 
1014 #ifdef RTABMAP_SYNC_USER_DATA
1015  // Scan + User Data
1016  SYNC_DEL(dataScan2d);
1017  SYNC_DEL(dataScan3d);
1018  SYNC_DEL(dataScanDesc);
1019  SYNC_DEL(dataScan2dInfo);
1020  SYNC_DEL(dataScan3dInfo);
1021  SYNC_DEL(dataScanDescInfo);
1022 
1023  // Scan + Odom + User Data
1024  SYNC_DEL(odomDataScan2d);
1025  SYNC_DEL(odomDataScan3d);
1026  SYNC_DEL(odomDataScanDesc);
1027  SYNC_DEL(odomDataScan2dInfo);
1028  SYNC_DEL(odomDataScan3dInfo);
1029  SYNC_DEL(odomDataScanDescInfo);
1030 #endif
1031 
1032  // Odom
1033  SYNC_DEL(odomInfo);
1034 #ifdef RTABMAP_SYNC_USER_DATA
1035  // Odom + User Data
1036  SYNC_DEL(odomData);
1037  SYNC_DEL(odomDataInfo);
1038 #endif
1039 
1040 
1041  for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
1042  {
1043  delete rgbdSubs_[i];
1044  }
1045  rgbdSubs_.clear();
1046 }
1047 
1048 void CommonDataSubscriber::commonSingleCameraCallback(
1049  const nav_msgs::OdometryConstPtr & odomMsg,
1050  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
1051  const cv_bridge::CvImageConstPtr & imageMsg,
1052  const cv_bridge::CvImageConstPtr & depthMsg,
1053  const sensor_msgs::CameraInfo & rgbCameraInfoMsg,
1054  const sensor_msgs::CameraInfo & depthCameraInfoMsg,
1055  const sensor_msgs::LaserScan& scanMsg,
1056  const sensor_msgs::PointCloud2& scan3dMsg,
1057  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg,
1058  const std::vector<rtabmap_msgs::GlobalDescriptor> & globalDescriptorMsgs,
1059  const std::vector<rtabmap_msgs::KeyPoint> & localKeyPoints,
1060  const std::vector<rtabmap_msgs::Point3f> & localPoints3d,
1061  const cv::Mat & localDescriptors)
1062 {
1063  std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPointsMsgs;
1064  localKeyPointsMsgs.push_back(localKeyPoints);
1065  std::vector<std::vector<rtabmap_msgs::Point3f> > localPoints3dMsgs;
1066  localPoints3dMsgs.push_back(localPoints3d);
1067  std::vector<cv::Mat> localDescriptorsMsgs;
1068  localDescriptorsMsgs.push_back(localDescriptors);
1069 
1070  std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
1071  std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
1072  std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
1073  std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs;
1074  if(imageMsg.get())
1075  {
1076  imageMsgs.push_back(imageMsg);
1077  }
1078  if(depthMsg.get())
1079  {
1080  depthMsgs.push_back(depthMsg);
1081  }
1082  cameraInfoMsgs.push_back(rgbCameraInfoMsg);
1083  depthCameraInfoMsgs.push_back(depthCameraInfoMsg);
1084  commonMultiCameraCallback(
1085  odomMsg,
1086  userDataMsg,
1087  imageMsgs,
1088  depthMsgs,
1089  cameraInfoMsgs,
1090  depthCameraInfoMsgs,
1091  scanMsg,
1092  scan3dMsg,
1093  odomInfoMsg,
1094  globalDescriptorMsgs,
1095  localKeyPointsMsgs,
1096  localPoints3dMsgs,
1097  localDescriptorsMsgs);
1098 }
1099 
1100 void CommonDataSubscriber::tick(const ros::Time & stamp, double targetFrequency)
1101 {
1102  if(syncDiagnostic_.get())
1103  {
1104  syncDiagnostic_->tick(stamp, targetFrequency);
1105  }
1106 }
1107 
1108 } /* namespace rtabmap_sync */
rtabmap_sync::CommonDataSubscriber::name_
std::string name_
Definition: CommonDataSubscriber.h:260
name
rtabmap_sync::CommonDataSubscriber::setupSensorDataCallbacks
void setupSensorDataCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeOdomInfo)
Definition: CommonDataSubscriberSensorData.cpp:67
SYNC_DEL
#define SYNC_DEL(PREFIX)
Definition: CommonDataSubscriberDefines.h:103
rtabmap_sync::CommonDataSubscriber::setupOdomCallbacks
void setupOdomCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeUserData, bool subscribeOdomInfo)
Definition: CommonDataSubscriberOdom.cpp:66
rtabmap_sync::CommonDataSubscriber::rgbdCameras
int rgbdCameras() const
Definition: CommonDataSubscriber.h:76
boost::shared_ptr
rtabmap_sync::CommonDataSubscriber::setupRGBCallbacks
void setupRGBCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, bool subscribeOdomInfo)
Definition: CommonDataSubscriberRGB.cpp:458
rtabmap_sync::CommonDataSubscriber::~CommonDataSubscriber
virtual ~CommonDataSubscriber()
Definition: CommonDataSubscriber.cpp:731
rtabmap_sync
Definition: CommonDataSubscriber.h:59
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
uFormat
std::string uFormat(const char *fmt,...)
rtabmap_sync::SyncDiagnostic
Definition: SyncDiagnostic.h:13
rtabmap_sync::CommonDataSubscriber::setupStereoCallbacks
void setupStereoCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeOdomInfo)
Definition: CommonDataSubscriberStereo.cpp:88
rtabmap_sync::CommonDataSubscriber::setupCallbacks
void setupCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, const std::string &name, std::vector< diagnostic_updater::DiagnosticTask * > otherTasks=std::vector< diagnostic_updater::DiagnosticTask * >())
Definition: CommonDataSubscriber.cpp:362
rtabmap_sync::CommonDataSubscriber::name
const std::string & name() const
Definition: CommonDataSubscriber.h:80
true
#define true
rtabmap_sync::CommonDataSubscriber::commonSingleCameraCallback
void commonSingleCameraCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_msgs::UserDataConstPtr &userDataMsg, const cv_bridge::CvImageConstPtr &imageMsg, const cv_bridge::CvImageConstPtr &depthMsg, const sensor_msgs::CameraInfo &rgbCameraInfoMsg, const sensor_msgs::CameraInfo &depthCameraInfoMsg, const sensor_msgs::LaserScan &scanMsg, const sensor_msgs::PointCloud2 &scan3dMsg, const rtabmap_msgs::OdomInfoConstPtr &odomInfoMsg, const std::vector< rtabmap_msgs::GlobalDescriptor > &globalDescriptorMsgs=std::vector< rtabmap_msgs::GlobalDescriptor >(), const std::vector< rtabmap_msgs::KeyPoint > &localKeyPoints=std::vector< rtabmap_msgs::KeyPoint >(), const std::vector< rtabmap_msgs::Point3f > &localPoints3d=std::vector< rtabmap_msgs::Point3f >(), const cv::Mat &localDescriptors=cv::Mat())
Definition: CommonDataSubscriber.cpp:1048
rtabmap_sync::CommonDataSubscriber::subscribedTopicsMsg_
std::string subscribedTopicsMsg_
Definition: CommonDataSubscriber.h:244
CommonDataSubscriber.h
rtabmap_sync::CommonDataSubscriber::syncQueueSize_
int syncQueueSize_
Definition: CommonDataSubscriber.h:246
rtabmap_sync::CommonDataSubscriber::subscribedToOdom_
bool subscribedToOdom_
Definition: CommonDataSubscriber.h:253
rtabmap_sync::CommonDataSubscriber::topicQueueSize_
int topicQueueSize_
Definition: CommonDataSubscriber.h:245
rtabmap_sync::CommonDataSubscriber::setupDepthCallbacks
void setupDepthCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, bool subscribeOdomInfo)
Definition: CommonDataSubscriberDepth.cpp:458
rtabmap_sync::CommonDataSubscriber::CommonDataSubscriber
CommonDataSubscriber(bool gui)
Definition: CommonDataSubscriber.cpp:32
rtabmap_sync::CommonDataSubscriber::subscribedToDepth_
bool subscribedToDepth_
Definition: CommonDataSubscriber.h:250
rtabmap_sync::CommonDataSubscriber::subscribedToScan3d_
bool subscribedToScan3d_
Definition: CommonDataSubscriber.h:257
rtabmap_sync::CommonDataSubscriber::subscribedToRGB_
bool subscribedToRGB_
Definition: CommonDataSubscriber.h:252
rtabmap_sync::CommonDataSubscriber::subscribedToScanDescriptor_
bool subscribedToScanDescriptor_
Definition: CommonDataSubscriber.h:258
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
rtabmap_sync::CommonDataSubscriber::setupRGBDCallbacks
void setupRGBDCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, bool subscribeOdomInfo)
Definition: CommonDataSubscriberRGBD.cpp:534
ROS_WARN
#define ROS_WARN(...)
rtabmap_sync::CommonDataSubscriber::setupRGBDXCallbacks
void setupRGBDXCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, bool subscribeOdomInfo)
Definition: CommonDataSubscriberRGBDX.cpp:321
rtabmap_sync::CommonDataSubscriber::setupScanCallbacks
void setupScanCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeScan2d, bool subscribeScanDesc, bool subscribeOdom, bool subscribeUserData, bool subscribeOdomInfo)
Definition: CommonDataSubscriberScan.cpp:246
ROS_FATAL
#define ROS_FATAL(...)
rtabmap_sync::CommonDataSubscriber::tick
void tick(const ros::Time &stamp, double targetFrequency=0)
Definition: CommonDataSubscriber.cpp:1100
rtabmap_sync::CommonDataSubscriber::approxSync_
bool approxSync_
Definition: CommonDataSubscriber.h:249
rtabmap_sync::CommonDataSubscriber::rgbdSubs_
std::vector< message_filters::Subscriber< rtabmap_msgs::RGBDImage > * > rgbdSubs_
Definition: CommonDataSubscriber.h:269
rtabmap_sync::CommonDataSubscriber::subscribedToSensorData_
bool subscribedToSensorData_
Definition: CommonDataSubscriber.h:255
rtabmap_sync::CommonDataSubscriber::subscribedToRGBD_
bool subscribedToRGBD_
Definition: CommonDataSubscriber.h:254
rtabmap_sync::CommonDataSubscriber::syncDiagnostic_
std::unique_ptr< SyncDiagnostic > syncDiagnostic_
Definition: CommonDataSubscriber.h:295
ros::Time
c_str
const char * c_str(Args &&...args)
ROS_ERROR
#define ROS_ERROR(...)
rtabmap_sync::CommonDataSubscriber::subscribedToStereo_
bool subscribedToStereo_
Definition: CommonDataSubscriber.h:251
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
false
#define false
rtabmap_sync::CommonDataSubscriber::subscribedToScan2d_
bool subscribedToScan2d_
Definition: CommonDataSubscriber.h:256
ROS_INFO
#define ROS_INFO(...)
SYNC_INIT
#define SYNC_INIT(PREFIX)
Definition: CommonDataSubscriberDefines.h:98
rtabmap_sync::CommonDataSubscriber::commonMultiCameraCallback
virtual void commonMultiCameraCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_msgs::UserDataConstPtr &userDataMsg, const std::vector< cv_bridge::CvImageConstPtr > &imageMsgs, const std::vector< cv_bridge::CvImageConstPtr > &depthMsgs, const std::vector< sensor_msgs::CameraInfo > &cameraInfoMsgs, const std::vector< sensor_msgs::CameraInfo > &depthCameraInfoMsgs, const sensor_msgs::LaserScan &scanMsg, const sensor_msgs::PointCloud2 &scan3dMsg, const rtabmap_msgs::OdomInfoConstPtr &odomInfoMsg, const std::vector< rtabmap_msgs::GlobalDescriptor > &globalDescriptorMsgs=std::vector< rtabmap_msgs::GlobalDescriptor >(), const std::vector< std::vector< rtabmap_msgs::KeyPoint > > &localKeyPoints=std::vector< std::vector< rtabmap_msgs::KeyPoint > >(), const std::vector< std::vector< rtabmap_msgs::Point3f > > &localPoints3d=std::vector< std::vector< rtabmap_msgs::Point3f > >(), const std::vector< cv::Mat > &localDescriptors=std::vector< cv::Mat >())=0
i
int i
ros::NodeHandle

rtabmap_sync
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:39:12