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

rtabmap_sync
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:39:12