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<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
50  std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
51  std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
52  std::vector<cv::Mat> localDescriptors; \
53  if(!image1Msg->global_descriptor.data.empty()) \
54  globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
55  if(!image2Msg->global_descriptor.data.empty()) \
56  globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
57  if(!image3Msg->global_descriptor.data.empty()) \
58  globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
59  localKeyPoints.push_back(image1Msg->key_points); \
60  localKeyPoints.push_back(image2Msg->key_points); \
61  localKeyPoints.push_back(image3Msg->key_points); \
62  localPoints3d.push_back(image1Msg->points); \
63  localPoints3d.push_back(image2Msg->points); \
64  localPoints3d.push_back(image3Msg->points); \
65  localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
66  localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
67  localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors));
68 
69 // 3 RGBD
70 void CommonDataSubscriber::rgbd3Callback(
71  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
72  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
73  const rtabmap_ros::RGBDImageConstPtr& image3Msg)
74 {
75  IMAGE_CONVERSION();
76 
77  nav_msgs::OdometryConstPtr odomMsg; // Null
78  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
79  sensor_msgs::LaserScan scanMsg; // Null
80  sensor_msgs::PointCloud2 scan3dMsg; // Null
81  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
82  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
83  depthMsgs, cameraInfoMsgs, scanMsg,
84  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
85  localKeyPoints, localPoints3d, localDescriptors);
86 }
87 void CommonDataSubscriber::rgbd3Scan2dCallback(
88  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
89  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
90  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
91  const sensor_msgs::LaserScanConstPtr& scanMsg)
92 {
93  IMAGE_CONVERSION();
94 
95  nav_msgs::OdometryConstPtr odomMsg; // Null
96  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
97  sensor_msgs::PointCloud2 scan3dMsg; // Null
98  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
99  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
100  depthMsgs, cameraInfoMsgs, *scanMsg,
101  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
102  localKeyPoints, localPoints3d, localDescriptors);
103 }
104 void CommonDataSubscriber::rgbd3Scan3dCallback(
105  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
106  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
107  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
108  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
109 {
110  IMAGE_CONVERSION();
111 
112  nav_msgs::OdometryConstPtr odomMsg; // Null
113  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
114  sensor_msgs::LaserScan scanMsg; // Null
115  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
116  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
117  depthMsgs, cameraInfoMsgs, scanMsg,
118  *scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
119  localKeyPoints, localPoints3d, localDescriptors);
120 }
121 void CommonDataSubscriber::rgbd3ScanDescCallback(
122  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
123  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
124  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
125  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
126 {
127  IMAGE_CONVERSION();
128 
129  nav_msgs::OdometryConstPtr odomMsg; // Null
130  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
131  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
132  if(!scanDescMsg->global_descriptor.data.empty())
133  {
134  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
135  }
136  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
137  depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
138  scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
139  localKeyPoints, localPoints3d, localDescriptors);
140 }
141 void CommonDataSubscriber::rgbd3InfoCallback(
142  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
143  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
144  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
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,
154  depthMsgs, cameraInfoMsgs, scanMsg,
155  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
156  localKeyPoints, localPoints3d, localDescriptors);
157 }
158 
159 // 2 RGBD + Odom
160 void CommonDataSubscriber::rgbd3OdomCallback(
161  const nav_msgs::OdometryConstPtr & odomMsg,
162  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
163  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
164  const rtabmap_ros::RGBDImageConstPtr& image3Msg)
165 {
166  IMAGE_CONVERSION();
167 
168  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
169  sensor_msgs::LaserScan scanMsg; // Null
170  sensor_msgs::PointCloud2 scan3dMsg; // Null
171  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
172  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
173  depthMsgs, cameraInfoMsgs, scanMsg,
174  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
175  localKeyPoints, localPoints3d, localDescriptors);
176 }
177 void CommonDataSubscriber::rgbd3OdomScan2dCallback(
178  const nav_msgs::OdometryConstPtr & odomMsg,
179  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
180  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
181  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
182  const sensor_msgs::LaserScanConstPtr& scanMsg)
183 {
184  IMAGE_CONVERSION();
185 
186  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
187  sensor_msgs::PointCloud2 scan3dMsg; // Null
188  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
189  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
190  depthMsgs, cameraInfoMsgs, *scanMsg,
191  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
192  localKeyPoints, localPoints3d, localDescriptors);
193 }
194 void CommonDataSubscriber::rgbd3OdomScan3dCallback(
195  const nav_msgs::OdometryConstPtr & odomMsg,
196  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
197  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
198  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
199  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
200 {
201  IMAGE_CONVERSION();
202 
203  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
204  sensor_msgs::LaserScan scanMsg; // Null
205  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
206  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
207  depthMsgs, cameraInfoMsgs, scanMsg,
208  *scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
209  localKeyPoints, localPoints3d, localDescriptors);
210 }
211 void CommonDataSubscriber::rgbd3OdomScanDescCallback(
212  const nav_msgs::OdometryConstPtr & odomMsg,
213  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
214  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
215  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
216  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
217 {
218  IMAGE_CONVERSION();
219 
220  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
221  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
222  if(!scanDescMsg->global_descriptor.data.empty())
223  {
224  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
225  }
226  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
227  depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
228  scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
229  localKeyPoints, localPoints3d, localDescriptors);
230 }
231 void CommonDataSubscriber::rgbd3OdomInfoCallback(
232  const nav_msgs::OdometryConstPtr & odomMsg,
233  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
234  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
235  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
236  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
237 {
238  IMAGE_CONVERSION();
239 
240  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
241  sensor_msgs::LaserScan scanMsg; // Null
242  sensor_msgs::PointCloud2 scan3dMsg; // Null
243  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
244  depthMsgs, cameraInfoMsgs, scanMsg,
245  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
246  localKeyPoints, localPoints3d, localDescriptors);
247 }
248 
249 #ifdef RTABMAP_SYNC_USER_DATA
250 // 2 RGBD + User Data
251 void CommonDataSubscriber::rgbd3DataCallback(
252  const rtabmap_ros::UserDataConstPtr& userDataMsg,
253  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
254  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
255  const rtabmap_ros::RGBDImageConstPtr& image3Msg)
256 {
257  IMAGE_CONVERSION();
258 
259  nav_msgs::OdometryConstPtr odomMsg; // Null
260  sensor_msgs::LaserScan scanMsg; // Null
261  sensor_msgs::PointCloud2 scan3dMsg; // Null
262  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
263  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
264  depthMsgs, cameraInfoMsgs, scanMsg,
265  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
266  localKeyPoints, localPoints3d, localDescriptors);
267 }
268 void CommonDataSubscriber::rgbd3DataScan2dCallback(
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 sensor_msgs::LaserScanConstPtr& scanMsg)
274 {
275  IMAGE_CONVERSION();
276 
277  nav_msgs::OdometryConstPtr odomMsg; // Null
278  sensor_msgs::PointCloud2 scan3dMsg; // Null
279  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
280  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
281  depthMsgs, cameraInfoMsgs, *scanMsg,
282  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
283  localKeyPoints, localPoints3d, localDescriptors);
284 }
285 void CommonDataSubscriber::rgbd3DataScan3dCallback(
286  const rtabmap_ros::UserDataConstPtr& userDataMsg,
287  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
288  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
289  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
290  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
291 {
292  IMAGE_CONVERSION();
293 
294  nav_msgs::OdometryConstPtr odomMsg; // Null
295  sensor_msgs::LaserScan scanMsg; // Null
296  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
297  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
298  depthMsgs, cameraInfoMsgs, scanMsg,
299  *scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
300  localKeyPoints, localPoints3d, localDescriptors);
301 }
302 void CommonDataSubscriber::rgbd3DataScanDescCallback(
303  const rtabmap_ros::UserDataConstPtr& userDataMsg,
304  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
305  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
306  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
307  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
308 {
309  IMAGE_CONVERSION();
310 
311  nav_msgs::OdometryConstPtr odomMsg; // Null
312  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
313  if(!scanDescMsg->global_descriptor.data.empty())
314  {
315  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
316  }
317  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
318  depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
319  scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
320  localKeyPoints, localPoints3d, localDescriptors);
321 }
322 void CommonDataSubscriber::rgbd3DataInfoCallback(
323  const rtabmap_ros::UserDataConstPtr& userDataMsg,
324  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
325  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
326  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
327  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
328 {
329  IMAGE_CONVERSION();
330 
331  nav_msgs::OdometryConstPtr odomMsg; // Null
332  sensor_msgs::LaserScan scanMsg; // Null
333  sensor_msgs::PointCloud2 scan3dMsg; // Null
334  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
335  depthMsgs, cameraInfoMsgs, scanMsg,
336  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
337  localKeyPoints, localPoints3d, localDescriptors);
338 }
339 // 2 RGBD + Odom + User Data
340 void CommonDataSubscriber::rgbd3OdomDataCallback(
341  const nav_msgs::OdometryConstPtr& odomMsg,
342  const rtabmap_ros::UserDataConstPtr& userDataMsg,
343  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
344  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
345  const rtabmap_ros::RGBDImageConstPtr& image3Msg)
346 {
347  IMAGE_CONVERSION();
348 
349  sensor_msgs::LaserScan scanMsg; // Null
350  sensor_msgs::PointCloud2 scan3dMsg; // Null
351  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
352  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
353  depthMsgs, cameraInfoMsgs, scanMsg,
354  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
355  localKeyPoints, localPoints3d, localDescriptors);
356 }
357 void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
358  const nav_msgs::OdometryConstPtr& odomMsg,
359  const rtabmap_ros::UserDataConstPtr& userDataMsg,
360  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
361  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
362  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
363  const sensor_msgs::LaserScanConstPtr& scanMsg)
364 {
365  IMAGE_CONVERSION();
366 
367  sensor_msgs::PointCloud2 scan3dMsg; // Null
368  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
369  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
370  depthMsgs, cameraInfoMsgs, *scanMsg,
371  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
372  localKeyPoints, localPoints3d, localDescriptors);
373 }
374 void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
375  const nav_msgs::OdometryConstPtr& odomMsg,
376  const rtabmap_ros::UserDataConstPtr& userDataMsg,
377  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
378  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
379  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
380  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
381 {
382  IMAGE_CONVERSION();
383 
384  sensor_msgs::LaserScan scanMsg; // Null
385  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
386  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
387  depthMsgs, cameraInfoMsgs, scanMsg,
388  *scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
389  localKeyPoints, localPoints3d, localDescriptors);
390 }
391 void CommonDataSubscriber::rgbd3OdomDataScanDescCallback(
392  const nav_msgs::OdometryConstPtr& odomMsg,
393  const rtabmap_ros::UserDataConstPtr& userDataMsg,
394  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
395  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
396  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
397  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
398 {
399  IMAGE_CONVERSION();
400 
401  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
402  if(!scanDescMsg->global_descriptor.data.empty())
403  {
404  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
405  }
406  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
407  depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
408  scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
409  localKeyPoints, localPoints3d, localDescriptors);
410 }
411 void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
412  const nav_msgs::OdometryConstPtr& odomMsg,
413  const rtabmap_ros::UserDataConstPtr& userDataMsg,
414  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
415  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
416  const rtabmap_ros::RGBDImageConstPtr& image3Msg,
417  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
418 {
419  IMAGE_CONVERSION();
420 
421  sensor_msgs::LaserScan scanMsg; // Null
422  sensor_msgs::PointCloud2 scan3dMsg; // Null
423  commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
424  depthMsgs, cameraInfoMsgs, scanMsg,
425  scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
426  localKeyPoints, localPoints3d, localDescriptors);
427 }
428 #endif
429 
430 void CommonDataSubscriber::setupRGBD3Callbacks(
431  ros::NodeHandle & nh,
432  ros::NodeHandle & pnh,
433  bool subscribeOdom,
434  bool subscribeUserData,
435  bool subscribeScan2d,
436  bool subscribeScan3d,
437  bool subscribeScanDescriptor,
438  bool subscribeOdomInfo,
439  int queueSize,
440  bool approxSync)
441 {
442  ROS_INFO("Setup rgbd3 callback");
443 
444  rgbdSubs_.resize(3);
445  for(int i=0; i<3; ++i)
446  {
447  rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
448  rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
449  }
450 #ifdef RTABMAP_SYNC_USER_DATA
451  if(subscribeOdom && subscribeUserData)
452  {
453  odomSub_.subscribe(nh, "odom", queueSize);
454  userDataSub_.subscribe(nh, "user_data", queueSize);
455  if(subscribeScanDescriptor)
456  {
457  subscribedToScanDescriptor_ = true;
458  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
459  if(subscribeOdomInfo)
460  {
461  subscribedToOdomInfo_ = false;
462  ROS_WARN("subscribe_odom_info ignored...");
463  }
464  SYNC_DECL6(rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
465  }
466  else if(subscribeScan2d)
467  {
468  subscribedToScan2d_ = true;
469  scanSub_.subscribe(nh, "scan", queueSize);
470  if(subscribeOdomInfo)
471  {
472  subscribedToOdomInfo_ = false;
473  ROS_WARN("subscribe_odom_info ignored...");
474  }
475  SYNC_DECL6(rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
476  }
477  else if(subscribeScan3d)
478  {
479  subscribedToScan3d_ = true;
480  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
481  if(subscribeOdomInfo)
482  {
483  subscribedToOdomInfo_ = false;
484  ROS_WARN("subscribe_odom_info ignored...");
485  }
486  SYNC_DECL6(rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
487  }
488  else if(subscribeOdomInfo)
489  {
490  subscribedToOdomInfo_ = true;
491  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
492  SYNC_DECL6(rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
493  }
494  else
495  {
496  SYNC_DECL5(rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
497  }
498  }
499  else
500 #endif
501  if(subscribeOdom)
502  {
503  odomSub_.subscribe(nh, "odom", queueSize);
504  if(subscribeScanDescriptor)
505  {
506  subscribedToScanDescriptor_ = true;
507  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
508  if(subscribeOdomInfo)
509  {
510  subscribedToOdomInfo_ = false;
511  ROS_WARN("subscribe_odom_info ignored...");
512  }
513  SYNC_DECL5(rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
514  }
515  else if(subscribeScan2d)
516  {
517  subscribedToScan2d_ = true;
518  scanSub_.subscribe(nh, "scan", queueSize);
519  if(subscribeOdomInfo)
520  {
521  subscribedToOdomInfo_ = false;
522  ROS_WARN("subscribe_odom_info ignored...");
523  }
524  SYNC_DECL5(rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
525  }
526  else if(subscribeScan3d)
527  {
528  subscribedToScan3d_ = true;
529  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
530  if(subscribeOdomInfo)
531  {
532  subscribedToOdomInfo_ = false;
533  ROS_WARN("subscribe_odom_info ignored...");
534  }
535  SYNC_DECL5(rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
536  }
537  else if(subscribeOdomInfo)
538  {
539  subscribedToOdomInfo_ = true;
540  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
541  SYNC_DECL5(rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
542  }
543  else
544  {
545  SYNC_DECL4(rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
546  }
547  }
548 #ifdef RTABMAP_SYNC_USER_DATA
549  else if(subscribeUserData)
550  {
551  userDataSub_.subscribe(nh, "user_data", queueSize);
552  if(subscribeScanDescriptor)
553  {
554  subscribedToScanDescriptor_ = true;
555  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
556  if(subscribeOdomInfo)
557  {
558  subscribedToOdomInfo_ = false;
559  ROS_WARN("subscribe_odom_info ignored...");
560  }
561  SYNC_DECL5(rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); }
562  else if(subscribeScan2d)
563  {
564  subscribedToScan2d_ = true;
565  scanSub_.subscribe(nh, "scan", queueSize);
566  if(subscribeOdomInfo)
567  {
568  subscribedToOdomInfo_ = false;
569  ROS_WARN("subscribe_odom_info ignored...");
570  }
571  SYNC_DECL5(rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
572  }
573  else if(subscribeScan3d)
574  {
575  subscribedToScan3d_ = true;
576  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
577  if(subscribeOdomInfo)
578  {
579  subscribedToOdomInfo_ = false;
580  ROS_WARN("subscribe_odom_info ignored...");
581  }
582  SYNC_DECL5(rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
583  }
584  else if(subscribeOdomInfo)
585  {
586  subscribedToOdomInfo_ = true;
587  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
588  SYNC_DECL5(rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
589  }
590  else
591  {
592  SYNC_DECL4(rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
593  }
594  }
595 #endif
596  else
597  {
598  if(subscribeScanDescriptor)
599  {
600  subscribedToScanDescriptor_ = true;
601  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
602  if(subscribeOdomInfo)
603  {
604  subscribedToOdomInfo_ = false;
605  ROS_WARN("subscribe_odom_info ignored...");
606  }
607  SYNC_DECL4(rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
608  }
609  else if(subscribeScan2d)
610  {
611  subscribedToScan2d_ = true;
612  scanSub_.subscribe(nh, "scan", queueSize);
613  if(subscribeOdomInfo)
614  {
615  subscribedToOdomInfo_ = false;
616  ROS_WARN("subscribe_odom_info ignored...");
617  }
618  SYNC_DECL4(rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
619  }
620  else if(subscribeScan3d)
621  {
622  subscribedToScan3d_ = true;
623  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
624  if(subscribeOdomInfo)
625  {
626  subscribedToOdomInfo_ = false;
627  ROS_WARN("subscribe_odom_info ignored...");
628  }
629  SYNC_DECL4(rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
630  }
631  else if(subscribeOdomInfo)
632  {
633  subscribedToOdomInfo_ = true;
634  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
635  SYNC_DECL4(rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
636  }
637  else
638  {
639  SYNC_DECL3(rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
640  }
641  }
642 }
643 
644 } /* namespace rtabmap_ros */
uFormat
std::string uFormat(const char *fmt,...)
ros::NodeHandle
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
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(...)
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
cv_bridge.h
IMAGE_CONVERSION
#define IMAGE_CONVERSION()
Definition: CommonDataSubscriberRGBD3.cpp:36
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