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