CommonDataSubscriberRGBD2.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(2); \
39  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
40  rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
41  rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
42  if(!depthMsgs[0].get()) \
43  depthMsgs.clear(); \
44  std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
45  cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
46  cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
47  std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
48  depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
49  depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
50  std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptorMsgs; \
51  std::vector<std::vector<rtabmap_ros::KeyPoint> > localKeyPoints; \
52  std::vector<std::vector<rtabmap_ros::Point3f> > localPoints3d; \
53  std::vector<cv::Mat> localDescriptors; \
54  if(!image1Msg->global_descriptor.data.empty()) \
55  globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
56  if(!image2Msg->global_descriptor.data.empty()) \
57  globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
58  localKeyPoints.push_back(image1Msg->key_points); \
59  localKeyPoints.push_back(image2Msg->key_points); \
60  localPoints3d.push_back(image1Msg->points); \
61  localPoints3d.push_back(image2Msg->points); \
62  localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
63  localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors));
64 
65 // 2 RGBD
66 void CommonDataSubscriber::rgbd2Callback(
67  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
68  const rtabmap_ros::RGBDImageConstPtr& image2Msg)
69 {
70  IMAGE_CONVERSION();
71 
72  nav_msgs::OdometryConstPtr odomMsg; // Null
73  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
74  sensor_msgs::LaserScan scanMsg; // Null
75  sensor_msgs::PointCloud2 scan3dMsg; // Null
76  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
77  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
78 }
79 void CommonDataSubscriber::rgbd2Scan2dCallback(
80  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
81  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
82  const sensor_msgs::LaserScanConstPtr& scanMsg)
83 {
84  IMAGE_CONVERSION();
85 
86  nav_msgs::OdometryConstPtr odomMsg; // Null
87  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
88  sensor_msgs::PointCloud2 scan3dMsg; // Null
89  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
90  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
91 }
92 void CommonDataSubscriber::rgbd2Scan3dCallback(
93  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
94  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
95  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
96 {
97  IMAGE_CONVERSION();
98 
99  nav_msgs::OdometryConstPtr odomMsg; // Null
100  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
101  sensor_msgs::LaserScan scanMsg; // Null
102  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
103  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
104 }
105 void CommonDataSubscriber::rgbd2ScanDescCallback(
106  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
107  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
108  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
109 {
110  IMAGE_CONVERSION();
111 
112  nav_msgs::OdometryConstPtr odomMsg; // Null
113  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
114  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
115  if(!scanDescMsg->global_descriptor.data.empty())
116  {
117  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
118  }
119  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
120 }
121 void CommonDataSubscriber::rgbd2InfoCallback(
122  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
123  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
124  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
125 {
126  IMAGE_CONVERSION();
127 
128  nav_msgs::OdometryConstPtr odomMsg; // Null
129  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
130  sensor_msgs::LaserScan scanMsg; // Null
131  sensor_msgs::PointCloud2 scan3dMsg; // Null
132  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
133 }
134 // 2 RGBD + Odom
135 void CommonDataSubscriber::rgbd2OdomCallback(
136  const nav_msgs::OdometryConstPtr & odomMsg,
137  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
138  const rtabmap_ros::RGBDImageConstPtr& image2Msg)
139 {
140  IMAGE_CONVERSION();
141 
142  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
143  sensor_msgs::LaserScan scanMsg; // Null
144  sensor_msgs::PointCloud2 scan3dMsg; // Null
145  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
146  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
147 }
148 void CommonDataSubscriber::rgbd2OdomScan2dCallback(
149  const nav_msgs::OdometryConstPtr & odomMsg,
150  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
151  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
152  const sensor_msgs::LaserScanConstPtr& scanMsg)
153 {
154  IMAGE_CONVERSION();
155 
156  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
157  sensor_msgs::PointCloud2 scan3dMsg; // Null
158  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
159  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
160 }
161 void CommonDataSubscriber::rgbd2OdomScan3dCallback(
162  const nav_msgs::OdometryConstPtr & odomMsg,
163  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
164  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
165  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
166 {
167  IMAGE_CONVERSION();
168 
169  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
170  sensor_msgs::LaserScan scanMsg; // Null
171  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
172  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
173 }
174 void CommonDataSubscriber::rgbd2OdomScanDescCallback(
175  const nav_msgs::OdometryConstPtr & odomMsg,
176  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
177  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
178  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
179 {
180  IMAGE_CONVERSION();
181 
182  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
183  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
184  if(!scanDescMsg->global_descriptor.data.empty())
185  {
186  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
187  }
188  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
189 }
190 void CommonDataSubscriber::rgbd2OdomInfoCallback(
191  const nav_msgs::OdometryConstPtr & odomMsg,
192  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
193  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
194  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
195 {
196  IMAGE_CONVERSION();
197 
198  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
199  sensor_msgs::LaserScan scanMsg; // Null
200  sensor_msgs::PointCloud2 scan3dMsg; // Null
201  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
202 }
203 
204 #ifdef RTABMAP_SYNC_USER_DATA
205 // 2 RGBD + User Data
206 void CommonDataSubscriber::rgbd2DataCallback(
207  const rtabmap_ros::UserDataConstPtr& userDataMsg,
208  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
209  const rtabmap_ros::RGBDImageConstPtr& image2Msg)
210 {
211  IMAGE_CONVERSION();
212 
213  nav_msgs::OdometryConstPtr odomMsg; // Null
214  sensor_msgs::LaserScan scanMsg; // Null
215  sensor_msgs::PointCloud2 scan3dMsg; // Null
216  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
217  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
218 }
219 void CommonDataSubscriber::rgbd2DataScan2dCallback(
220  const rtabmap_ros::UserDataConstPtr& userDataMsg,
221  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
222  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
223  const sensor_msgs::LaserScanConstPtr& scanMsg)
224 {
225  IMAGE_CONVERSION();
226 
227  nav_msgs::OdometryConstPtr odomMsg; // Null
228  sensor_msgs::PointCloud2 scan3dMsg; // Null
229  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
230  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
231 }
232 void CommonDataSubscriber::rgbd2DataScan3dCallback(
233  const rtabmap_ros::UserDataConstPtr& userDataMsg,
234  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
235  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
236  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
237 {
238  IMAGE_CONVERSION();
239 
240  nav_msgs::OdometryConstPtr odomMsg; // Null
241  sensor_msgs::LaserScan scanMsg; // Null
242  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
243  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
244 }
245 void CommonDataSubscriber::rgbd2DataScanDescCallback(
246  const rtabmap_ros::UserDataConstPtr& userDataMsg,
247  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
248  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
249  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
250 {
251  IMAGE_CONVERSION();
252 
253  nav_msgs::OdometryConstPtr odomMsg; // Null
254  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
255  if(!scanDescMsg->global_descriptor.data.empty())
256  {
257  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
258  }
259  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
260 }
261 void CommonDataSubscriber::rgbd2DataInfoCallback(
262  const rtabmap_ros::UserDataConstPtr& userDataMsg,
263  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
264  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
265  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
266 {
267  IMAGE_CONVERSION();
268 
269  nav_msgs::OdometryConstPtr odomMsg; // Null
270  sensor_msgs::LaserScan scanMsg; // Null
271  sensor_msgs::PointCloud2 scan3dMsg; // Null
272  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
273 }
274 
275 // 2 RGBD + Odom + User Data
276 void CommonDataSubscriber::rgbd2OdomDataCallback(
277  const nav_msgs::OdometryConstPtr& odomMsg,
278  const rtabmap_ros::UserDataConstPtr& userDataMsg,
279  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
280  const rtabmap_ros::RGBDImageConstPtr& image2Msg)
281 {
282  IMAGE_CONVERSION();
283 
284  sensor_msgs::LaserScan scanMsg; // Null
285  sensor_msgs::PointCloud2 scan3dMsg; // Null
286  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
287  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
288 }
289 void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
290  const nav_msgs::OdometryConstPtr& odomMsg,
291  const rtabmap_ros::UserDataConstPtr& userDataMsg,
292  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
293  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
294  const sensor_msgs::LaserScanConstPtr& scanMsg)
295 {
296  IMAGE_CONVERSION();
297 
298  sensor_msgs::PointCloud2 scan3dMsg; // Null
299  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
300  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
301 }
302 void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
303  const nav_msgs::OdometryConstPtr& odomMsg,
304  const rtabmap_ros::UserDataConstPtr& userDataMsg,
305  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
306  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
307  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
308 {
309  IMAGE_CONVERSION();
310 
311  sensor_msgs::LaserScan scanMsg; // Null
312  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
313  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
314 }
315 void CommonDataSubscriber::rgbd2OdomDataScanDescCallback(
316  const nav_msgs::OdometryConstPtr& odomMsg,
317  const rtabmap_ros::UserDataConstPtr& userDataMsg,
318  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
319  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
320  const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg)
321 {
322  IMAGE_CONVERSION();
323 
324  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
325  if(!scanDescMsg->global_descriptor.data.empty())
326  {
327  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
328  }
329  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
330 }
331 void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
332  const nav_msgs::OdometryConstPtr& odomMsg,
333  const rtabmap_ros::UserDataConstPtr& userDataMsg,
334  const rtabmap_ros::RGBDImageConstPtr& image1Msg,
335  const rtabmap_ros::RGBDImageConstPtr& image2Msg,
336  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
337 {
338  IMAGE_CONVERSION();
339 
340  sensor_msgs::LaserScan scanMsg; // Null
341  sensor_msgs::PointCloud2 scan3dMsg; // Null
342  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
343 }
344 #endif
345 
346 void CommonDataSubscriber::setupRGBD2Callbacks(
347  ros::NodeHandle & nh,
348  ros::NodeHandle & pnh,
349  bool subscribeOdom,
350  bool subscribeUserData,
351  bool subscribeScan2d,
352  bool subscribeScan3d,
353  bool subscribeScanDesc,
354  bool subscribeOdomInfo,
355  int queueSize,
356  bool approxSync)
357 {
358  ROS_INFO("Setup rgbd2 callback");
359 
360  rgbdSubs_.resize(2);
361  for(int i=0; i<2; ++i)
362  {
363  rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
364  rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
365  }
366 #ifdef RTABMAP_SYNC_USER_DATA
367  if(subscribeOdom && subscribeUserData)
368  {
369  odomSub_.subscribe(nh, "odom", queueSize);
370  userDataSub_.subscribe(nh, "user_data", queueSize);
371  if(subscribeScanDesc)
372  {
373  subscribedToScanDescriptor_ = true;
374  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
375  if(subscribeOdomInfo)
376  {
377  subscribedToOdomInfo_ = false;
378  ROS_WARN("subscribe_odom_info ignored...");
379  }
380  SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
381  }
382  else if(subscribeScan2d)
383  {
384  subscribedToScan2d_ = true;
385  scanSub_.subscribe(nh, "scan", queueSize);
386  if(subscribeOdomInfo)
387  {
388  subscribedToOdomInfo_ = false;
389  ROS_WARN("subscribe_odom_info ignored...");
390  }
391  SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
392  }
393  else if(subscribeScan3d)
394  {
395  subscribedToScan3d_ = true;
396  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
397  if(subscribeOdomInfo)
398  {
399  subscribedToOdomInfo_ = false;
400  ROS_WARN("subscribe_odom_info ignored...");
401  }
402  SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
403  }
404  else if(subscribeOdomInfo)
405  {
406  subscribedToOdomInfo_ = true;
407  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
408  SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
409  }
410  else
411  {
412  SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
413  }
414  }
415  else
416 #endif
417  if(subscribeOdom)
418  {
419  odomSub_.subscribe(nh, "odom", queueSize);
420  if(subscribeScanDesc)
421  {
422  subscribedToScanDescriptor_ = true;
423  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
424  if(subscribeOdomInfo)
425  {
426  subscribedToOdomInfo_ = false;
427  ROS_WARN("subscribe_odom_info ignored...");
428  }
429  SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
430  }
431  else if(subscribeScan2d)
432  {
433  subscribedToScan2d_ = true;
434  scanSub_.subscribe(nh, "scan", queueSize);
435  if(subscribeOdomInfo)
436  {
437  subscribedToOdomInfo_ = false;
438  ROS_WARN("subscribe_odom_info ignored...");
439  }
440  SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
441  }
442  else if(subscribeScan3d)
443  {
444  subscribedToScan3d_ = true;
445  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
446  if(subscribeOdomInfo)
447  {
448  subscribedToOdomInfo_ = false;
449  ROS_WARN("subscribe_odom_info ignored...");
450  }
451  SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
452  }
453  else if(subscribeOdomInfo)
454  {
455  subscribedToOdomInfo_ = true;
456  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
457  SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
458  }
459  else
460  {
461  SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
462  }
463  }
464 #ifdef RTABMAP_SYNC_USER_DATA
465  else if(subscribeUserData)
466  {
467  userDataSub_.subscribe(nh, "user_data", queueSize);
468  if(subscribeScanDesc)
469  {
470  subscribedToScanDescriptor_ = true;
471  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
472  if(subscribeOdomInfo)
473  {
474  subscribedToOdomInfo_ = false;
475  ROS_WARN("subscribe_odom_info ignored...");
476  }
477  SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
478  }
479  else if(subscribeScan2d)
480  {
481  subscribedToScan2d_ = true;
482  scanSub_.subscribe(nh, "scan", queueSize);
483  if(subscribeOdomInfo)
484  {
485  subscribedToOdomInfo_ = false;
486  ROS_WARN("subscribe_odom_info ignored...");
487  }
488  SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
489  }
490  else if(subscribeScan3d)
491  {
492  subscribedToScan3d_ = true;
493  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
494  if(subscribeOdomInfo)
495  {
496  subscribedToOdomInfo_ = false;
497  ROS_WARN("subscribe_odom_info ignored...");
498  }
499  SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
500  }
501  else if(subscribeOdomInfo)
502  {
503  subscribedToOdomInfo_ = true;
504  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
505  SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
506  }
507  else
508  {
509  SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
510  }
511  }
512 #endif
513  else
514  {
515  if(subscribeScanDesc)
516  {
517  subscribedToScanDescriptor_ = true;
518  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
519  if(subscribeOdomInfo)
520  {
521  subscribedToOdomInfo_ = false;
522  ROS_WARN("subscribe_odom_info ignored...");
523  }
524  SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
525  }
526  else if(subscribeScan2d)
527  {
528  subscribedToScan2d_ = true;
529  scanSub_.subscribe(nh, "scan", queueSize);
530  if(subscribeOdomInfo)
531  {
532  subscribedToOdomInfo_ = false;
533  ROS_WARN("subscribe_odom_info ignored...");
534  }
535  SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
536  }
537  else if(subscribeScan3d)
538  {
539  subscribedToScan3d_ = true;
540  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
541  if(subscribeOdomInfo)
542  {
543  subscribedToOdomInfo_ = false;
544  ROS_WARN("subscribe_odom_info ignored...");
545  }
546  SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
547  }
548  else if(subscribeOdomInfo)
549  {
550  subscribedToOdomInfo_ = true;
551  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
552  SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
553  }
554  else
555  {
556  SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
557  }
558  }
559 }
560 
561 } /* 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
IMAGE_CONVERSION
#define IMAGE_CONVERSION()
Definition: CommonDataSubscriberRGBD2.cpp:36
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
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
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
SYNC_DECL2
#define SYNC_DECL2(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1)
Definition: CommonDataSubscriberDefines.h:108

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