CommonDataSubscriberRGB.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 
29 
30 namespace rtabmap_ros {
31 
32 // RGB
33 void CommonDataSubscriber::rgbCallback(
34  const sensor_msgs::ImageConstPtr& imageMsg,
35  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
36 {
37  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
38  nav_msgs::OdometryConstPtr odomMsg; // Null
39  sensor_msgs::LaserScan scanMsg; // Null
40  sensor_msgs::PointCloud2 scan3dMsg; // Null
41  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
42  cv_bridge::CvImageConstPtr depthMsg;// Null
43  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
44 }
45 void CommonDataSubscriber::rgbScan2dCallback(
46  const sensor_msgs::ImageConstPtr& imageMsg,
47  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
48  const sensor_msgs::LaserScanConstPtr& scanMsg)
49 {
50  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
51  nav_msgs::OdometryConstPtr odomMsg; // Null
52  sensor_msgs::PointCloud2 scan3dMsg; // Null
53  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
54  cv_bridge::CvImageConstPtr depthMsg;// Null
55  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
56 }
57 void CommonDataSubscriber::rgbScan3dCallback(
58  const sensor_msgs::ImageConstPtr& imageMsg,
59  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
60  const sensor_msgs::PointCloud2ConstPtr& scanMsg)
61 {
62  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
63  nav_msgs::OdometryConstPtr odomMsg; // Null
64  sensor_msgs::LaserScan scan2dMsg; // Null
65  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
66  cv_bridge::CvImageConstPtr depthMsg;// Null
67  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
68 }
69 void CommonDataSubscriber::rgbScanDescCallback(
70  const sensor_msgs::ImageConstPtr& imageMsg,
71  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
72  const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
73 {
74  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
75  nav_msgs::OdometryConstPtr odomMsg; // Null
76  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
77  cv_bridge::CvImageConstPtr depthMsg;// Null
78  std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
79  if(!scanMsg->global_descriptor.data.empty())
80  {
81  globalDescriptor.push_back(scanMsg->global_descriptor);
82  }
83  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
84 }
85 void CommonDataSubscriber::rgbInfoCallback(
86  const sensor_msgs::ImageConstPtr& imageMsg,
87  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
88  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
89 {
90  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
91  nav_msgs::OdometryConstPtr odomMsg; // Null
92  sensor_msgs::LaserScan scan2dMsg; // Null
93  sensor_msgs::PointCloud2 scan3dMsg; // Null
94  cv_bridge::CvImageConstPtr depthMsg;// Null
95  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
96 }
97 void CommonDataSubscriber::rgbScan2dInfoCallback(
98  const sensor_msgs::ImageConstPtr& imageMsg,
99  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
100  const sensor_msgs::LaserScanConstPtr& scanMsg,
101  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
102 {
103  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
104  nav_msgs::OdometryConstPtr odomMsg; // Null
105  sensor_msgs::PointCloud2 scan3dMsg; // Null
106  cv_bridge::CvImageConstPtr depthMsg;// Null
107  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
108 }
109 void CommonDataSubscriber::rgbScan3dInfoCallback(
110  const sensor_msgs::ImageConstPtr& imageMsg,
111  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
112  const sensor_msgs::PointCloud2ConstPtr& scanMsg,
113  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
114 {
115  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
116  nav_msgs::OdometryConstPtr odomMsg; // Null
117  sensor_msgs::LaserScan scan2dMsg; // Null
118  cv_bridge::CvImageConstPtr depthMsg;// Null
119  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
120 }
121 void CommonDataSubscriber::rgbScanDescInfoCallback(
122  const sensor_msgs::ImageConstPtr& imageMsg,
123  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
124  const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
125  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
126 {
127  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
128  nav_msgs::OdometryConstPtr odomMsg; // Null
129  cv_bridge::CvImageConstPtr depthMsg;// Null
130  std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
131  if(!scanMsg->global_descriptor.data.empty())
132  {
133  globalDescriptor.push_back(scanMsg->global_descriptor);
134  }
135  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
136 }
137 
138 // RGB + Odom
139 void CommonDataSubscriber::rgbOdomCallback(
140  const nav_msgs::OdometryConstPtr & odomMsg,
141  const sensor_msgs::ImageConstPtr& imageMsg,
142  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
143 {
144  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
145  sensor_msgs::LaserScan scanMsg; // Null
146  sensor_msgs::PointCloud2 scan3dMsg; // null
147  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
148  cv_bridge::CvImageConstPtr depthMsg;// Null
149  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
150 }
151 void CommonDataSubscriber::rgbOdomScan2dCallback(
152  const nav_msgs::OdometryConstPtr & odomMsg,
153  const sensor_msgs::ImageConstPtr& imageMsg,
154  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
155  const sensor_msgs::LaserScanConstPtr& scanMsg)
156 {
157  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
158  sensor_msgs::PointCloud2 scan3dMsg; // Null
159  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
160  cv_bridge::CvImageConstPtr depthMsg;// Null
161  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
162 }
163 void CommonDataSubscriber::rgbOdomScan3dCallback(
164  const nav_msgs::OdometryConstPtr & odomMsg,
165  const sensor_msgs::ImageConstPtr& imageMsg,
166  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
167  const sensor_msgs::PointCloud2ConstPtr& scanMsg)
168 {
169  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
170  sensor_msgs::LaserScan scan2dMsg; // Null
171  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
172  cv_bridge::CvImageConstPtr depthMsg;// Null
173  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
174 }
175 void CommonDataSubscriber::rgbOdomScanDescCallback(
176  const nav_msgs::OdometryConstPtr & odomMsg,
177  const sensor_msgs::ImageConstPtr& imageMsg,
178  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
179  const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
180 {
181  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
182  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
183  cv_bridge::CvImageConstPtr depthMsg;// Null
184  std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
185  if(!scanMsg->global_descriptor.data.empty())
186  {
187  globalDescriptor.push_back(scanMsg->global_descriptor);
188  }
189  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
190 }
191 void CommonDataSubscriber::rgbOdomInfoCallback(
192  const nav_msgs::OdometryConstPtr & odomMsg,
193  const sensor_msgs::ImageConstPtr& imageMsg,
194  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
195  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
196 {
197  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
198  sensor_msgs::LaserScan scan2dMsg; // Null
199  sensor_msgs::PointCloud2 scan3dMsg; // Null
200  cv_bridge::CvImageConstPtr depthMsg;// Null
201  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
202 }
203 void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
204  const nav_msgs::OdometryConstPtr & odomMsg,
205  const sensor_msgs::ImageConstPtr& imageMsg,
206  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
207  const sensor_msgs::LaserScanConstPtr& scanMsg,
208  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
209 {
210  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
211  sensor_msgs::PointCloud2 scan3dMsg; // Null
212  cv_bridge::CvImageConstPtr depthMsg;// Null
213  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
214 }
215 void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
216  const nav_msgs::OdometryConstPtr & odomMsg,
217  const sensor_msgs::ImageConstPtr& imageMsg,
218  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
219  const sensor_msgs::PointCloud2ConstPtr& scanMsg,
220  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
221 {
222  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
223  sensor_msgs::LaserScan scan2dMsg; // Null
224  cv_bridge::CvImageConstPtr depthMsg;// Null
225  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
226 }
227 void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
228  const nav_msgs::OdometryConstPtr & odomMsg,
229  const sensor_msgs::ImageConstPtr& imageMsg,
230  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
231  const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
232  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
233 {
234  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
235  cv_bridge::CvImageConstPtr depthMsg;// Null
236  std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
237  if(!scanMsg->global_descriptor.data.empty())
238  {
239  globalDescriptor.push_back(scanMsg->global_descriptor);
240  }
241  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
242 }
243 
244 #ifdef RTABMAP_SYNC_USER_DATA
245 // RGB + Depth + User Data
246 void CommonDataSubscriber::rgbDataCallback(
247  const rtabmap_ros::UserDataConstPtr & userDataMsg,
248  const sensor_msgs::ImageConstPtr& imageMsg,
249  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
250 {
251  nav_msgs::OdometryConstPtr odomMsg; // Null
252  sensor_msgs::LaserScan scanMsg; // Null
253  sensor_msgs::PointCloud2 scan3dMsg; // null
254  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
255  cv_bridge::CvImageConstPtr depthMsg;// Null
256  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
257 }
258 void CommonDataSubscriber::rgbDataScan2dCallback(
259  const rtabmap_ros::UserDataConstPtr & userDataMsg,
260  const sensor_msgs::ImageConstPtr& imageMsg,
261  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
262  const sensor_msgs::LaserScanConstPtr& scanMsg)
263 {
264  nav_msgs::OdometryConstPtr odomMsg; // null
265  sensor_msgs::PointCloud2 scan3dMsg; // Null
266  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
267  cv_bridge::CvImageConstPtr depthMsg;// Null
268  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
269 }
270 void CommonDataSubscriber::rgbDataScan3dCallback(
271  const rtabmap_ros::UserDataConstPtr & userDataMsg,
272  const sensor_msgs::ImageConstPtr& imageMsg,
273  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
274  const sensor_msgs::PointCloud2ConstPtr& scanMsg)
275 {
276  nav_msgs::OdometryConstPtr odomMsg; // null
277  sensor_msgs::LaserScan scan2dMsg; // Null
278  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
279  cv_bridge::CvImageConstPtr depthMsg;// Null
280  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
281 }
282 void CommonDataSubscriber::rgbDataScanDescCallback(
283  const rtabmap_ros::UserDataConstPtr & userDataMsg,
284  const sensor_msgs::ImageConstPtr& imageMsg,
285  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
286  const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
287 {
288  nav_msgs::OdometryConstPtr odomMsg; // null
289  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
290  cv_bridge::CvImageConstPtr depthMsg;// Null
291  std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
292  if(!scanMsg->global_descriptor.data.empty())
293  {
294  globalDescriptor.push_back(scanMsg->global_descriptor);
295  }
296  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
297 }
298 void CommonDataSubscriber::rgbDataInfoCallback(
299  const rtabmap_ros::UserDataConstPtr & userDataMsg,
300  const sensor_msgs::ImageConstPtr& imageMsg,
301  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
302  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
303 {
304  nav_msgs::OdometryConstPtr odomMsg; // null
305  sensor_msgs::LaserScan scan2dMsg; // Null
306  sensor_msgs::PointCloud2 scan3dMsg; // Null
307  cv_bridge::CvImageConstPtr depthMsg;// Null
308  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
309 }
310 void CommonDataSubscriber::rgbDataScan2dInfoCallback(
311  const rtabmap_ros::UserDataConstPtr & userDataMsg,
312  const sensor_msgs::ImageConstPtr& imageMsg,
313  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
314  const sensor_msgs::LaserScanConstPtr& scanMsg,
315  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
316 {
317  nav_msgs::OdometryConstPtr odomMsg; // null
318  sensor_msgs::PointCloud2 scan3dMsg; // Null
319  cv_bridge::CvImageConstPtr depthMsg;// Null
320  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
321 }
322 void CommonDataSubscriber::rgbDataScan3dInfoCallback(
323  const rtabmap_ros::UserDataConstPtr & userDataMsg,
324  const sensor_msgs::ImageConstPtr& imageMsg,
325  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
326  const sensor_msgs::PointCloud2ConstPtr& scanMsg,
327  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
328 {
329  nav_msgs::OdometryConstPtr odomMsg; // null
330  sensor_msgs::LaserScan scan2dMsg; // Null
331  cv_bridge::CvImageConstPtr depthMsg;// Null
332  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
333 }
334 void CommonDataSubscriber::rgbDataScanDescInfoCallback(
335  const rtabmap_ros::UserDataConstPtr & userDataMsg,
336  const sensor_msgs::ImageConstPtr& imageMsg,
337  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
338  const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
339  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
340 {
341  nav_msgs::OdometryConstPtr odomMsg; // null
342  cv_bridge::CvImageConstPtr depthMsg;// Null
343  std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
344  if(!scanMsg->global_descriptor.data.empty())
345  {
346  globalDescriptor.push_back(scanMsg->global_descriptor);
347  }
348  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
349 }
350 
351 // RGB + Depth + Odom + User Data
352 void CommonDataSubscriber::rgbOdomDataCallback(
353  const nav_msgs::OdometryConstPtr & odomMsg,
354  const rtabmap_ros::UserDataConstPtr & userDataMsg,
355  const sensor_msgs::ImageConstPtr& imageMsg,
356  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
357 {
358  sensor_msgs::LaserScan scanMsg; // Null
359  sensor_msgs::PointCloud2 scan3dMsg; // null
360  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
361  cv_bridge::CvImageConstPtr depthMsg;// Null
362  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
363 }
364 void CommonDataSubscriber::rgbOdomDataScan2dCallback(
365  const nav_msgs::OdometryConstPtr & odomMsg,
366  const rtabmap_ros::UserDataConstPtr & userDataMsg,
367  const sensor_msgs::ImageConstPtr& imageMsg,
368  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
369  const sensor_msgs::LaserScanConstPtr& scanMsg)
370 {
371  sensor_msgs::PointCloud2 scan3dMsg; // Null
372  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
373  cv_bridge::CvImageConstPtr depthMsg;// Null
374  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
375 }
376 void CommonDataSubscriber::rgbOdomDataScan3dCallback(
377  const nav_msgs::OdometryConstPtr & odomMsg,
378  const rtabmap_ros::UserDataConstPtr & userDataMsg,
379  const sensor_msgs::ImageConstPtr& imageMsg,
380  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
381  const sensor_msgs::PointCloud2ConstPtr& scanMsg)
382 {
383  sensor_msgs::LaserScan scan2dMsg; // Null
384  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
385  cv_bridge::CvImageConstPtr depthMsg;// Null
386  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
387 }
388 void CommonDataSubscriber::rgbOdomDataScanDescCallback(
389  const nav_msgs::OdometryConstPtr & odomMsg,
390  const rtabmap_ros::UserDataConstPtr & userDataMsg,
391  const sensor_msgs::ImageConstPtr& imageMsg,
392  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
393  const rtabmap_ros::ScanDescriptorConstPtr& scanMsg)
394 {
395  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
396  cv_bridge::CvImageConstPtr depthMsg;// Null
397  std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
398  if(!scanMsg->global_descriptor.data.empty())
399  {
400  globalDescriptor.push_back(scanMsg->global_descriptor);
401  }
402  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
403 }
404 void CommonDataSubscriber::rgbOdomDataInfoCallback(
405  const nav_msgs::OdometryConstPtr & odomMsg,
406  const rtabmap_ros::UserDataConstPtr & userDataMsg,
407  const sensor_msgs::ImageConstPtr& imageMsg,
408  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
409  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
410 {
411  sensor_msgs::LaserScan scan2dMsg; // Null
412  sensor_msgs::PointCloud2 scan3dMsg; // Null
413  cv_bridge::CvImageConstPtr depthMsg;// Null
414  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
415 }
416 void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
417  const nav_msgs::OdometryConstPtr & odomMsg,
418  const rtabmap_ros::UserDataConstPtr & userDataMsg,
419  const sensor_msgs::ImageConstPtr& imageMsg,
420  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
421  const sensor_msgs::LaserScanConstPtr& scanMsg,
422  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
423 {
424  sensor_msgs::PointCloud2 scan3dMsg; // Null
425  cv_bridge::CvImageConstPtr depthMsg;// Null
426  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
427 }
428 void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
429  const nav_msgs::OdometryConstPtr & odomMsg,
430  const rtabmap_ros::UserDataConstPtr & userDataMsg,
431  const sensor_msgs::ImageConstPtr& imageMsg,
432  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
433  const sensor_msgs::PointCloud2ConstPtr& scanMsg,
434  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
435 {
436  sensor_msgs::LaserScan scan2dMsg; // Null
437  cv_bridge::CvImageConstPtr depthMsg;// Null
438  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
439 }
440 void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
441  const nav_msgs::OdometryConstPtr & odomMsg,
442  const rtabmap_ros::UserDataConstPtr & userDataMsg,
443  const sensor_msgs::ImageConstPtr& imageMsg,
444  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
445  const rtabmap_ros::ScanDescriptorConstPtr& scanMsg,
446  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
447 {
448  cv_bridge::CvImageConstPtr depthMsg;// Null
449  std::vector<rtabmap_ros::GlobalDescriptor> globalDescriptor;
450  if(!scanMsg->global_descriptor.data.empty())
451  {
452  globalDescriptor.push_back(scanMsg->global_descriptor);
453  }
454  commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
455 }
456 #endif
457 
459  ros::NodeHandle & nh,
460  ros::NodeHandle & pnh,
461  bool subscribeOdom,
462  bool subscribeUserData,
463  bool subscribeScan2d,
464  bool subscribeScan3d,
465  bool subscribeScanDesc,
466  bool subscribeOdomInfo,
467  int queueSize,
468  bool approxSync)
469 {
470  ROS_INFO("Setup rgb-only callback");
471 
472  std::string rgbPrefix = "rgb";
473  ros::NodeHandle rgb_nh(nh, rgbPrefix);
474  ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
475  image_transport::ImageTransport rgb_it(rgb_nh);
476  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
477 
478  imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb);
479  cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize);
480 
481 #ifdef RTABMAP_SYNC_USER_DATA
482  if(subscribeOdom && subscribeUserData)
483  {
484  odomSub_.subscribe(nh, "odom", queueSize);
485  userDataSub_.subscribe(nh, "user_data", queueSize);
486 
487  if(subscribeScanDesc)
488  {
490  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
491  if(subscribeOdomInfo)
492  {
493  subscribedToOdomInfo_ = true;
494  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
495  SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
496  }
497  else
498  {
499  SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
500  }
501  }
502  else if(subscribeScan2d)
503  {
504  subscribedToScan2d_ = true;
505  scanSub_.subscribe(nh, "scan", queueSize);
506  if(subscribeOdomInfo)
507  {
508  subscribedToOdomInfo_ = true;
509  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
510  SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
511  }
512  else
513  {
514  SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
515  }
516  }
517  else if(subscribeScan3d)
518  {
519  subscribedToScan3d_ = true;
520  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
521  if(subscribeOdomInfo)
522  {
523  subscribedToOdomInfo_ = true;
524  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
525  SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
526  }
527  else
528  {
529  SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
530  }
531  }
532  else if(subscribeOdomInfo)
533  {
534  subscribedToOdomInfo_ = true;
535  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
536  SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
537  }
538  else
539  {
540  SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
541  }
542  }
543  else
544 #endif
545  if(subscribeOdom)
546  {
547  odomSub_.subscribe(nh, "odom", queueSize);
548 
549  if(subscribeScanDesc)
550  {
552  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
553  if(subscribeOdomInfo)
554  {
555  subscribedToOdomInfo_ = true;
556  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
557  SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
558  }
559  else
560  {
561  SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
562  }
563  }
564  else if(subscribeScan2d)
565  {
566  subscribedToScan2d_ = true;
567  scanSub_.subscribe(nh, "scan", queueSize);
568  if(subscribeOdomInfo)
569  {
570  subscribedToOdomInfo_ = true;
571  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
572  SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
573  }
574  else
575  {
576  SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
577  }
578  }
579  else if(subscribeScan3d)
580  {
581  subscribedToScan3d_ = true;
582  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
583  if(subscribeOdomInfo)
584  {
585  subscribedToOdomInfo_ = true;
586  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
587  SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
588  }
589  else
590  {
591  SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
592  }
593  }
594  else if(subscribeOdomInfo)
595  {
596  subscribedToOdomInfo_ = true;
597  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
598  SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
599  }
600  else
601  {
602  SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
603  }
604  }
605 #ifdef RTABMAP_SYNC_USER_DATA
606  else if(subscribeUserData)
607  {
608  userDataSub_.subscribe(nh, "user_data", queueSize);
609 
610  if(subscribeScanDesc)
611  {
613  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
614 
615  if(subscribeOdomInfo)
616  {
617  subscribedToOdomInfo_ = true;
618  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
619  SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
620  }
621  else
622  {
623  SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
624  }
625  }
626  else if(subscribeScan2d)
627  {
628  subscribedToScan2d_ = true;
629  scanSub_.subscribe(nh, "scan", queueSize);
630 
631  if(subscribeOdomInfo)
632  {
633  subscribedToOdomInfo_ = true;
634  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
635  SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
636  }
637  else
638  {
639  SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
640  }
641  }
642  else if(subscribeScan3d)
643  {
644  subscribedToScan3d_ = true;
645  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
646  if(subscribeOdomInfo)
647  {
648  subscribedToOdomInfo_ = true;
649  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
650  SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
651  }
652  else
653  {
654  SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
655  }
656  }
657  else if(subscribeOdomInfo)
658  {
659  subscribedToOdomInfo_ = true;
660  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
661  SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
662  }
663  else
664  {
665  SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
666  }
667  }
668 #endif
669  else
670  {
671  if(subscribeScanDesc)
672  {
674  scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
675  if(subscribeOdomInfo)
676  {
677  subscribedToOdomInfo_ = true;
678  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
679  SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
680  }
681  else
682  {
683  SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
684  }
685  }
686  else if(subscribeScan2d)
687  {
688  subscribedToScan2d_ = true;
689  scanSub_.subscribe(nh, "scan", queueSize);
690  if(subscribeOdomInfo)
691  {
692  subscribedToOdomInfo_ = true;
693  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
694  SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
695  }
696  else
697  {
698  SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
699  }
700  }
701  else if(subscribeScan3d)
702  {
703  subscribedToScan3d_ = true;
704  scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
705  if(subscribeOdomInfo)
706  {
707  subscribedToOdomInfo_ = true;
708  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
709  SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
710  }
711  else
712  {
713  SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
714  }
715  }
716  else if(subscribeOdomInfo)
717  {
718  subscribedToOdomInfo_ = true;
719  odomInfoSub_.subscribe(nh, "odom_info", queueSize);
720  SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
721  }
722  else
723  {
724  SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
725  }
726  }
727 }
728 
729 } /* namespace rtabmap_ros */
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
#define SYNC_DECL3(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2)
message_filters::Subscriber< rtabmap_ros::UserData > userDataSub_
message_filters::Subscriber< rtabmap_ros::OdomInfo > odomInfoSub_
void setupRGBCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, bool subscribeOdomInfo, int queueSize, bool approxSync)
#define SYNC_DECL5(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4)
#define ROS_INFO(...)
image_transport::SubscriberFilter imageSub_
message_filters::Subscriber< sensor_msgs::LaserScan > scanSub_
message_filters::Subscriber< rtabmap_ros::ScanDescriptor > scanDescSub_
message_filters::Subscriber< nav_msgs::Odometry > odomSub_
std::string resolveName(const std::string &name, bool remap=true) const
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
#define SYNC_DECL6(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5)
message_filters::Subscriber< sensor_msgs::PointCloud2 > scan3dSub_
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< sensor_msgs::CameraInfo > cameraInfoSub_
#define SYNC_DECL4(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3)
#define SYNC_DECL2(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1)
void commonSingleCameraCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_ros::UserDataConstPtr &userDataMsg, const cv_bridge::CvImageConstPtr &imageMsg, const cv_bridge::CvImageConstPtr &depthMsg, const sensor_msgs::CameraInfo &rgbCameraInfoMsg, const sensor_msgs::CameraInfo &depthCameraInfoMsg, 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< rtabmap_ros::KeyPoint > &localKeyPoints=std::vector< rtabmap_ros::KeyPoint >(), const std::vector< rtabmap_ros::Point3f > &localPoints3d=std::vector< rtabmap_ros::Point3f >(), const cv::Mat &localDescriptors=cv::Mat())


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