CommonDataSubscriberRGBD.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 // 1 RGBD camera
36 void CommonDataSubscriber::rgbdCallback(
37  const rtabmap_ros::RGBDImageConstPtr& image1Msg)
38 {
39  cv_bridge::CvImageConstPtr rgb, depth;
40  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
41 
42  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
43  nav_msgs::OdometryConstPtr odomMsg; // Null
44  sensor_msgs::LaserScanConstPtr scanMsg; // Null
45  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
46  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
47  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
48 }
49 void CommonDataSubscriber::rgbdScan2dCallback(
50  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
51  const sensor_msgs::LaserScanConstPtr& scanMsg)
52 {
53  cv_bridge::CvImageConstPtr rgb, depth;
54  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
55 
56  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
57  nav_msgs::OdometryConstPtr odomMsg; // Null
58  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
59  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
60  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
61 }
62 void CommonDataSubscriber::rgbdScan3dCallback(
63  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
64  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
65 {
66  cv_bridge::CvImageConstPtr rgb, depth;
67  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
68 
69  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
70  nav_msgs::OdometryConstPtr odomMsg; // Null
71  sensor_msgs::LaserScanConstPtr scanMsg; // Null
72  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
73  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
74 }
75 void CommonDataSubscriber::rgbdInfoCallback(
76  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
77  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
78 {
79  cv_bridge::CvImageConstPtr rgb, depth;
80  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
81 
82  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
83  nav_msgs::OdometryConstPtr odomMsg; // Null
84  sensor_msgs::LaserScanConstPtr scanMsg; // Null
85  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
86  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
87 }
88 void CommonDataSubscriber::rgbdScan2dInfoCallback(
89  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
90  const sensor_msgs::LaserScanConstPtr& scanMsg,
91  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
92 {
93  cv_bridge::CvImageConstPtr rgb, depth;
94  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
95 
96  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
97  nav_msgs::OdometryConstPtr odomMsg; // Null
98  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
99  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
100 }
101 void CommonDataSubscriber::rgbdScan3dInfoCallback(
102  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
103  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
104  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
105 {
106  cv_bridge::CvImageConstPtr rgb, depth;
107  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
108 
109  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
110  nav_msgs::OdometryConstPtr odomMsg; // Null
111  sensor_msgs::LaserScanConstPtr scanMsg; // Null
112  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
113 }
114 
115 // 1 RGBD camera + Odom
116 void CommonDataSubscriber::rgbdOdomCallback(
117  const nav_msgs::OdometryConstPtr & odomMsg,
118  const rtabmap_ros::RGBDImageConstPtr& image1Msg)
119 {
120  cv_bridge::CvImageConstPtr rgb, depth;
121  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
122 
123  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
124  sensor_msgs::LaserScanConstPtr scanMsg; // Null
125  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
126  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
127  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
128 }
129 void CommonDataSubscriber::rgbdOdomScan2dCallback(
130  const nav_msgs::OdometryConstPtr & odomMsg,
131  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
132  const sensor_msgs::LaserScanConstPtr& scanMsg)
133 {
134  cv_bridge::CvImageConstPtr rgb, depth;
135  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
136 
137  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
138  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
139  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
140  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
141 }
142 void CommonDataSubscriber::rgbdOdomScan3dCallback(
143  const nav_msgs::OdometryConstPtr & odomMsg,
144  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
145  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
146 {
147  cv_bridge::CvImageConstPtr rgb, depth;
148  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
149 
150  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
151  sensor_msgs::LaserScanConstPtr scanMsg; // Null
152  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
153  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
154 }
155 void CommonDataSubscriber::rgbdOdomInfoCallback(
156  const nav_msgs::OdometryConstPtr & odomMsg,
157  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
158  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
159 {
160  cv_bridge::CvImageConstPtr rgb, depth;
161  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
162 
163  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
164  sensor_msgs::LaserScanConstPtr scanMsg; // Null
165  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
166  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
167 }
168 void CommonDataSubscriber::rgbdOdomScan2dInfoCallback(
169  const nav_msgs::OdometryConstPtr & odomMsg,
170  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
171  const sensor_msgs::LaserScanConstPtr& scanMsg,
172  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
173 {
174  cv_bridge::CvImageConstPtr rgb, depth;
175  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
176 
177  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
178  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
179  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
180 }
181 void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(
182  const nav_msgs::OdometryConstPtr & odomMsg,
183  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
184  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
185  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
186 {
187  cv_bridge::CvImageConstPtr rgb, depth;
188  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
189 
190  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
191  sensor_msgs::LaserScanConstPtr scanMsg; // Null
192  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
193 }
194 
195 // 1 RGBD camera + User Data
196 void CommonDataSubscriber::rgbdDataCallback(
197  const rtabmap_ros::UserDataConstPtr & userDataMsg,
198  const rtabmap_ros::RGBDImageConstPtr& image1Msg)
199 {
200  cv_bridge::CvImageConstPtr rgb, depth;
201  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
202 
203  nav_msgs::OdometryConstPtr odomMsg; // Null
204  sensor_msgs::LaserScanConstPtr scanMsg; // Null
205  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
206  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
207  commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
208 }
209 void CommonDataSubscriber::rgbdDataScan2dCallback(
210  const rtabmap_ros::UserDataConstPtr & userDataMsg,
211  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
212  const sensor_msgs::LaserScanConstPtr& scanMsg)
213 {
214  cv_bridge::CvImageConstPtr rgb, depth;
215  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
216 
217  nav_msgs::OdometryConstPtr odomMsg; // Null
218  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
219  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
220  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
221 }
222 void CommonDataSubscriber::rgbdDataScan3dCallback(
223  const rtabmap_ros::UserDataConstPtr & userDataMsg,
224  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
225  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
226 {
227  cv_bridge::CvImageConstPtr rgb, depth;
228  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
229 
230  nav_msgs::OdometryConstPtr odomMsg; // Null
231  sensor_msgs::LaserScanConstPtr scanMsg; // Null
232  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
233  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
234 }
235 void CommonDataSubscriber::rgbdDataInfoCallback(
236  const rtabmap_ros::UserDataConstPtr & userDataMsg,
237  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
238  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
239 {
240  cv_bridge::CvImageConstPtr rgb, depth;
241  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
242 
243  nav_msgs::OdometryConstPtr odomMsg; // Null
244  sensor_msgs::LaserScanConstPtr scanMsg; // Null
245  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
246  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
247 }
248 void CommonDataSubscriber::rgbdDataScan2dInfoCallback(
249  const rtabmap_ros::UserDataConstPtr & userDataMsg,
250  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
251  const sensor_msgs::LaserScanConstPtr& scanMsg,
252  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
253 {
254  cv_bridge::CvImageConstPtr rgb, depth;
255  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
256 
257  nav_msgs::OdometryConstPtr odomMsg; // Null
258  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
259  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
260 }
261 void CommonDataSubscriber::rgbdDataScan3dInfoCallback(
262  const rtabmap_ros::UserDataConstPtr & userDataMsg,
263  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
264  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
265  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
266 {
267  cv_bridge::CvImageConstPtr rgb, depth;
268  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
269 
270  nav_msgs::OdometryConstPtr odomMsg; // Null
271  sensor_msgs::LaserScanConstPtr scanMsg; // Null
272  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
273 }
274 
275 // 1 RGBD camera + Odom + User Data
276 void CommonDataSubscriber::rgbdOdomDataCallback(
277  const nav_msgs::OdometryConstPtr & odomMsg,
278  const rtabmap_ros::UserDataConstPtr & userDataMsg,
279  const rtabmap_ros::RGBDImageConstPtr& image1Msg)
280 {
281  cv_bridge::CvImageConstPtr rgb, depth;
282  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
283 
284  sensor_msgs::LaserScanConstPtr scanMsg; // Null
285  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
286  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
287  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
288 }
289 void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
290  const nav_msgs::OdometryConstPtr & odomMsg,
291  const rtabmap_ros::UserDataConstPtr & userDataMsg,
292  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
293  const sensor_msgs::LaserScanConstPtr& scanMsg)
294 {
295  cv_bridge::CvImageConstPtr rgb, depth;
296  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
297 
298  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
299  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
300  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
301 }
302 void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
303  const nav_msgs::OdometryConstPtr & odomMsg,
304  const rtabmap_ros::UserDataConstPtr & userDataMsg,
305  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
306  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
307 {
308  cv_bridge::CvImageConstPtr rgb, depth;
309  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
310 
311  sensor_msgs::LaserScanConstPtr scanMsg; // Null
312  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
313  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
314 }
315 void CommonDataSubscriber::rgbdOdomDataInfoCallback(
316  const nav_msgs::OdometryConstPtr & odomMsg,
317  const rtabmap_ros::UserDataConstPtr & userDataMsg,
318  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
319  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
320 {
321  cv_bridge::CvImageConstPtr rgb, depth;
322  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
323 
324  sensor_msgs::LaserScanConstPtr scanMsg; // Null
325  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
326  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
327 }
328 void CommonDataSubscriber::rgbdOdomDataScan2dInfoCallback(
329  const nav_msgs::OdometryConstPtr & odomMsg,
330  const rtabmap_ros::UserDataConstPtr & userDataMsg,
331  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
332  const sensor_msgs::LaserScanConstPtr& scanMsg,
333  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
334 {
335  cv_bridge::CvImageConstPtr rgb, depth;
336  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
337 
338  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
339  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
340 }
341 void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback(
342  const nav_msgs::OdometryConstPtr & odomMsg,
343  const rtabmap_ros::UserDataConstPtr & userDataMsg,
344  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
345  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
346  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
347 {
348  cv_bridge::CvImageConstPtr rgb, depth;
349  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
350 
351  sensor_msgs::LaserScanConstPtr scanMsg; // Null
352  commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
353 }
354 
355 void CommonDataSubscriber::setupRGBDCallbacks(
356  ros::NodeHandle & nh,
357  ros::NodeHandle & pnh,
358  bool subscribeOdom,
359  bool subscribeUserData,
360  bool subscribeScan2d,
361  bool subscribeScan3d,
362  bool subscribeOdomInfo,
363  int queueSize,
364  bool approxSync)
365 {
366  ROS_INFO("Setup rgbd callback");
367 
368  if(subscribeOdom || subscribeUserData || subscribeScan2d || subscribeScan3d || subscribeOdomInfo)
369  {
370  rgbdSubs_.resize(1);
371  rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
372  rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1);
373 
374 
375  if(subscribeOdom && subscribeUserData)
376  {
377  odomSub_.subscribe(nh, "odom", 1);
378  userDataSub_.subscribe(nh, "user_data", 1);
379  if(subscribeScan2d)
380  {
381  subscribedToScan2d_ = true;
382  scanSub_.subscribe(nh, "scan", 1);
383  if(subscribeOdomInfo)
384  {
385  subscribedToOdomInfo_ = true;
386  odomInfoSub_.subscribe(nh, "odom_info", 1);
387  SYNC_DECL5(rgbdOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
388  }
389  else
390  {
391  SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
392  }
393  }
394  else if(subscribeScan3d)
395  {
396  subscribedToScan3d_ = true;
397  scan3dSub_.subscribe(nh, "scan_cloud", 1);
398  if(subscribeOdomInfo)
399  {
400  subscribedToOdomInfo_ = true;
401  odomInfoSub_.subscribe(nh, "odom_info", 1);
402  SYNC_DECL5(rgbdOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
403  }
404  else
405  {
406  SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
407  }
408  }
409  else if(subscribeOdomInfo)
410  {
411  subscribedToOdomInfo_ = true;
412  odomInfoSub_.subscribe(nh, "odom_info", 1);
413  SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
414  }
415  else
416  {
417  SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
418  }
419  }
420  else if(subscribeOdom)
421  {
422  odomSub_.subscribe(nh, "odom", 1);
423  if(subscribeScan2d)
424  {
425  subscribedToScan2d_ = true;
426  scanSub_.subscribe(nh, "scan", 1);
427  if(subscribeOdomInfo)
428  {
429  subscribedToOdomInfo_ = true;
430  odomInfoSub_.subscribe(nh, "odom_info", 1);
431  SYNC_DECL4(rgbdOdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
432  }
433  else
434  {
435  SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
436  }
437  }
438  else if(subscribeScan3d)
439  {
440  subscribedToScan3d_ = true;
441  scan3dSub_.subscribe(nh, "scan_cloud", 1);
442  if(subscribeOdomInfo)
443  {
444  subscribedToOdomInfo_ = true;
445  odomInfoSub_.subscribe(nh, "odom_info", 1);
446  SYNC_DECL4(rgbdOdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
447  }
448  else
449  {
450  SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
451  }
452  }
453  else if(subscribeOdomInfo)
454  {
455  subscribedToOdomInfo_ = true;
456  odomInfoSub_.subscribe(nh, "odom_info", 1);
457  SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
458  }
459  else
460  {
461  SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
462  }
463  }
464  else if(subscribeUserData)
465  {
466  userDataSub_.subscribe(nh, "user_data", 1);
467  if(subscribeScan2d)
468  {
469  subscribedToScan2d_ = true;
470  scanSub_.subscribe(nh, "scan", 1);
471  if(subscribeOdomInfo)
472  {
473  subscribedToOdomInfo_ = true;
474  odomInfoSub_.subscribe(nh, "odom_info", 1);
475  SYNC_DECL4(rgbdDataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
476  }
477  else
478  {
479  SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
480  }
481  }
482  else if(subscribeScan3d)
483  {
484  subscribedToScan3d_ = true;
485  scan3dSub_.subscribe(nh, "scan_cloud", 1);
486  if(subscribeOdomInfo)
487  {
488  subscribedToOdomInfo_ = true;
489  odomInfoSub_.subscribe(nh, "odom_info", 1);
490  SYNC_DECL4(rgbdDataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
491  }
492  else
493  {
494  SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
495  }
496  }
497  else if(subscribeOdomInfo)
498  {
499  subscribedToOdomInfo_ = true;
500  odomInfoSub_.subscribe(nh, "odom_info", 1);
501  SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
502  }
503  else
504  {
505  SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
506  }
507  }
508  else
509  {
510  if(subscribeScan2d)
511  {
512  subscribedToScan2d_ = true;
513  scanSub_.subscribe(nh, "scan", 1);
514  if(subscribeOdomInfo)
515  {
516  subscribedToOdomInfo_ = true;
517  odomInfoSub_.subscribe(nh, "odom_info", 1);
518  SYNC_DECL3(rgbdScan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
519  }
520  else
521  {
522  SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
523  }
524  }
525  else if(subscribeScan3d)
526  {
527  subscribedToScan3d_ = true;
528  scan3dSub_.subscribe(nh, "scan_cloud", 1);
529  if(subscribeOdomInfo)
530  {
531  subscribedToOdomInfo_ = true;
532  odomInfoSub_.subscribe(nh, "odom_info", 1);
533  SYNC_DECL3(rgbdScan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
534  }
535  else
536  {
537  SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
538  }
539  }
540  else if(subscribeOdomInfo)
541  {
542  subscribedToOdomInfo_ = true;
543  odomInfoSub_.subscribe(nh, "odom_info", 1);
544  SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
545  }
546  else
547  {
548  ROS_FATAL("Not supposed to be here!");
549  }
550  }
551  }
552  else
553  {
554  rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this);
555 
556  subscribedTopicsMsg_ =
557  uFormat("\n%s subscribed to:\n %s",
558  ros::this_node::getName().c_str(),
559  rgbdSub_.getTopic().c_str());
560  }
561 }
562 
563 } /* namespace rtabmap_ros */
rtabmap_ros::CommonDataSubscriber::rgbdCallback
void rgbdCallback(const rtabmap_ros::RGBDImageConstPtr &)
Definition: CommonDataSubscriberRGBD.cpp:36
uFormat
std::string uFormat(const char *fmt,...)
ROS_FATAL
#define ROS_FATAL(...)
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::LaserScanConstPtr &scanMsg, const sensor_msgs::PointCloud2ConstPtr &scan3dMsg, const rtabmap_ros::OdomInfoConstPtr &odomInfoMsg)
Definition: CommonDataSubscriber.cpp:587
ros::NodeHandle
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
SYNC_DECL5
#define SYNC_DECL5(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4)
Definition: CommonDataSubscriberDefines.h:168
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
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::odomInfoSub_
message_filters::Subscriber< rtabmap_ros::OdomInfo > odomInfoSub_
Definition: CommonDataSubscriber.h:202
boost::shared_ptr
rtabmap_ros::CommonDataSubscriber::subscribedToScan2d_
bool subscribedToScan2d_
Definition: CommonDataSubscriber.h:178
rtabmap_ros::CommonDataSubscriber::setupRGBDCallbacks
void setupRGBDCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeOdomInfo, int queueSize, bool approxSync)
Definition: CommonDataSubscriberRGBD.cpp:355
cv_bridge.h
ROS_INFO
#define ROS_INFO(...)
ros::Subscriber::getTopic
std::string getTopic() const
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)
rtabmap_ros::toCvShare
void toCvShare(const rtabmap_ros::RGBDImageConstPtr &image, cv_bridge::CvImageConstPtr &rgb, cv_bridge::CvImageConstPtr &depth)
Definition: MsgConversion.cpp:170
message_filters::Subscriber< rtabmap_ros::RGBDImage >
rtabmap_ros::CommonDataSubscriber::subscribedTopicsMsg_
std::string subscribedTopicsMsg_
Definition: CommonDataSubscriber.h:168
rtabmap_ros::CommonDataSubscriber::rgbdSub_
ros::Subscriber rgbdSub_
Definition: CommonDataSubscriber.h:189
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