CommonDataSubscriberRGBD3.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap_sync/CommonDataSubscriber.h>
29 #include <rtabmap/utilite/UConversion.h>
30 #include <rtabmap/core/Compression.h>
31 #include <cv_bridge/cv_bridge.h>
32 #include "../../../rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h"
33 
34 namespace rtabmap_sync {
35 
36 #define IMAGE_CONVERSION() \
37  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3); \
38  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3); \
39  rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
40  rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
41  rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
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  cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
48  std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
49  depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
50  depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
51  depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
52  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs; \
53  std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPoints; \
54  std::vector<std::vector<rtabmap_msgs::Point3f> > localPoints3d; \
55  std::vector<cv::Mat> localDescriptors; \
56  if(!image1Msg->global_descriptor.data.empty()) \
57  globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
58  if(!image2Msg->global_descriptor.data.empty()) \
59  globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
60  if(!image3Msg->global_descriptor.data.empty()) \
61  globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
62  localKeyPoints.push_back(image1Msg->key_points); \
63  localKeyPoints.push_back(image2Msg->key_points); \
64  localKeyPoints.push_back(image3Msg->key_points); \
65  localPoints3d.push_back(image1Msg->points); \
66  localPoints3d.push_back(image2Msg->points); \
67  localPoints3d.push_back(image3Msg->points); \
68  localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
69  localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
70  localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors));
71 
72 // 3 RGBD
73 void CommonDataSubscriber::rgbd3Callback(
74  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
75  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
76  const rtabmap_msgs::RGBDImageConstPtr& image3Msg)
77 {
78  IMAGE_CONVERSION();
79 
80  nav_msgs::OdometryConstPtr odomMsg; // Null
81  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
82  sensor_msgs::LaserScan scanMsg; // Null
83  sensor_msgs::PointCloud2 scan3dMsg; // Null
84  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
85  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
86  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
87  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
88  localKeyPoints, localPoints3d, localDescriptors);
89 }
90 void CommonDataSubscriber::rgbd3Scan2dCallback(
91  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
92  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
93  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
94  const sensor_msgs::LaserScanConstPtr& scanMsg)
95 {
96  IMAGE_CONVERSION();
97 
98  nav_msgs::OdometryConstPtr odomMsg; // Null
99  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
100  sensor_msgs::PointCloud2 scan3dMsg; // Null
101  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
102  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
103  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
104  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
105  localKeyPoints, localPoints3d, localDescriptors);
106 }
107 void CommonDataSubscriber::rgbd3Scan3dCallback(
108  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
109  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
110  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
111  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
112 {
113  IMAGE_CONVERSION();
114 
115  nav_msgs::OdometryConstPtr odomMsg; // Null
116  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
117  sensor_msgs::LaserScan scanMsg; // Null
118  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
119  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
120  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
121  *scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
122  localKeyPoints, localPoints3d, localDescriptors);
123 }
124 void CommonDataSubscriber::rgbd3ScanDescCallback(
125  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
126  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
127  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
128  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
129 {
130  IMAGE_CONVERSION();
131 
132  nav_msgs::OdometryConstPtr odomMsg; // Null
133  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
134  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
135  if(!scanDescMsg->global_descriptor.data.empty())
136  {
137  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
138  }
139  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
140  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
141  scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
142  localKeyPoints, localPoints3d, localDescriptors);
143 }
144 void CommonDataSubscriber::rgbd3InfoCallback(
145  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
146  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
147  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
148  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
149 {
150  IMAGE_CONVERSION();
151 
152  nav_msgs::OdometryConstPtr odomMsg; // Null
153  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
154  sensor_msgs::LaserScan scanMsg; // Null
155  sensor_msgs::PointCloud2 scan3dMsg; // Null
156  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
157  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
158  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
159  localKeyPoints, localPoints3d, localDescriptors);
160 }
161 
162 // 2 RGBD + Odom
163 void CommonDataSubscriber::rgbd3OdomCallback(
164  const nav_msgs::OdometryConstPtr & odomMsg,
165  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
166  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
167  const rtabmap_msgs::RGBDImageConstPtr& image3Msg)
168 {
169  IMAGE_CONVERSION();
170 
171  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
172  sensor_msgs::LaserScan scanMsg; // Null
173  sensor_msgs::PointCloud2 scan3dMsg; // Null
174  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
175  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
176  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
177  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
178  localKeyPoints, localPoints3d, localDescriptors);
179 }
180 void CommonDataSubscriber::rgbd3OdomScan2dCallback(
181  const nav_msgs::OdometryConstPtr & odomMsg,
182  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
183  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
184  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
185  const sensor_msgs::LaserScanConstPtr& scanMsg)
186 {
187  IMAGE_CONVERSION();
188 
189  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
190  sensor_msgs::PointCloud2 scan3dMsg; // Null
191  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
192  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
193  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
194  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
195  localKeyPoints, localPoints3d, localDescriptors);
196 }
197 void CommonDataSubscriber::rgbd3OdomScan3dCallback(
198  const nav_msgs::OdometryConstPtr & odomMsg,
199  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
200  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
201  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
202  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
203 {
204  IMAGE_CONVERSION();
205 
206  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
207  sensor_msgs::LaserScan scanMsg; // Null
208  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
209  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
210  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
211  *scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
212  localKeyPoints, localPoints3d, localDescriptors);
213 }
214 void CommonDataSubscriber::rgbd3OdomScanDescCallback(
215  const nav_msgs::OdometryConstPtr & odomMsg,
216  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
217  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
218  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
219  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
220 {
221  IMAGE_CONVERSION();
222 
223  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
224  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
225  if(!scanDescMsg->global_descriptor.data.empty())
226  {
227  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
228  }
229  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
230  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
231  scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
232  localKeyPoints, localPoints3d, localDescriptors);
233 }
234 void CommonDataSubscriber::rgbd3OdomInfoCallback(
235  const nav_msgs::OdometryConstPtr & odomMsg,
236  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
237  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
238  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
239  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
240 {
241  IMAGE_CONVERSION();
242 
243  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
244  sensor_msgs::LaserScan scanMsg; // Null
245  sensor_msgs::PointCloud2 scan3dMsg; // Null
246  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
247  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
248  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
249  localKeyPoints, localPoints3d, localDescriptors);
250 }
251 
252 #ifdef RTABMAP_SYNC_USER_DATA
253 // 2 RGBD + User Data
254 void CommonDataSubscriber::rgbd3DataCallback(
255  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
256  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
257  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
258  const rtabmap_msgs::RGBDImageConstPtr& image3Msg)
259 {
260  IMAGE_CONVERSION();
261 
262  nav_msgs::OdometryConstPtr odomMsg; // Null
263  sensor_msgs::LaserScan scanMsg; // Null
264  sensor_msgs::PointCloud2 scan3dMsg; // Null
265  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
266  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
267  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
268  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
269  localKeyPoints, localPoints3d, localDescriptors);
270 }
271 void CommonDataSubscriber::rgbd3DataScan2dCallback(
272  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
273  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
274  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
275  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
276  const sensor_msgs::LaserScanConstPtr& scanMsg)
277 {
278  IMAGE_CONVERSION();
279 
280  nav_msgs::OdometryConstPtr odomMsg; // Null
281  sensor_msgs::PointCloud2 scan3dMsg; // Null
282  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
283  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
284  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
285  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
286  localKeyPoints, localPoints3d, localDescriptors);
287 }
288 void CommonDataSubscriber::rgbd3DataScan3dCallback(
289  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
290  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
291  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
292  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
293  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
294 {
295  IMAGE_CONVERSION();
296 
297  nav_msgs::OdometryConstPtr odomMsg; // Null
298  sensor_msgs::LaserScan scanMsg; // Null
299  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
300  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
301  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
302  *scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
303  localKeyPoints, localPoints3d, localDescriptors);
304 }
305 void CommonDataSubscriber::rgbd3DataScanDescCallback(
306  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
307  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
308  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
309  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
310  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
311 {
312  IMAGE_CONVERSION();
313 
314  nav_msgs::OdometryConstPtr odomMsg; // Null
315  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
316  if(!scanDescMsg->global_descriptor.data.empty())
317  {
318  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
319  }
320  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
321  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
322  scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
323  localKeyPoints, localPoints3d, localDescriptors);
324 }
325 void CommonDataSubscriber::rgbd3DataInfoCallback(
326  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
327  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
328  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
329  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
330  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
331 {
332  IMAGE_CONVERSION();
333 
334  nav_msgs::OdometryConstPtr odomMsg; // Null
335  sensor_msgs::LaserScan scanMsg; // Null
336  sensor_msgs::PointCloud2 scan3dMsg; // Null
337  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
338  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
339  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
340  localKeyPoints, localPoints3d, localDescriptors);
341 }
342 // 2 RGBD + Odom + User Data
343 void CommonDataSubscriber::rgbd3OdomDataCallback(
344  const nav_msgs::OdometryConstPtr& odomMsg,
345  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
346  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
347  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
348  const rtabmap_msgs::RGBDImageConstPtr& image3Msg)
349 {
350  IMAGE_CONVERSION();
351 
352  sensor_msgs::LaserScan scanMsg; // Null
353  sensor_msgs::PointCloud2 scan3dMsg; // Null
354  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
355  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
356  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
357  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
358  localKeyPoints, localPoints3d, localDescriptors);
359 }
360 void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
361  const nav_msgs::OdometryConstPtr& odomMsg,
362  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
363  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
364  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
365  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
366  const sensor_msgs::LaserScanConstPtr& scanMsg)
367 {
368  IMAGE_CONVERSION();
369 
370  sensor_msgs::PointCloud2 scan3dMsg; // Null
371  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
372  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
373  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
374  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
375  localKeyPoints, localPoints3d, localDescriptors);
376 }
377 void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
378  const nav_msgs::OdometryConstPtr& odomMsg,
379  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
380  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
381  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
382  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
383  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
384 {
385  IMAGE_CONVERSION();
386 
387  sensor_msgs::LaserScan scanMsg; // Null
388  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
389  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
390  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
391  *scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
392  localKeyPoints, localPoints3d, localDescriptors);
393 }
394 void CommonDataSubscriber::rgbd3OdomDataScanDescCallback(
395  const nav_msgs::OdometryConstPtr& odomMsg,
396  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
397  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
398  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
399  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
400  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
401 {
402  IMAGE_CONVERSION();
403 
404  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
405  if(!scanDescMsg->global_descriptor.data.empty())
406  {
407  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
408  }
409  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
410  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
411  scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
412  localKeyPoints, localPoints3d, localDescriptors);
413 }
414 void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
415  const nav_msgs::OdometryConstPtr& odomMsg,
416  const rtabmap_msgs::UserDataConstPtr& userDataMsg,
417  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
418  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
419  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
420  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
421 {
422  IMAGE_CONVERSION();
423 
424  sensor_msgs::LaserScan scanMsg; // Null
425  sensor_msgs::PointCloud2 scan3dMsg; // Null
426  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
427  depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
428  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
429  localKeyPoints, localPoints3d, localDescriptors);
430 }
431 #endif
432 
433 void CommonDataSubscriber::setupRGBD3Callbacks(
434  ros::NodeHandle & nh,
435  ros::NodeHandle & pnh,
436  bool subscribeOdom,
437  bool subscribeUserData,
438  bool subscribeScan2d,
439  bool subscribeScan3d,
440  bool subscribeScanDescriptor,
441  bool subscribeOdomInfo)
442 {
443  ROS_INFO("Setup rgbd3 callback");
444 
445  rgbdSubs_.resize(3);
446  for(int i=0; i<3; ++i)
447  {
448  rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
449  rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_);
450  }
451 #ifdef RTABMAP_SYNC_USER_DATA
452  if(subscribeOdom && subscribeUserData)
453  {
454  odomSub_.subscribe(nh, "odom", topicQueueSize_);
455  userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
456  if(subscribeScanDescriptor)
457  {
458  subscribedToScanDescriptor_ = true;
459  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
460  if(subscribeOdomInfo)
461  {
462  subscribedToOdomInfo_ = false;
463  ROS_WARN("subscribe_odom_info ignored...");
464  }
465  SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
466  }
467  else if(subscribeScan2d)
468  {
469  subscribedToScan2d_ = true;
470  scanSub_.subscribe(nh, "scan", topicQueueSize_);
471  if(subscribeOdomInfo)
472  {
473  subscribedToOdomInfo_ = false;
474  ROS_WARN("subscribe_odom_info ignored...");
475  }
476  SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
477  }
478  else if(subscribeScan3d)
479  {
480  subscribedToScan3d_ = true;
481  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
482  if(subscribeOdomInfo)
483  {
484  subscribedToOdomInfo_ = false;
485  ROS_WARN("subscribe_odom_info ignored...");
486  }
487  SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
488  }
489  else if(subscribeOdomInfo)
490  {
491  subscribedToOdomInfo_ = true;
492  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
493  SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
494  }
495  else
496  {
497  SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
498  }
499  }
500  else
501 #endif
502  if(subscribeOdom)
503  {
504  odomSub_.subscribe(nh, "odom", topicQueueSize_);
505  if(subscribeScanDescriptor)
506  {
507  subscribedToScanDescriptor_ = true;
508  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
509  if(subscribeOdomInfo)
510  {
511  subscribedToOdomInfo_ = false;
512  ROS_WARN("subscribe_odom_info ignored...");
513  }
514  SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
515  }
516  else if(subscribeScan2d)
517  {
518  subscribedToScan2d_ = true;
519  scanSub_.subscribe(nh, "scan", topicQueueSize_);
520  if(subscribeOdomInfo)
521  {
522  subscribedToOdomInfo_ = false;
523  ROS_WARN("subscribe_odom_info ignored...");
524  }
525  SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
526  }
527  else if(subscribeScan3d)
528  {
529  subscribedToScan3d_ = true;
530  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
531  if(subscribeOdomInfo)
532  {
533  subscribedToOdomInfo_ = false;
534  ROS_WARN("subscribe_odom_info ignored...");
535  }
536  SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
537  }
538  else if(subscribeOdomInfo)
539  {
540  subscribedToOdomInfo_ = true;
541  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
542  SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
543  }
544  else
545  {
546  SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
547  }
548  }
549 #ifdef RTABMAP_SYNC_USER_DATA
550  else if(subscribeUserData)
551  {
552  userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
553  if(subscribeScanDescriptor)
554  {
555  subscribedToScanDescriptor_ = true;
556  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
557  if(subscribeOdomInfo)
558  {
559  subscribedToOdomInfo_ = false;
560  ROS_WARN("subscribe_odom_info ignored...");
561  }
562  SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); }
563  else if(subscribeScan2d)
564  {
565  subscribedToScan2d_ = true;
566  scanSub_.subscribe(nh, "scan", topicQueueSize_);
567  if(subscribeOdomInfo)
568  {
569  subscribedToOdomInfo_ = false;
570  ROS_WARN("subscribe_odom_info ignored...");
571  }
572  SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
573  }
574  else if(subscribeScan3d)
575  {
576  subscribedToScan3d_ = true;
577  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
578  if(subscribeOdomInfo)
579  {
580  subscribedToOdomInfo_ = false;
581  ROS_WARN("subscribe_odom_info ignored...");
582  }
583  SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
584  }
585  else if(subscribeOdomInfo)
586  {
587  subscribedToOdomInfo_ = true;
588  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
589  SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
590  }
591  else
592  {
593  SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
594  }
595  }
596 #endif
597  else
598  {
599  if(subscribeScanDescriptor)
600  {
601  subscribedToScanDescriptor_ = true;
602  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
603  if(subscribeOdomInfo)
604  {
605  subscribedToOdomInfo_ = false;
606  ROS_WARN("subscribe_odom_info ignored...");
607  }
608  SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
609  }
610  else if(subscribeScan2d)
611  {
612  subscribedToScan2d_ = true;
613  scanSub_.subscribe(nh, "scan", topicQueueSize_);
614  if(subscribeOdomInfo)
615  {
616  subscribedToOdomInfo_ = false;
617  ROS_WARN("subscribe_odom_info ignored...");
618  }
619  SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
620  }
621  else if(subscribeScan3d)
622  {
623  subscribedToScan3d_ = true;
624  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
625  if(subscribeOdomInfo)
626  {
627  subscribedToOdomInfo_ = false;
628  ROS_WARN("subscribe_odom_info ignored...");
629  }
630  SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
631  }
632  else if(subscribeOdomInfo)
633  {
634  subscribedToOdomInfo_ = true;
635  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
636  SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
637  }
638  else
639  {
640  SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
641  }
642  }
643 }
644 
645 } /* namespace rtabmap_sync */
SYNC_DECL6
#define SYNC_DECL6(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5)
Definition: CommonDataSubscriberDefines.h:190
rtabmap_sync
Definition: CommonDataSubscriber.h:59
uFormat
std::string uFormat(const char *fmt,...)
Compression.h
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
IMAGE_CONVERSION
#define IMAGE_CONVERSION()
Definition: CommonDataSubscriberRGBD3.cpp:36
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
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