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

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