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

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