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