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

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40