CommonDataSubscriberRGBD.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap_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 // 1 RGBD camera
37 void CommonDataSubscriber::rgbdCallback(
38  const rtabmap_msgs::RGBDImageConstPtr& image1Msg)
39 {
40  cv_bridge::CvImageConstPtr rgb, depth;
41  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
42 
43  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
44  nav_msgs::OdometryConstPtr odomMsg; // Null
45  sensor_msgs::LaserScan scanMsg; // Null
46  sensor_msgs::PointCloud2 scan3dMsg; // Null
47  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
48 
49  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
50  if(!image1Msg->global_descriptor.data.empty())
51  {
52  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
53  }
54 
55  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
56  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
57  scanMsg, scan3dMsg, odomInfoMsg,
58  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
59  rtabmap::uncompressData(image1Msg->descriptors));
60 }
61 void CommonDataSubscriber::rgbdScan2dCallback(
62  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
63  const sensor_msgs::LaserScanConstPtr& scanMsg)
64 {
65  cv_bridge::CvImageConstPtr rgb, depth;
66  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
67 
68  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
69  nav_msgs::OdometryConstPtr odomMsg; // Null
70  sensor_msgs::PointCloud2 scan3dMsg; // Null
71  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
72 
73  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
74  if(!image1Msg->global_descriptor.data.empty())
75  {
76  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
77  }
78 
79  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
80  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
81  *scanMsg, scan3dMsg, odomInfoMsg,
82  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
83  rtabmap::uncompressData(image1Msg->descriptors));
84 }
85 void CommonDataSubscriber::rgbdScan3dCallback(
86  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
87  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
88 {
89  cv_bridge::CvImageConstPtr rgb, depth;
90  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
91 
92  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
93  nav_msgs::OdometryConstPtr odomMsg; // Null
94  sensor_msgs::LaserScan scanMsg; // Null
95  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
96 
97  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
98  if(!image1Msg->global_descriptor.data.empty())
99  {
100  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
101  }
102 
103  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
104  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
105  scanMsg, *scan3dMsg, odomInfoMsg,
106  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
107  rtabmap::uncompressData(image1Msg->descriptors));
108 }
109 void CommonDataSubscriber::rgbdScanDescCallback(
110  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
111  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
112 {
113  cv_bridge::CvImageConstPtr rgb, depth;
114  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
115 
116  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
117  nav_msgs::OdometryConstPtr odomMsg; // Null
118  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
119 
120  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
121  if(!image1Msg->global_descriptor.data.empty())
122  {
123  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
124  }
125 
126  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
127  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
128  scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
129  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
130  rtabmap::uncompressData(image1Msg->descriptors));
131 }
132 void CommonDataSubscriber::rgbdInfoCallback(
133  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
134  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
135 {
136  cv_bridge::CvImageConstPtr rgb, depth;
137  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
138 
139  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
140  nav_msgs::OdometryConstPtr odomMsg; // Null
141  sensor_msgs::LaserScan scanMsg; // Null
142  sensor_msgs::PointCloud2 scan3dMsg; // Null
143 
144  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
145  if(!image1Msg->global_descriptor.data.empty())
146  {
147  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
148  }
149 
150  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
151  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
152  scanMsg, scan3dMsg, odomInfoMsg,
153  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
154  rtabmap::uncompressData(image1Msg->descriptors));
155 }
156 
157 // 1 RGBD camera + Odom
158 void CommonDataSubscriber::rgbdOdomCallback(
159  const nav_msgs::OdometryConstPtr & odomMsg,
160  const rtabmap_msgs::RGBDImageConstPtr& image1Msg)
161 {
162  cv_bridge::CvImageConstPtr rgb, depth;
163  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
164 
165  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
166  sensor_msgs::LaserScan scanMsg; // Null
167  sensor_msgs::PointCloud2 scan3dMsg; // Null
168  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
169 
170  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
171  if(!image1Msg->global_descriptor.data.empty())
172  {
173  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
174  }
175 
176  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
177  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
178  scanMsg, scan3dMsg, odomInfoMsg,
179  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
180  rtabmap::uncompressData(image1Msg->descriptors));
181 }
182 void CommonDataSubscriber::rgbdOdomScan2dCallback(
183  const nav_msgs::OdometryConstPtr & odomMsg,
184  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
185  const sensor_msgs::LaserScanConstPtr& scanMsg)
186 {
187  cv_bridge::CvImageConstPtr rgb, depth;
188  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
189 
190  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
191  sensor_msgs::PointCloud2 scan3dMsg; // Null
192  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
193 
194  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
195  if(!image1Msg->global_descriptor.data.empty())
196  {
197  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
198  }
199 
200  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
201  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
202  *scanMsg, scan3dMsg, odomInfoMsg,
203  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
204  rtabmap::uncompressData(image1Msg->descriptors));
205 }
206 void CommonDataSubscriber::rgbdOdomScan3dCallback(
207  const nav_msgs::OdometryConstPtr & odomMsg,
208  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
209  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
210 {
211  cv_bridge::CvImageConstPtr rgb, depth;
212  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
213 
214  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
215  sensor_msgs::LaserScan scanMsg; // Null
216  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
217 
218  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
219  if(!image1Msg->global_descriptor.data.empty())
220  {
221  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
222  }
223 
224  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
225  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
226  scanMsg, *scan3dMsg, odomInfoMsg,
227  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
228  rtabmap::uncompressData(image1Msg->descriptors));
229 }
230 void CommonDataSubscriber::rgbdOdomScanDescCallback(
231  const nav_msgs::OdometryConstPtr & odomMsg,
232  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
233  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
234 {
235  cv_bridge::CvImageConstPtr rgb, depth;
236  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
237 
238  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
239  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
240 
241  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
242  if(!image1Msg->global_descriptor.data.empty())
243  {
244  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
245  }
246  if(!scanDescMsg->global_descriptor.data.empty())
247  {
248  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
249  }
250 
251  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
252  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
253  scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
254  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
255  rtabmap::uncompressData(image1Msg->descriptors));
256 }
257 void CommonDataSubscriber::rgbdOdomInfoCallback(
258  const nav_msgs::OdometryConstPtr & odomMsg,
259  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
260  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
261 {
262  cv_bridge::CvImageConstPtr rgb, depth;
263  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
264 
265  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
266  sensor_msgs::LaserScan scanMsg; // Null
267  sensor_msgs::PointCloud2 scan3dMsg; // Null
268 
269  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
270  if(!image1Msg->global_descriptor.data.empty())
271  {
272  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
273  }
274 
275  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
276  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
277  scanMsg, scan3dMsg, odomInfoMsg,
278  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
279  rtabmap::uncompressData(image1Msg->descriptors));
280 }
281 
282 #ifdef RTABMAP_SYNC_USER_DATA
283 // 1 RGBD camera + User Data
284 void CommonDataSubscriber::rgbdDataCallback(
285  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
286  const rtabmap_msgs::RGBDImageConstPtr& image1Msg)
287 {
288  cv_bridge::CvImageConstPtr rgb, depth;
289  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
290 
291  nav_msgs::OdometryConstPtr odomMsg; // Null
292  sensor_msgs::LaserScan scanMsg; // Null
293  sensor_msgs::PointCloud2 scan3dMsg; // Null
294  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
295 
296  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
297  if(!image1Msg->global_descriptor.data.empty())
298  {
299  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
300  }
301 
302  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
303  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
304  scanMsg, scan3dMsg, odomInfoMsg,
305  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
306  rtabmap::uncompressData(image1Msg->descriptors));
307 }
308 void CommonDataSubscriber::rgbdDataScan2dCallback(
309  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
310  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
311  const sensor_msgs::LaserScanConstPtr& scanMsg)
312 {
313  cv_bridge::CvImageConstPtr rgb, depth;
314  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
315 
316  nav_msgs::OdometryConstPtr odomMsg; // Null
317  sensor_msgs::PointCloud2 scan3dMsg; // Null
318  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
319 
320  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
321  if(!image1Msg->global_descriptor.data.empty())
322  {
323  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
324  }
325 
326  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
327  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
328  *scanMsg, scan3dMsg, odomInfoMsg,
329  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
330  rtabmap::uncompressData(image1Msg->descriptors));
331 }
332 void CommonDataSubscriber::rgbdDataScan3dCallback(
333  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
334  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
335  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
336 {
337  cv_bridge::CvImageConstPtr rgb, depth;
338  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
339 
340  nav_msgs::OdometryConstPtr odomMsg; // Null
341  sensor_msgs::LaserScan scanMsg; // Null
342  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
343 
344  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
345  if(!image1Msg->global_descriptor.data.empty())
346  {
347  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
348  }
349 
350  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
351  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
352  scanMsg, *scan3dMsg, odomInfoMsg,
353  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
354  rtabmap::uncompressData(image1Msg->descriptors));
355 }
356 void CommonDataSubscriber::rgbdDataScanDescCallback(
357  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
358  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
359  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
360 {
361  cv_bridge::CvImageConstPtr rgb, depth;
362  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
363 
364  nav_msgs::OdometryConstPtr odomMsg; // Null
365  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
366 
367  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
368  if(!image1Msg->global_descriptor.data.empty())
369  {
370  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
371  }
372  if(!scanDescMsg->global_descriptor.data.empty())
373  {
374  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
375  }
376 
377  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
378  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
379  scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
380  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
381  rtabmap::uncompressData(image1Msg->descriptors));
382 }
383 void CommonDataSubscriber::rgbdDataInfoCallback(
384  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
385  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
386  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
387 {
388  cv_bridge::CvImageConstPtr rgb, depth;
389  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
390 
391  nav_msgs::OdometryConstPtr odomMsg; // Null
392  sensor_msgs::LaserScan scanMsg; // Null
393  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
394 
395  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
396  if(!image1Msg->global_descriptor.data.empty())
397  {
398  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
399  }
400 
401  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
402  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
403  scanMsg, *scan3dMsg, odomInfoMsg,
404  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
405  rtabmap::uncompressData(image1Msg->descriptors));
406 }
407 
408 // 1 RGBD camera + Odom + User Data
409 void CommonDataSubscriber::rgbdOdomDataCallback(
410  const nav_msgs::OdometryConstPtr & odomMsg,
411  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
412  const rtabmap_msgs::RGBDImageConstPtr& image1Msg)
413 {
414  cv_bridge::CvImageConstPtr rgb, depth;
415  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
416 
417  sensor_msgs::LaserScan scanMsg; // Null
418  sensor_msgs::PointCloud2 scan3dMsg; // Null
419  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
420 
421  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
422  if(!image1Msg->global_descriptor.data.empty())
423  {
424  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
425  }
426 
427  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
428  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
429  scanMsg, scan3dMsg, odomInfoMsg,
430  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
431  rtabmap::uncompressData(image1Msg->descriptors));
432 }
433 void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
434  const nav_msgs::OdometryConstPtr & odomMsg,
435  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
436  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
437  const sensor_msgs::LaserScanConstPtr& scanMsg)
438 {
439  cv_bridge::CvImageConstPtr rgb, depth;
440  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
441 
442  sensor_msgs::PointCloud2 scan3dMsg; // Null
443  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
444 
445  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
446  if(!image1Msg->global_descriptor.data.empty())
447  {
448  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
449  }
450 
451  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
452  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
453  *scanMsg, scan3dMsg, odomInfoMsg,
454  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
455  rtabmap::uncompressData(image1Msg->descriptors));
456 }
457 void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
458  const nav_msgs::OdometryConstPtr & odomMsg,
459  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
460  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
461  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
462 {
463  cv_bridge::CvImageConstPtr rgb, depth;
464  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
465 
466  sensor_msgs::LaserScan scanMsg; // Null
467  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
468 
469  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
470  if(!image1Msg->global_descriptor.data.empty())
471  {
472  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
473  }
474 
475  commonSingleCameraCallback(odomMsg, userDataMsg ,rgb,
476  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
477  scanMsg, *scan3dMsg, odomInfoMsg,
478  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
479  rtabmap::uncompressData(image1Msg->descriptors));
480 }
481 void CommonDataSubscriber::rgbdOdomDataScanDescCallback(
482  const nav_msgs::OdometryConstPtr & odomMsg,
483  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
484  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
485  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
486 {
487  cv_bridge::CvImageConstPtr rgb, depth;
488  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
489 
490  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
491 
492  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
493  if(!image1Msg->global_descriptor.data.empty())
494  {
495  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
496  }
497  if(!scanDescMsg->global_descriptor.data.empty())
498  {
499  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
500  }
501 
502  commonSingleCameraCallback(odomMsg, userDataMsg ,rgb,
503  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
504  scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
505  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
506  rtabmap::uncompressData(image1Msg->descriptors));
507 }
508 void CommonDataSubscriber::rgbdOdomDataInfoCallback(
509  const nav_msgs::OdometryConstPtr & odomMsg,
510  const rtabmap_msgs::UserDataConstPtr & userDataMsg,
511  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
512  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
513 {
514  cv_bridge::CvImageConstPtr rgb, depth;
515  rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
516 
517  sensor_msgs::LaserScan scanMsg; // Null
518  sensor_msgs::PointCloud2 scan3dMsg; // Null
519 
520  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs;
521  if(!image1Msg->global_descriptor.data.empty())
522  {
523  globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
524  }
525 
526  commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
527  depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
528  scanMsg, scan3dMsg, odomInfoMsg,
529  globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
530  rtabmap::uncompressData(image1Msg->descriptors));
531 }
532 #endif
533 
534 void CommonDataSubscriber::setupRGBDCallbacks(
535  ros::NodeHandle & nh,
536  ros::NodeHandle & pnh,
537  bool subscribeOdom,
538  bool subscribeUserData,
539  bool subscribeScan2d,
540  bool subscribeScan3d,
541  bool subscribeScanDesc,
542  bool subscribeOdomInfo)
543 {
544  ROS_INFO("Setup rgbd callback");
545 
546  if(subscribeOdom ||
547 #ifdef RTABMAP_SYNC_USER_DATA
548  subscribeUserData ||
549 #endif
550  subscribeScan2d ||
551  subscribeScan3d ||
552  subscribeScanDesc ||
553  subscribeOdomInfo)
554  {
555  rgbdSubs_.resize(1);
556  rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
557  rgbdSubs_[0]->subscribe(nh, "rgbd_image", topicQueueSize_);
558 
559 #ifdef RTABMAP_SYNC_USER_DATA
560  if(subscribeOdom && subscribeUserData)
561  {
562  odomSub_.subscribe(nh, "odom", topicQueueSize_);
563  userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
564  if(subscribeScanDesc)
565  {
566  subscribedToScanDescriptor_ = true;
567  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
568  if(subscribeOdomInfo)
569  {
570  subscribedToOdomInfo_ = false;
571  ROS_WARN("subscribe_odom_info ignored...");
572  }
573  SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
574  }
575  else if(subscribeScan2d)
576  {
577  subscribedToScan2d_ = true;
578  scanSub_.subscribe(nh, "scan", topicQueueSize_);
579  if(subscribeOdomInfo)
580  {
581  subscribedToOdomInfo_ = false;
582  ROS_WARN("subscribe_odom_info ignored...");
583  }
584  SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
585  }
586  else if(subscribeScan3d)
587  {
588  subscribedToScan3d_ = true;
589  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
590  if(subscribeOdomInfo)
591  {
592  subscribedToOdomInfo_ = false;
593  ROS_WARN("subscribe_odom_info ignored...");
594  }
595  SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
596  }
597  else if(subscribeOdomInfo)
598  {
599  subscribedToOdomInfo_ = true;
600  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
601  SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
602  }
603  else
604  {
605  SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]));
606  }
607  }
608  else
609 #endif
610  if(subscribeOdom)
611  {
612  odomSub_.subscribe(nh, "odom", topicQueueSize_);
613  if(subscribeScanDesc)
614  {
615  subscribedToScanDescriptor_ = true;
616  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
617  if(subscribeOdomInfo)
618  {
619  subscribedToOdomInfo_ = false;
620  ROS_WARN("subscribe_odom_info ignored...");
621  }
622  SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
623  }
624  else if(subscribeScan2d)
625  {
626  subscribedToScan2d_ = true;
627  scanSub_.subscribe(nh, "scan", topicQueueSize_);
628  if(subscribeOdomInfo)
629  {
630  subscribedToOdomInfo_ = false;
631  ROS_WARN("subscribe_odom_info ignored...");
632  }
633  SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanSub_);
634  }
635  else if(subscribeScan3d)
636  {
637  subscribedToScan3d_ = true;
638  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
639  if(subscribeOdomInfo)
640  {
641  subscribedToOdomInfo_ = false;
642  ROS_WARN("subscribe_odom_info ignored...");
643  }
644  SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
645  }
646  else if(subscribeOdomInfo)
647  {
648  subscribedToOdomInfo_ = true;
649  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
650  SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
651  }
652  else
653  {
654  SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]));
655  }
656  }
657 #ifdef RTABMAP_SYNC_USER_DATA
658  else if(subscribeUserData)
659  {
660  userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
661  if(subscribeScanDesc)
662  {
663  subscribedToScanDescriptor_ = true;
664  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
665  if(subscribeOdomInfo)
666  {
667  subscribedToOdomInfo_ = false;
668  ROS_WARN("subscribe_odom_info ignored...");
669  }
670  SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
671  }
672  else if(subscribeScan2d)
673  {
674  subscribedToScan2d_ = true;
675  scanSub_.subscribe(nh, "scan", topicQueueSize_);
676  if(subscribeOdomInfo)
677  {
678  subscribedToOdomInfo_ = false;
679  ROS_WARN("subscribe_odom_info ignored...");
680  }
681  SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
682  }
683  else if(subscribeScan3d)
684  {
685  subscribedToScan3d_ = true;
686  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
687  if(subscribeOdomInfo)
688  {
689  subscribedToOdomInfo_ = false;
690  ROS_WARN("subscribe_odom_info ignored...");
691  }
692  SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
693  }
694  else if(subscribeOdomInfo)
695  {
696  subscribedToOdomInfo_ = true;
697  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
698  SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
699  }
700  else
701  {
702  SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]));
703  }
704  }
705 #endif
706  else
707  {
708  if(subscribeScanDesc)
709  {
710  subscribedToScanDescriptor_ = true;
711  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
712  if(subscribeOdomInfo)
713  {
714  subscribedToOdomInfo_ = false;
715  ROS_WARN("subscribe_odom_info ignored...");
716  }
717  SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanDescSub_);
718  }
719  else if(subscribeScan2d)
720  {
721  subscribedToScan2d_ = true;
722  scanSub_.subscribe(nh, "scan", topicQueueSize_);
723  if(subscribeOdomInfo)
724  {
725  subscribedToOdomInfo_ = false;
726  ROS_WARN("subscribe_odom_info ignored...");
727  }
728  SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanSub_);
729  }
730  else if(subscribeScan3d)
731  {
732  subscribedToScan3d_ = true;
733  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
734  if(subscribeOdomInfo)
735  {
736  subscribedToOdomInfo_ = false;
737  ROS_WARN("subscribe_odom_info ignored...");
738  }
739  SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scan3dSub_);
740  }
741  else if(subscribeOdomInfo)
742  {
743  subscribedToOdomInfo_ = true;
744  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
745  SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_);
746  }
747  else
748  {
749  ROS_FATAL("Not supposed to be here!");
750  }
751  }
752  }
753  else
754  {
755  rgbdSub_ = nh.subscribe("rgbd_image", syncQueueSize_, &CommonDataSubscriber::rgbdCallback, this);
756 
757  subscribedTopicsMsg_ =
758  uFormat("\n%s subscribed to:\n %s",
759  ros::this_node::getName().c_str(),
760  rgbdSub_.getTopic().c_str());
761  }
762 }
763 
764 } /* namespace rtabmap_sync */
SYNC_DECL2
#define SYNC_DECL2(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1)
Definition: CommonDataSubscriberDefines.h:108
boost::shared_ptr
rtabmap_sync
Definition: CommonDataSubscriber.h:59
uFormat
std::string uFormat(const char *fmt,...)
Compression.h
ros::Subscriber::getTopic
std::string getTopic() const
SYNC_DECL3
#define SYNC_DECL3(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2)
Definition: CommonDataSubscriberDefines.h:127
rtabmap_sync::CommonDataSubscriber::commonSingleCameraCallback
void commonSingleCameraCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_msgs::UserDataConstPtr &userDataMsg, const cv_bridge::CvImageConstPtr &imageMsg, const cv_bridge::CvImageConstPtr &depthMsg, const sensor_msgs::CameraInfo &rgbCameraInfoMsg, const sensor_msgs::CameraInfo &depthCameraInfoMsg, const sensor_msgs::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< rtabmap_msgs::KeyPoint > &localKeyPoints=std::vector< rtabmap_msgs::KeyPoint >(), const std::vector< rtabmap_msgs::Point3f > &localPoints3d=std::vector< rtabmap_msgs::Point3f >(), const cv::Mat &localDescriptors=cv::Mat())
Definition: CommonDataSubscriber.cpp:1048
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
rtabmap_sync::CommonDataSubscriber::subscribedTopicsMsg_
std::string subscribedTopicsMsg_
Definition: CommonDataSubscriber.h:244
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::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
rtabmap_sync::CommonDataSubscriber::setupRGBDCallbacks
void setupRGBDCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, bool subscribeOdomInfo)
Definition: CommonDataSubscriberRGBD.cpp:534
ROS_WARN
#define ROS_WARN(...)
rtabmap_sync::CommonDataSubscriber::rgbdSub_
ros::Subscriber rgbdSub_
Definition: CommonDataSubscriber.h:268
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
ROS_FATAL
#define ROS_FATAL(...)
message_filters::Subscriber::subscribe
void subscribe()
ros::this_node::getName
const ROSCPP_DECL std::string & getName()
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
c_str
const char * c_str(Args &&...args)
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::rgbdCallback
void rgbdCallback(const rtabmap_msgs::RGBDImageConstPtr &)
Definition: CommonDataSubscriberRGBD.cpp:37
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
Definition: CommonDataSubscriber.h:61
rtabmap_conversions::toCvShare
void toCvShare(const rtabmap_msgs::RGBDImage &image, const boost::shared_ptr< void const > &trackedObject, cv_bridge::CvImageConstPtr &rgb, cv_bridge::CvImageConstPtr &depth)
ros::NodeHandle
rtabmap::uncompressData
cv::Mat RTABMAP_CORE_EXPORT uncompressData(const cv::Mat &bytes)

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