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

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:18