CommonDataSubscriberRGBD2.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 #include <rtabmap/utilite/UConversion.h>
30 #include <rtabmap_ros/MsgConversion.h>
31 #include <cv_bridge/cv_bridge.h>
32 
33 namespace rtabmap_ros {
34 
35 #define IMAGE_CONVERSION() \
36  callbackCalled(); \
37  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2); \
38  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
39  rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
40  rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
41  std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
42  cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
43  cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo);
44 
45 // 2 RGBD
46 void CommonDataSubscriber::rgbd2Callback(
47  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
48  const rtabmap_ros::RGBDImageConstPtr& image2Msg)
49 {
50  IMAGE_CONVERSION();
51 
52  nav_msgs::OdometryConstPtr odomMsg; // Null
53  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
54  sensor_msgs::LaserScanConstPtr scanMsg; // Null
55  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
56  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
57  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
58 }
59 void CommonDataSubscriber::rgbd2Scan2dCallback(
60  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
61  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
62  const sensor_msgs::LaserScanConstPtr& scanMsg)
63 {
64  IMAGE_CONVERSION();
65 
66  nav_msgs::OdometryConstPtr odomMsg; // Null
67  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
68  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
69  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
70  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
71 }
72 void CommonDataSubscriber::rgbd2Scan3dCallback(
73  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
74  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
75  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
76 {
77  IMAGE_CONVERSION();
78 
79  nav_msgs::OdometryConstPtr odomMsg; // Null
80  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
81  sensor_msgs::LaserScanConstPtr scanMsg; // Null
82  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
83  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
84 }
85 void CommonDataSubscriber::rgbd2InfoCallback(
86  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
87  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
88  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
89 {
90  IMAGE_CONVERSION();
91 
92  nav_msgs::OdometryConstPtr odomMsg; // Null
93  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
94  sensor_msgs::LaserScanConstPtr scanMsg; // Null
95  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
96  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
97 }
98 void CommonDataSubscriber::rgbd2Scan2dInfoCallback(
99  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
100  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
101  const sensor_msgs::LaserScanConstPtr& scanMsg,
102  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
103 {
104  IMAGE_CONVERSION();
105 
106  nav_msgs::OdometryConstPtr odomMsg; // Null
107  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
108  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
109  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
110 }
111 void CommonDataSubscriber::rgbd2Scan3dInfoCallback(
112  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
113  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
114  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
115  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
116 {
117  IMAGE_CONVERSION();
118 
119  nav_msgs::OdometryConstPtr odomMsg; // Null
120  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
121  sensor_msgs::LaserScanConstPtr scanMsg; // Null
122  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
123 }
124 
125 // 2 RGBD + Odom
126 void CommonDataSubscriber::rgbd2OdomCallback(
127  const nav_msgs::OdometryConstPtr & odomMsg,
128  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
129  const rtabmap_ros::RGBDImageConstPtr& image2Msg)
130 {
131  IMAGE_CONVERSION();
132 
133  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
134  sensor_msgs::LaserScanConstPtr scanMsg; // Null
135  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
136  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
137  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
138 }
139 void CommonDataSubscriber::rgbd2OdomScan2dCallback(
140  const nav_msgs::OdometryConstPtr & odomMsg,
141  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
142  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
143  const sensor_msgs::LaserScanConstPtr& scanMsg)
144 {
145  IMAGE_CONVERSION();
146 
147  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
148  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
149  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
150  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
151 }
152 void CommonDataSubscriber::rgbd2OdomScan3dCallback(
153  const nav_msgs::OdometryConstPtr & odomMsg,
154  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
155  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
156  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
157 {
158  IMAGE_CONVERSION();
159 
160  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
161  sensor_msgs::LaserScanConstPtr scanMsg; // Null
162  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
163  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
164 }
165 void CommonDataSubscriber::rgbd2OdomInfoCallback(
166  const nav_msgs::OdometryConstPtr & odomMsg,
167  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
168  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
169  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
170 {
171  IMAGE_CONVERSION();
172 
173  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
174  sensor_msgs::LaserScanConstPtr scanMsg; // Null
175  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
176  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
177 }
178 void CommonDataSubscriber::rgbd2OdomScan2dInfoCallback(
179  const nav_msgs::OdometryConstPtr & odomMsg,
180  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
181  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
182  const sensor_msgs::LaserScanConstPtr& scanMsg,
183  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
184 {
185  IMAGE_CONVERSION();
186 
187  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
188  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
189  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
190 }
191 void CommonDataSubscriber::rgbd2OdomScan3dInfoCallback(
192  const nav_msgs::OdometryConstPtr & odomMsg,
193  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
194  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
195  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
196  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
197 {
198  IMAGE_CONVERSION();
199 
200  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
201  sensor_msgs::LaserScanConstPtr scanMsg; // Null
202  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
203 }
204 
205 // 2 RGBD + User Data
206 void CommonDataSubscriber::rgbd2DataCallback(
207  const rtabmap_ros::UserDataConstPtr& userDataMsg,
208  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
209  const rtabmap_ros::RGBDImageConstPtr& image2Msg)
210 {
211  IMAGE_CONVERSION();
212 
213  nav_msgs::OdometryConstPtr odomMsg; // Null
214  sensor_msgs::LaserScanConstPtr scanMsg; // Null
215  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
216  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
217  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
218 }
219 void CommonDataSubscriber::rgbd2DataScan2dCallback(
220  const rtabmap_ros::UserDataConstPtr& userDataMsg,
221  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
222  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
223  const sensor_msgs::LaserScanConstPtr& scanMsg)
224 {
225  IMAGE_CONVERSION();
226 
227  nav_msgs::OdometryConstPtr odomMsg; // Null
228  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
229  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
230  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
231 }
232 void CommonDataSubscriber::rgbd2DataScan3dCallback(
233  const rtabmap_ros::UserDataConstPtr& userDataMsg,
234  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
235  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
236  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
237 {
238  IMAGE_CONVERSION();
239 
240  nav_msgs::OdometryConstPtr odomMsg; // Null
241  sensor_msgs::LaserScanConstPtr scanMsg; // Null
242  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
243  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
244 }
245 void CommonDataSubscriber::rgbd2DataInfoCallback(
246  const rtabmap_ros::UserDataConstPtr& userDataMsg,
247  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
248  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
249  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
250 {
251  IMAGE_CONVERSION();
252 
253  nav_msgs::OdometryConstPtr odomMsg; // Null
254  sensor_msgs::LaserScanConstPtr scanMsg; // Null
255  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
256  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
257 }
258 void CommonDataSubscriber::rgbd2DataScan2dInfoCallback(
259  const rtabmap_ros::UserDataConstPtr& userDataMsg,
260  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
261  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
262  const sensor_msgs::LaserScanConstPtr& scanMsg,
263  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
264 {
265  IMAGE_CONVERSION();
266 
267  nav_msgs::OdometryConstPtr odomMsg; // Null
268  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
269  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
270 }
271 void CommonDataSubscriber::rgbd2DataScan3dInfoCallback(
272  const rtabmap_ros::UserDataConstPtr& userDataMsg,
273  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
274  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
275  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
276  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
277 {
278  IMAGE_CONVERSION();
279 
280  nav_msgs::OdometryConstPtr odomMsg; // Null
281  sensor_msgs::LaserScanConstPtr scanMsg; // Null
282  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
283 }
284 
285 // 2 RGBD + Odom + User Data
286 void CommonDataSubscriber::rgbd2OdomDataCallback(
287  const nav_msgs::OdometryConstPtr& odomMsg,
288  const rtabmap_ros::UserDataConstPtr& userDataMsg,
289  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
290  const rtabmap_ros::RGBDImageConstPtr& image2Msg)
291 {
292  IMAGE_CONVERSION();
293 
294  sensor_msgs::LaserScanConstPtr scanMsg; // Null
295  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
296  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
297  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
298 }
299 void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
300  const nav_msgs::OdometryConstPtr& odomMsg,
301  const rtabmap_ros::UserDataConstPtr& userDataMsg,
302  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
303  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
304  const sensor_msgs::LaserScanConstPtr& scanMsg)
305 {
306  IMAGE_CONVERSION();
307 
308  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
309  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
310  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
311 }
312 void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
313  const nav_msgs::OdometryConstPtr& odomMsg,
314  const rtabmap_ros::UserDataConstPtr& userDataMsg,
315  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
316  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
317  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
318 {
319  IMAGE_CONVERSION();
320 
321  sensor_msgs::LaserScanConstPtr scanMsg; // Null
322  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
323  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
324 }
325 void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
326  const nav_msgs::OdometryConstPtr& odomMsg,
327  const rtabmap_ros::UserDataConstPtr& userDataMsg,
328  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
329  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
330  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
331 {
332  IMAGE_CONVERSION();
333 
334  sensor_msgs::LaserScanConstPtr scanMsg; // Null
335  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
336  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
337 }
338 void CommonDataSubscriber::rgbd2OdomDataScan2dInfoCallback(
339  const nav_msgs::OdometryConstPtr& odomMsg,
340  const rtabmap_ros::UserDataConstPtr& userDataMsg,
341  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
342  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
343  const sensor_msgs::LaserScanConstPtr& scanMsg,
344  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
345 {
346  IMAGE_CONVERSION();
347 
348  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
349  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
350 }
351 void CommonDataSubscriber::rgbd2OdomDataScan3dInfoCallback(
352  const nav_msgs::OdometryConstPtr& odomMsg,
353  const rtabmap_ros::UserDataConstPtr& userDataMsg,
354  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
355  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
356  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
357  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
358 {
359  IMAGE_CONVERSION();
360 
361  sensor_msgs::LaserScanConstPtr scanMsg; // Null
362  commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
363 }
364 
365 void CommonDataSubscriber::setupRGBD2Callbacks(
366  ros::NodeHandle & nh,
367  ros::NodeHandle & pnh,
368  bool subscribeOdom,
369  bool subscribeUserData,
370  bool subscribeScan2d,
371  bool subscribeScan3d,
372  bool subscribeOdomInfo,
373  int queueSize,
374  bool approxSync)
375 {
376  ROS_INFO("Setup rgbd2 callback");
377 
378  rgbdSubs_.resize(2);
379  for(int i=0; i<2; ++i)
380  {
381  rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
382  rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
383  }
384  if(subscribeOdom && subscribeUserData)
385  {
386  odomSub_.subscribe(nh, "odom", 1);
387  userDataSub_.subscribe(nh, "user_data", 1);
388  if(subscribeScan2d)
389  {
390  subscribedToScan2d_ = true;
391  scanSub_.subscribe(nh, "scan", 1);
392  if(subscribeOdomInfo)
393  {
394  subscribedToOdomInfo_ = true;
395  odomInfoSub_.subscribe(nh, "odom_info", 1);
396  SYNC_DECL6(rgbd2OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
397  }
398  else
399  {
400  SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
401  }
402  }
403  else if(subscribeScan3d)
404  {
405  subscribedToScan3d_ = true;
406  scan3dSub_.subscribe(nh, "scan_cloud", 1);
407  if(subscribeOdomInfo)
408  {
409  subscribedToOdomInfo_ = true;
410  odomInfoSub_.subscribe(nh, "odom_info", 1);
411  SYNC_DECL6(rgbd2OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
412  }
413  else
414  {
415  SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
416  }
417  }
418  else if(subscribeOdomInfo)
419  {
420  subscribedToOdomInfo_ = true;
421  odomInfoSub_.subscribe(nh, "odom_info", 1);
422  SYNC_DECL5(rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
423  }
424  else
425  {
426  SYNC_DECL4(rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
427  }
428  }
429  else if(subscribeOdom)
430  {
431  odomSub_.subscribe(nh, "odom", 1);
432  if(subscribeScan2d)
433  {
434  subscribedToScan2d_ = true;
435  scanSub_.subscribe(nh, "scan", 1);
436  if(subscribeOdomInfo)
437  {
438  subscribedToOdomInfo_ = true;
439  odomInfoSub_.subscribe(nh, "odom_info", 1);
440  SYNC_DECL5(rgbd2OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
441  }
442  else
443  {
444  SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
445  }
446  }
447  else if(subscribeScan3d)
448  {
449  subscribedToScan3d_ = true;
450  scan3dSub_.subscribe(nh, "scan_cloud", 1);
451  if(subscribeOdomInfo)
452  {
453  subscribedToOdomInfo_ = true;
454  odomInfoSub_.subscribe(nh, "odom_info", 1);
455  SYNC_DECL5(rgbd2OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
456  }
457  else
458  {
459  SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
460  }
461  }
462  else if(subscribeOdomInfo)
463  {
464  subscribedToOdomInfo_ = true;
465  odomInfoSub_.subscribe(nh, "odom_info", 1);
466  SYNC_DECL4(rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
467  }
468  else
469  {
470  SYNC_DECL3(rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
471  }
472  }
473  else if(subscribeUserData)
474  {
475  userDataSub_.subscribe(nh, "user_data", 1);
476  if(subscribeScan2d)
477  {
478  subscribedToScan2d_ = true;
479  scanSub_.subscribe(nh, "scan", 1);
480  if(subscribeOdomInfo)
481  {
482  subscribedToOdomInfo_ = true;
483  odomInfoSub_.subscribe(nh, "odom_info", 1);
484  SYNC_DECL5(rgbd2DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
485  }
486  else
487  {
488  SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
489  }
490  }
491  else if(subscribeScan3d)
492  {
493  subscribedToScan3d_ = true;
494  scan3dSub_.subscribe(nh, "scan_cloud", 1);
495  if(subscribeOdomInfo)
496  {
497  subscribedToOdomInfo_ = true;
498  odomInfoSub_.subscribe(nh, "odom_info", 1);
499  SYNC_DECL5(rgbd2DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
500  }
501  else
502  {
503  SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
504  }
505  }
506  else if(subscribeOdomInfo)
507  {
508  subscribedToOdomInfo_ = true;
509  odomInfoSub_.subscribe(nh, "odom_info", 1);
510  SYNC_DECL4(rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
511  }
512  else
513  {
514  SYNC_DECL3(rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
515  }
516  }
517  else
518  {
519  if(subscribeScan2d)
520  {
521  subscribedToScan2d_ = true;
522  scanSub_.subscribe(nh, "scan", 1);
523  if(subscribeOdomInfo)
524  {
525  subscribedToOdomInfo_ = true;
526  odomInfoSub_.subscribe(nh, "odom_info", 1);
527  SYNC_DECL4(rgbd2Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
528  }
529  else
530  {
531  SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
532  }
533  }
534  else if(subscribeScan3d)
535  {
536  subscribedToScan3d_ = true;
537  scan3dSub_.subscribe(nh, "scan_cloud", 1);
538  if(subscribeOdomInfo)
539  {
540  subscribedToOdomInfo_ = true;
541  odomInfoSub_.subscribe(nh, "odom_info", 1);
542  SYNC_DECL4(rgbd2Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
543  }
544  else
545  {
546  SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
547  }
548  }
549  else if(subscribeOdomInfo)
550  {
551  subscribedToOdomInfo_ = true;
552  odomInfoSub_.subscribe(nh, "odom_info", 1);
553  SYNC_DECL3(rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
554  }
555  else
556  {
557  SYNC_DECL2(rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
558  }
559  }
560 }
561 
562 } /* namespace rtabmap_ros */
uFormat
std::string uFormat(const char *fmt,...)
ros::NodeHandle
rtabmap_ros::CommonDataSubscriber::setupRGBD2Callbacks
void setupRGBD2Callbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeOdomInfo, int queueSize, bool approxSync)
Definition: CommonDataSubscriberRGBD2.cpp:365
SYNC_DECL5
#define SYNC_DECL5(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4)
Definition: CommonDataSubscriberDefines.h:168
IMAGE_CONVERSION
#define IMAGE_CONVERSION()
Definition: CommonDataSubscriberRGBD2.cpp:35
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::LaserScanConstPtr &scanMsg, const sensor_msgs::PointCloud2ConstPtr &scan3dMsg, const rtabmap_ros::OdomInfoConstPtr &odomInfoMsg)=0
SYNC_DECL3
#define SYNC_DECL3(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2)
Definition: CommonDataSubscriberDefines.h:127
rtabmap_ros::CommonDataSubscriber::userDataSub_
message_filters::Subscriber< rtabmap_ros::UserData > userDataSub_
Definition: CommonDataSubscriber.h:199
rtabmap_ros::CommonDataSubscriber::subscribedToOdomInfo_
bool subscribedToOdomInfo_
Definition: CommonDataSubscriber.h:180
rtabmap_ros
Definition: CommonDataSubscriber.h:55
SYNC_DECL6
#define SYNC_DECL6(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5)
Definition: CommonDataSubscriberDefines.h:190
rtabmap_ros::CommonDataSubscriber::odomInfoSub_
message_filters::Subscriber< rtabmap_ros::OdomInfo > odomInfoSub_
Definition: CommonDataSubscriber.h:202
rtabmap_ros::CommonDataSubscriber::subscribedToScan2d_
bool subscribedToScan2d_
Definition: CommonDataSubscriber.h:178
cv_bridge.h
ROS_INFO
#define ROS_INFO(...)
SYNC_DECL2
#define SYNC_DECL2(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1)
Definition: CommonDataSubscriberDefines.h:108
rtabmap_ros::CommonDataSubscriber::scanSub_
message_filters::Subscriber< sensor_msgs::LaserScan > scanSub_
Definition: CommonDataSubscriber.h:200
SYNC_DECL4
#define SYNC_DECL4(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3)
Definition: CommonDataSubscriberDefines.h:147
rtabmap_ros::CommonDataSubscriber::odomSub_
message_filters::Subscriber< nav_msgs::Odometry > odomSub_
Definition: CommonDataSubscriber.h:198
rtabmap_ros::CommonDataSubscriber::subscribedToScan3d_
bool subscribedToScan3d_
Definition: CommonDataSubscriber.h:179
rtabmap_ros::CommonDataSubscriber::scan3dSub_
message_filters::Subscriber< sensor_msgs::PointCloud2 > scan3dSub_
Definition: CommonDataSubscriber.h:201
message_filters::Subscriber::subscribe
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
message_filters::Subscriber< rtabmap_ros::RGBDImage >
rtabmap_ros::CommonDataSubscriber::rgbdSubs_
std::vector< message_filters::Subscriber< rtabmap_ros::RGBDImage > * > rgbdSubs_
Definition: CommonDataSubscriber.h:190

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:03