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