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_ros/CommonDataSubscriber.h>
29 #include <rtabmap/utilite/UConversion.h>
30 #include <rtabmap/core/Compression.h>
31 #include <rtabmap_ros/MsgConversion.h>
32 #include <cv_bridge/cv_bridge.h>
33 
34 namespace rtabmap_ros {
35 
36 // 1 RGBD camera
37 void CommonDataSubscriber::rgbdCallback(
38  const rtabmap_ros::RGBDImageConstPtr& image1Msg)
39 {
40  cv_bridge::CvImageConstPtr rgb, depth;
41  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
42 
43  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
44  nav_msgs::OdometryConstPtr odomMsg; // Null
45  sensor_msgs::LaserScan scanMsg; // Null
46  sensor_msgs::PointCloud2 scan3dMsg; // Null
47  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
48 
49  std::vector<rtabmap_ros::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_ros::RGBDImageConstPtr& image1Msg,
63  const sensor_msgs::LaserScanConstPtr& scanMsg)
64 {
65  cv_bridge::CvImageConstPtr rgb, depth;
66  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
67 
68  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
69  nav_msgs::OdometryConstPtr odomMsg; // Null
70  sensor_msgs::PointCloud2 scan3dMsg; // Null
71  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
72 
73  std::vector<rtabmap_ros::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_ros::RGBDImageConstPtr& image1Msg,
87  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
88 {
89  cv_bridge::CvImageConstPtr rgb, depth;
90  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
91 
92  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
93  nav_msgs::OdometryConstPtr odomMsg; // Null
94  sensor_msgs::LaserScan scanMsg; // Null
95  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
96 
97  std::vector<rtabmap_ros::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_ros::RGBDImageConstPtr& image1Msg,
111  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
112 {
113  cv_bridge::CvImageConstPtr rgb, depth;
114  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
115 
116  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
117  nav_msgs::OdometryConstPtr odomMsg; // Null
118  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
119 
120  std::vector<rtabmap_ros::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_ros::RGBDImageConstPtr& image1Msg,
134  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
135 {
136  cv_bridge::CvImageConstPtr rgb, depth;
137  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
138 
139  rtabmap_ros::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_ros::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_ros::RGBDImageConstPtr& image1Msg)
161 {
162  cv_bridge::CvImageConstPtr rgb, depth;
163  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
164 
165  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
166  sensor_msgs::LaserScan scanMsg; // Null
167  sensor_msgs::PointCloud2 scan3dMsg; // Null
168  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
169 
170  std::vector<rtabmap_ros::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_ros::RGBDImageConstPtr& image1Msg,
185  const sensor_msgs::LaserScanConstPtr& scanMsg)
186 {
187  cv_bridge::CvImageConstPtr rgb, depth;
188  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
189 
190  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
191  sensor_msgs::PointCloud2 scan3dMsg; // Null
192  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
193 
194  std::vector<rtabmap_ros::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_ros::RGBDImageConstPtr& image1Msg,
209  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
210 {
211  cv_bridge::CvImageConstPtr rgb, depth;
212  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
213 
214  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
215  sensor_msgs::LaserScan scanMsg; // Null
216  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
217 
218  std::vector<rtabmap_ros::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_ros::RGBDImageConstPtr& image1Msg,
233  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
234 {
235  cv_bridge::CvImageConstPtr rgb, depth;
236  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
237 
238  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
239  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
240 
241  std::vector<rtabmap_ros::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_ros::RGBDImageConstPtr& image1Msg,
260  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
261 {
262  cv_bridge::CvImageConstPtr rgb, depth;
263  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
264 
265  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
266  sensor_msgs::LaserScan scanMsg; // Null
267  sensor_msgs::PointCloud2 scan3dMsg; // Null
268 
269  std::vector<rtabmap_ros::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_ros::UserDataConstPtr & userDataMsg,
286  const rtabmap_ros::RGBDImageConstPtr& image1Msg)
287 {
288  cv_bridge::CvImageConstPtr rgb, depth;
289  rtabmap_ros::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_ros::OdomInfoConstPtr odomInfoMsg; // null
295 
296  std::vector<rtabmap_ros::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_ros::UserDataConstPtr & userDataMsg,
310  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
311  const sensor_msgs::LaserScanConstPtr& scanMsg)
312 {
313  cv_bridge::CvImageConstPtr rgb, depth;
314  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
315 
316  nav_msgs::OdometryConstPtr odomMsg; // Null
317  sensor_msgs::PointCloud2 scan3dMsg; // Null
318  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
319 
320  std::vector<rtabmap_ros::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_ros::UserDataConstPtr & userDataMsg,
334  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
335  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
336 {
337  cv_bridge::CvImageConstPtr rgb, depth;
338  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
339 
340  nav_msgs::OdometryConstPtr odomMsg; // Null
341  sensor_msgs::LaserScan scanMsg; // Null
342  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
343 
344  std::vector<rtabmap_ros::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_ros::UserDataConstPtr & userDataMsg,
358  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
359  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
360 {
361  cv_bridge::CvImageConstPtr rgb, depth;
362  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
363 
364  nav_msgs::OdometryConstPtr odomMsg; // Null
365  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
366 
367  std::vector<rtabmap_ros::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_ros::UserDataConstPtr & userDataMsg,
385  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
386  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
387 {
388  cv_bridge::CvImageConstPtr rgb, depth;
389  rtabmap_ros::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_ros::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_ros::UserDataConstPtr & userDataMsg,
412  const rtabmap_ros::RGBDImageConstPtr& image1Msg)
413 {
414  cv_bridge::CvImageConstPtr rgb, depth;
415  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
416 
417  sensor_msgs::LaserScan scanMsg; // Null
418  sensor_msgs::PointCloud2 scan3dMsg; // Null
419  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
420 
421  std::vector<rtabmap_ros::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_ros::UserDataConstPtr & userDataMsg,
436  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
437  const sensor_msgs::LaserScanConstPtr& scanMsg)
438 {
439  cv_bridge::CvImageConstPtr rgb, depth;
440  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
441 
442  sensor_msgs::PointCloud2 scan3dMsg; // Null
443  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
444 
445  std::vector<rtabmap_ros::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_ros::UserDataConstPtr & userDataMsg,
460  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
461  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
462 {
463  cv_bridge::CvImageConstPtr rgb, depth;
464  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
465 
466  sensor_msgs::LaserScan scanMsg; // Null
467  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
468 
469  std::vector<rtabmap_ros::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_ros::UserDataConstPtr & userDataMsg,
484  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
485  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
486 {
487  cv_bridge::CvImageConstPtr rgb, depth;
488  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
489 
490  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
491 
492  std::vector<rtabmap_ros::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_ros::UserDataConstPtr & userDataMsg,
511  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
512  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
513 {
514  cv_bridge::CvImageConstPtr rgb, depth;
515  rtabmap_ros::toCvShare(image1Msg, rgb, depth);
516 
517  sensor_msgs::LaserScan scanMsg; // Null
518  sensor_msgs::PointCloud2 scan3dMsg; // Null
519 
520  std::vector<rtabmap_ros::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  int queueSize,
544  bool approxSync)
545 {
546  ROS_INFO("Setup rgbd callback");
547 
548  if(subscribeOdom ||
549 #ifdef RTABMAP_SYNC_USER_DATA
550  subscribeUserData ||
551 #endif
552  subscribeScan2d ||
553  subscribeScan3d ||
554  subscribeScanDesc ||
555  subscribeOdomInfo)
556  {
557  rgbdSubs_.resize(1);
558  rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
559  rgbdSubs_[0]->subscribe(nh, "rgbd_image", queueSize);
560 
561 #ifdef RTABMAP_SYNC_USER_DATA
562  if(subscribeOdom && subscribeUserData)
563  {
564  odomSub_.subscribe(nh, "odom", queueSize);
565  userDataSub_.subscribe(nh, "user_data", queueSize);
566  if(subscribeScanDesc)
567  {
568  subscribedToScanDescriptor_ = true;
569  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
570  if(subscribeOdomInfo)
571  {
572  subscribedToOdomInfo_ = false;
573  ROS_WARN("subscribe_odom_info ignored...");
574  }
575  SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
576  }
577  else if(subscribeScan2d)
578  {
579  subscribedToScan2d_ = true;
580  scanSub_.subscribe(nh, "scan", queueSize);
581  if(subscribeOdomInfo)
582  {
583  subscribedToOdomInfo_ = false;
584  ROS_WARN("subscribe_odom_info ignored...");
585  }
586  SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
587  }
588  else if(subscribeScan3d)
589  {
590  subscribedToScan3d_ = true;
591  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
592  if(subscribeOdomInfo)
593  {
594  subscribedToOdomInfo_ = false;
595  ROS_WARN("subscribe_odom_info ignored...");
596  }
597  SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
598  }
599  else if(subscribeOdomInfo)
600  {
601  subscribedToOdomInfo_ = true;
602  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
603  SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
604  }
605  else
606  {
607  SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
608  }
609  }
610  else
611 #endif
612  if(subscribeOdom)
613  {
614  odomSub_.subscribe(nh, "odom", queueSize);
615  if(subscribeScanDesc)
616  {
617  subscribedToScanDescriptor_ = true;
618  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
619  if(subscribeOdomInfo)
620  {
621  subscribedToOdomInfo_ = false;
622  ROS_WARN("subscribe_odom_info ignored...");
623  }
624  SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
625  }
626  else if(subscribeScan2d)
627  {
628  subscribedToScan2d_ = true;
629  scanSub_.subscribe(nh, "scan", queueSize);
630  if(subscribeOdomInfo)
631  {
632  subscribedToOdomInfo_ = false;
633  ROS_WARN("subscribe_odom_info ignored...");
634  }
635  SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
636  }
637  else if(subscribeScan3d)
638  {
639  subscribedToScan3d_ = true;
640  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
641  if(subscribeOdomInfo)
642  {
643  subscribedToOdomInfo_ = false;
644  ROS_WARN("subscribe_odom_info ignored...");
645  }
646  SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
647  }
648  else if(subscribeOdomInfo)
649  {
650  subscribedToOdomInfo_ = true;
651  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
652  SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
653  }
654  else
655  {
656  SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
657  }
658  }
659 #ifdef RTABMAP_SYNC_USER_DATA
660  else if(subscribeUserData)
661  {
662  userDataSub_.subscribe(nh, "user_data", queueSize);
663  if(subscribeScanDesc)
664  {
665  subscribedToScanDescriptor_ = true;
666  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
667  if(subscribeOdomInfo)
668  {
669  subscribedToOdomInfo_ = false;
670  ROS_WARN("subscribe_odom_info ignored...");
671  }
672  SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
673  }
674  else if(subscribeScan2d)
675  {
676  subscribedToScan2d_ = true;
677  scanSub_.subscribe(nh, "scan", queueSize);
678  if(subscribeOdomInfo)
679  {
680  subscribedToOdomInfo_ = false;
681  ROS_WARN("subscribe_odom_info ignored...");
682  }
683  SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
684  }
685  else if(subscribeScan3d)
686  {
687  subscribedToScan3d_ = true;
688  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
689  if(subscribeOdomInfo)
690  {
691  subscribedToOdomInfo_ = false;
692  ROS_WARN("subscribe_odom_info ignored...");
693  }
694  SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
695  }
696  else if(subscribeOdomInfo)
697  {
698  subscribedToOdomInfo_ = true;
699  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
700  SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
701  }
702  else
703  {
704  SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
705  }
706  }
707 #endif
708  else
709  {
710  if(subscribeScanDesc)
711  {
712  subscribedToScanDescriptor_ = true;
713  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
714  if(subscribeOdomInfo)
715  {
716  subscribedToOdomInfo_ = false;
717  ROS_WARN("subscribe_odom_info ignored...");
718  }
719  SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
720  }
721  else if(subscribeScan2d)
722  {
723  subscribedToScan2d_ = true;
724  scanSub_.subscribe(nh, "scan", queueSize);
725  if(subscribeOdomInfo)
726  {
727  subscribedToOdomInfo_ = false;
728  ROS_WARN("subscribe_odom_info ignored...");
729  }
730  SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
731  }
732  else if(subscribeScan3d)
733  {
734  subscribedToScan3d_ = true;
735  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
736  if(subscribeOdomInfo)
737  {
738  subscribedToOdomInfo_ = false;
739  ROS_WARN("subscribe_odom_info ignored...");
740  }
741  SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
742  }
743  else if(subscribeOdomInfo)
744  {
745  subscribedToOdomInfo_ = true;
746  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
747  SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
748  }
749  else
750  {
751  ROS_FATAL("Not supposed to be here!");
752  }
753  }
754  }
755  else
756  {
757  rgbdSub_ = nh.subscribe("rgbd_image", queueSize, &CommonDataSubscriber::rgbdCallback, this);
758 
759  subscribedTopicsMsg_ =
760  uFormat("\n%s subscribed to:\n %s",
761  ros::this_node::getName().c_str(),
762  rgbdSub_.getTopic().c_str());
763  }
764 }
765 
766 } /* namespace rtabmap_ros */
rtabmap_ros::CommonDataSubscriber::rgbdCallback
void rgbdCallback(const rtabmap_ros::RGBDImageConstPtr &)
Definition: CommonDataSubscriberRGBD.cpp:37
rtabmap::uncompressData
cv::Mat RTABMAP_EXP uncompressData(const cv::Mat &bytes)
uFormat
std::string uFormat(const char *fmt,...)
ROS_FATAL
#define ROS_FATAL(...)
ros::Subscriber::getTopic
std::string getTopic() const
ros::NodeHandle
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
SYNC_DECL3
#define SYNC_DECL3(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2)
Definition: CommonDataSubscriberDefines.h:127
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
rtabmap_ros::CommonDataSubscriber::userDataSub_
message_filters::Subscriber< rtabmap_ros::UserData > userDataSub_
Definition: CommonDataSubscriber.h:290
rtabmap_ros::CommonDataSubscriber::subscribedToOdomInfo_
bool subscribedToOdomInfo_
Definition: CommonDataSubscriber.h:269
rtabmap_ros
Definition: CommonDataSubscriber.h:57
ROS_WARN
#define ROS_WARN(...)
rtabmap_ros::CommonDataSubscriber::odomInfoSub_
message_filters::Subscriber< rtabmap_ros::OdomInfo > odomInfoSub_
Definition: CommonDataSubscriber.h:294
boost::shared_ptr
rtabmap_ros::CommonDataSubscriber::subscribedToScan2d_
bool subscribedToScan2d_
Definition: CommonDataSubscriber.h:266
cv_bridge.h
ROS_INFO
#define ROS_INFO(...)
rtabmap_ros::CommonDataSubscriber::subscribedToScanDescriptor_
bool subscribedToScanDescriptor_
Definition: CommonDataSubscriber.h:268
rtabmap_ros::CommonDataSubscriber::scanSub_
message_filters::Subscriber< sensor_msgs::LaserScan > scanSub_
Definition: CommonDataSubscriber.h:291
rtabmap_ros::CommonDataSubscriber::scanDescSub_
message_filters::Subscriber< rtabmap_ros::ScanDescriptor > scanDescSub_
Definition: CommonDataSubscriber.h:293
rtabmap_ros::CommonDataSubscriber::odomSub_
message_filters::Subscriber< nav_msgs::Odometry > odomSub_
Definition: CommonDataSubscriber.h:289
rtabmap_ros::CommonDataSubscriber::subscribedToScan3d_
bool subscribedToScan3d_
Definition: CommonDataSubscriber.h:267
rtabmap_ros::CommonDataSubscriber::scan3dSub_
message_filters::Subscriber< sensor_msgs::PointCloud2 > scan3dSub_
Definition: CommonDataSubscriber.h:292
message_filters::Subscriber::subscribe
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
rtabmap_ros::toCvShare
void toCvShare(const rtabmap_ros::RGBDImageConstPtr &image, cv_bridge::CvImageConstPtr &rgb, cv_bridge::CvImageConstPtr &depth)
Definition: MsgConversion.cpp:166
rtabmap_ros::CommonDataSubscriber::setupRGBDCallbacks
void setupRGBDCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, bool subscribeOdomInfo, int queueSize, bool approxSync)
Definition: CommonDataSubscriberRGBD.cpp:534
message_filters::Subscriber< rtabmap_ros::RGBDImage >
rtabmap_ros::CommonDataSubscriber::subscribedTopicsMsg_
std::string subscribedTopicsMsg_
Definition: CommonDataSubscriber.h:254
rtabmap_ros::CommonDataSubscriber
Definition: CommonDataSubscriber.h:59
rtabmap_ros::CommonDataSubscriber::rgbdSub_
ros::Subscriber rgbdSub_
Definition: CommonDataSubscriber.h:278
rtabmap_ros::CommonDataSubscriber::rgbdSubs_
std::vector< message_filters::Subscriber< rtabmap_ros::RGBDImage > * > rgbdSubs_
Definition: CommonDataSubscriber.h:279
SYNC_DECL4
#define SYNC_DECL4(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3)
Definition: CommonDataSubscriberDefines.h:147
SYNC_DECL2
#define SYNC_DECL2(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1)
Definition: CommonDataSubscriberDefines.h:108
rtabmap_ros::CommonDataSubscriber::commonSingleCameraCallback
void commonSingleCameraCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_ros::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_ros::OdomInfoConstPtr &odomInfoMsg, const std::vector< rtabmap_ros::GlobalDescriptor > &globalDescriptorMsgs=std::vector< rtabmap_ros::GlobalDescriptor >(), const std::vector< rtabmap_ros::KeyPoint > &localKeyPoints=std::vector< rtabmap_ros::KeyPoint >(), const std::vector< rtabmap_ros::Point3f > &localPoints3d=std::vector< rtabmap_ros::Point3f >(), const cv::Mat &localDescriptors=cv::Mat())
Definition: CommonDataSubscriber.cpp:1014

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40