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