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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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  commonSingleDepthCallback(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 || subscribeUserData || subscribeScan2d || subscribeScan3d || subscribeOdomInfo)
549  {
550  rgbdSubs_.resize(1);
551  rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
552  rgbdSubs_[0]->subscribe(nh, "rgbd_image", queueSize);
553 
554 #ifdef RTABMAP_SYNC_USER_DATA
555  if(subscribeOdom && subscribeUserData)
556  {
557  odomSub_.subscribe(nh, "odom", queueSize);
558  userDataSub_.subscribe(nh, "user_data", queueSize);
559  if(subscribeScanDesc)
560  {
561  subscribedToScanDescriptor_ = true;
562  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
563  if(subscribeOdomInfo)
564  {
565  subscribedToOdomInfo_ = false;
566  ROS_WARN("subscribe_odom_info ignored...");
567  }
568  SYNC_DECL4(rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
569  }
570  else if(subscribeScan2d)
571  {
572  subscribedToScan2d_ = true;
573  scanSub_.subscribe(nh, "scan", queueSize);
574  if(subscribeOdomInfo)
575  {
576  subscribedToOdomInfo_ = false;
577  ROS_WARN("subscribe_odom_info ignored...");
578  }
579  SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
580  }
581  else if(subscribeScan3d)
582  {
583  subscribedToScan3d_ = true;
584  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
585  if(subscribeOdomInfo)
586  {
587  subscribedToOdomInfo_ = false;
588  ROS_WARN("subscribe_odom_info ignored...");
589  }
590  SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
591  }
592  else if(subscribeOdomInfo)
593  {
594  subscribedToOdomInfo_ = true;
595  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
596  SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
597  }
598  else
599  {
600  SYNC_DECL3(rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
601  }
602  }
603  else
604 #endif
605  if(subscribeOdom)
606  {
607  odomSub_.subscribe(nh, "odom", queueSize);
608  if(subscribeScanDesc)
609  {
610  subscribedToScanDescriptor_ = true;
611  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
612  if(subscribeOdomInfo)
613  {
614  subscribedToOdomInfo_ = false;
615  ROS_WARN("subscribe_odom_info ignored...");
616  }
617  SYNC_DECL3(rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
618  }
619  else if(subscribeScan2d)
620  {
621  subscribedToScan2d_ = true;
622  scanSub_.subscribe(nh, "scan", queueSize);
623  if(subscribeOdomInfo)
624  {
625  subscribedToOdomInfo_ = false;
626  ROS_WARN("subscribe_odom_info ignored...");
627  }
628  SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
629  }
630  else if(subscribeScan3d)
631  {
632  subscribedToScan3d_ = true;
633  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
634  if(subscribeOdomInfo)
635  {
636  subscribedToOdomInfo_ = false;
637  ROS_WARN("subscribe_odom_info ignored...");
638  }
639  SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
640  }
641  else if(subscribeOdomInfo)
642  {
643  subscribedToOdomInfo_ = true;
644  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
645  SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
646  }
647  else
648  {
649  SYNC_DECL2(rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
650  }
651  }
652 #ifdef RTABMAP_SYNC_USER_DATA
653  else if(subscribeUserData)
654  {
655  userDataSub_.subscribe(nh, "user_data", queueSize);
656  if(subscribeScanDesc)
657  {
658  subscribedToScanDescriptor_ = true;
659  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
660  if(subscribeOdomInfo)
661  {
662  subscribedToOdomInfo_ = false;
663  ROS_WARN("subscribe_odom_info ignored...");
664  }
665  SYNC_DECL3(rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
666  }
667  else if(subscribeScan2d)
668  {
669  subscribedToScan2d_ = true;
670  scanSub_.subscribe(nh, "scan", queueSize);
671  if(subscribeOdomInfo)
672  {
673  subscribedToOdomInfo_ = false;
674  ROS_WARN("subscribe_odom_info ignored...");
675  }
676  SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
677  }
678  else if(subscribeScan3d)
679  {
680  subscribedToScan3d_ = true;
681  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
682  if(subscribeOdomInfo)
683  {
684  subscribedToOdomInfo_ = false;
685  ROS_WARN("subscribe_odom_info ignored...");
686  }
687  SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
688  }
689  else if(subscribeOdomInfo)
690  {
691  subscribedToOdomInfo_ = true;
692  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
693  SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
694  }
695  else
696  {
697  SYNC_DECL2(rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
698  }
699  }
700 #endif
701  else
702  {
703  if(subscribeScanDesc)
704  {
705  subscribedToScanDescriptor_ = true;
706  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
707  if(subscribeOdomInfo)
708  {
709  subscribedToOdomInfo_ = false;
710  ROS_WARN("subscribe_odom_info ignored...");
711  }
712  SYNC_DECL2(rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
713  }
714  else if(subscribeScan2d)
715  {
716  subscribedToScan2d_ = true;
717  scanSub_.subscribe(nh, "scan", queueSize);
718  if(subscribeOdomInfo)
719  {
720  subscribedToOdomInfo_ = false;
721  ROS_WARN("subscribe_odom_info ignored...");
722  }
723  SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
724  }
725  else if(subscribeScan3d)
726  {
727  subscribedToScan3d_ = true;
728  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
729  if(subscribeOdomInfo)
730  {
731  subscribedToOdomInfo_ = false;
732  ROS_WARN("subscribe_odom_info ignored...");
733  }
734  SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
735  }
736  else if(subscribeOdomInfo)
737  {
738  subscribedToOdomInfo_ = true;
739  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
740  SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
741  }
742  else
743  {
744  ROS_FATAL("Not supposed to be here!");
745  }
746  }
747  }
748  else
749  {
750  rgbdSub_ = nh.subscribe("rgbd_image", queueSize, &CommonDataSubscriber::rgbdCallback, this);
751 
752  subscribedTopicsMsg_ =
753  uFormat("\n%s subscribed to:\n %s",
754  ros::this_node::getName().c_str(),
755  rgbdSub_.getTopic().c_str());
756  }
757 }
758 
759 } /* 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::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())
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
SYNC_DECL3
#define SYNC_DECL3(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2)
Definition: CommonDataSubscriberDefines.h:127
rtabmap_ros::CommonDataSubscriber::userDataSub_
message_filters::Subscriber< rtabmap_ros::UserData > userDataSub_
Definition: CommonDataSubscriber.h:289
rtabmap_ros::CommonDataSubscriber::subscribedToOdomInfo_
bool subscribedToOdomInfo_
Definition: CommonDataSubscriber.h:270
rtabmap_ros
Definition: CommonDataSubscriber.h:56
ROS_WARN
#define ROS_WARN(...)
rtabmap_ros::CommonDataSubscriber::odomInfoSub_
message_filters::Subscriber< rtabmap_ros::OdomInfo > odomInfoSub_
Definition: CommonDataSubscriber.h:293
boost::shared_ptr
rtabmap_ros::CommonDataSubscriber::subscribedToScan2d_
bool subscribedToScan2d_
Definition: CommonDataSubscriber.h:267
cv_bridge.h
rtabmap_ros::CommonDataSubscriber::commonSingleDepthCallback
void commonSingleDepthCallback(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:956
ROS_INFO
#define ROS_INFO(...)
rtabmap_ros::CommonDataSubscriber::subscribedToScanDescriptor_
bool subscribedToScanDescriptor_
Definition: CommonDataSubscriber.h:269
ros::Subscriber::getTopic
std::string getTopic() const
SYNC_DECL2
#define SYNC_DECL2(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1)
Definition: CommonDataSubscriberDefines.h:108
rtabmap_ros::CommonDataSubscriber::scanSub_
message_filters::Subscriber< sensor_msgs::LaserScan > scanSub_
Definition: CommonDataSubscriber.h:290
rtabmap_ros::CommonDataSubscriber::scanDescSub_
message_filters::Subscriber< rtabmap_ros::ScanDescriptor > scanDescSub_
Definition: CommonDataSubscriber.h:292
SYNC_DECL4
#define SYNC_DECL4(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3)
Definition: CommonDataSubscriberDefines.h:147
rtabmap_ros::CommonDataSubscriber::odomSub_
message_filters::Subscriber< nav_msgs::Odometry > odomSub_
Definition: CommonDataSubscriber.h:288
rtabmap_ros::CommonDataSubscriber::subscribedToScan3d_
bool subscribedToScan3d_
Definition: CommonDataSubscriber.h:268
rtabmap_ros::CommonDataSubscriber::scan3dSub_
message_filters::Subscriber< sensor_msgs::PointCloud2 > scan3dSub_
Definition: CommonDataSubscriber.h:291
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:164
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:255
rtabmap_ros::CommonDataSubscriber::rgbdSub_
ros::Subscriber rgbdSub_
Definition: CommonDataSubscriber.h:279
rtabmap_ros::CommonDataSubscriber::rgbdSubs_
std::vector< message_filters::Subscriber< rtabmap_ros::RGBDImage > * > rgbdSubs_
Definition: CommonDataSubscriber.h:280

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:18