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