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

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