CommonDataSubscriberDepth.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 
30 namespace rtabmap_ros {
31 
32 // RGB + Depth
33 void CommonDataSubscriber::depthCallback(
34  const sensor_msgs::ImageConstPtr& imageMsg,
35  const sensor_msgs::ImageConstPtr& depthMsg,
36  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
37 {
38  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
39  nav_msgs::OdometryConstPtr odomMsg; // Null
40  sensor_msgs::LaserScanConstPtr scanMsg; // Null
41  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
42  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
43  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
44 }
45 void CommonDataSubscriber::depthScan2dCallback(
46  const sensor_msgs::ImageConstPtr& imageMsg,
47  const sensor_msgs::ImageConstPtr& depthMsg,
48  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
49  const sensor_msgs::LaserScanConstPtr& scanMsg)
50 {
51  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
52  nav_msgs::OdometryConstPtr odomMsg; // Null
53  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
54  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
55  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
56 }
57 void CommonDataSubscriber::depthScan3dCallback(
58  const sensor_msgs::ImageConstPtr& imageMsg,
59  const sensor_msgs::ImageConstPtr& depthMsg,
60  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
61  const sensor_msgs::PointCloud2ConstPtr& scanMsg)
62 {
63  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
64  nav_msgs::OdometryConstPtr odomMsg; // Null
65  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
66  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
67  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
68 }
69 void CommonDataSubscriber::depthInfoCallback(
70  const sensor_msgs::ImageConstPtr& imageMsg,
71  const sensor_msgs::ImageConstPtr& depthMsg,
72  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
73  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
74 {
75  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
76  nav_msgs::OdometryConstPtr odomMsg; // Null
77  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
78  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
79  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
80 }
81 void CommonDataSubscriber::depthScan2dInfoCallback(
82  const sensor_msgs::ImageConstPtr& imageMsg,
83  const sensor_msgs::ImageConstPtr& depthMsg,
84  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
85  const sensor_msgs::LaserScanConstPtr& scanMsg,
86  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
87 {
88  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
89  nav_msgs::OdometryConstPtr odomMsg; // Null
90  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
91  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
92 }
93 void CommonDataSubscriber::depthScan3dInfoCallback(
94  const sensor_msgs::ImageConstPtr& imageMsg,
95  const sensor_msgs::ImageConstPtr& depthMsg,
96  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
97  const sensor_msgs::PointCloud2ConstPtr& scanMsg,
98  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
99 {
100  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
101  nav_msgs::OdometryConstPtr odomMsg; // Null
102  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
103  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
104 }
105 
106 // RGB + Depth + Odom
107 void CommonDataSubscriber::depthOdomCallback(
108  const nav_msgs::OdometryConstPtr & odomMsg,
109  const sensor_msgs::ImageConstPtr& imageMsg,
110  const sensor_msgs::ImageConstPtr& depthMsg,
111  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
112 {
113  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
114  sensor_msgs::LaserScanConstPtr scanMsg; // Null
115  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
116  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
117  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
118 }
119 void CommonDataSubscriber::depthOdomScan2dCallback(
120  const nav_msgs::OdometryConstPtr & odomMsg,
121  const sensor_msgs::ImageConstPtr& imageMsg,
122  const sensor_msgs::ImageConstPtr& depthMsg,
123  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
124  const sensor_msgs::LaserScanConstPtr& scanMsg)
125 {
126  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
127  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
128  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
129  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
130 }
131 void CommonDataSubscriber::depthOdomScan3dCallback(
132  const nav_msgs::OdometryConstPtr & odomMsg,
133  const sensor_msgs::ImageConstPtr& imageMsg,
134  const sensor_msgs::ImageConstPtr& depthMsg,
135  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
136  const sensor_msgs::PointCloud2ConstPtr& scanMsg)
137 {
138  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
139  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
140  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
141  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
142 }
143 void CommonDataSubscriber::depthOdomInfoCallback(
144  const nav_msgs::OdometryConstPtr & odomMsg,
145  const sensor_msgs::ImageConstPtr& imageMsg,
146  const sensor_msgs::ImageConstPtr& depthMsg,
147  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
148  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
149 {
150  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
151  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
152  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
153  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
154 }
155 void CommonDataSubscriber::depthOdomScan2dInfoCallback(
156  const nav_msgs::OdometryConstPtr & odomMsg,
157  const sensor_msgs::ImageConstPtr& imageMsg,
158  const sensor_msgs::ImageConstPtr& depthMsg,
159  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
160  const sensor_msgs::LaserScanConstPtr& scanMsg,
161  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
162 {
163  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
164  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
165  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
166 }
167 void CommonDataSubscriber::depthOdomScan3dInfoCallback(
168  const nav_msgs::OdometryConstPtr & odomMsg,
169  const sensor_msgs::ImageConstPtr& imageMsg,
170  const sensor_msgs::ImageConstPtr& depthMsg,
171  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
172  const sensor_msgs::PointCloud2ConstPtr& scanMsg,
173  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
174 {
175  rtabmap_ros::UserDataConstPtr userDataMsg; // Null
176  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
177  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
178 }
179 
180 // RGB + Depth + User Data
181 void CommonDataSubscriber::depthDataCallback(
182  const rtabmap_ros::UserDataConstPtr & userDataMsg,
183  const sensor_msgs::ImageConstPtr& imageMsg,
184  const sensor_msgs::ImageConstPtr& depthMsg,
185  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
186 {
187  nav_msgs::OdometryConstPtr odomMsg; // Null
188  sensor_msgs::LaserScanConstPtr scanMsg; // Null
189  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
190  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
191  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
192 }
193 void CommonDataSubscriber::depthDataScan2dCallback(
194  const rtabmap_ros::UserDataConstPtr & userDataMsg,
195  const sensor_msgs::ImageConstPtr& imageMsg,
196  const sensor_msgs::ImageConstPtr& depthMsg,
197  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
198  const sensor_msgs::LaserScanConstPtr& scanMsg)
199 {
200  nav_msgs::OdometryConstPtr odomMsg; // null
201  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
202  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
203  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
204 }
205 void CommonDataSubscriber::depthDataScan3dCallback(
206  const rtabmap_ros::UserDataConstPtr & userDataMsg,
207  const sensor_msgs::ImageConstPtr& imageMsg,
208  const sensor_msgs::ImageConstPtr& depthMsg,
209  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
210  const sensor_msgs::PointCloud2ConstPtr& scanMsg)
211 {
212  nav_msgs::OdometryConstPtr odomMsg; // null
213  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
214  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
215  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
216 }
217 void CommonDataSubscriber::depthDataInfoCallback(
218  const rtabmap_ros::UserDataConstPtr & userDataMsg,
219  const sensor_msgs::ImageConstPtr& imageMsg,
220  const sensor_msgs::ImageConstPtr& depthMsg,
221  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
222  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
223 {
224  nav_msgs::OdometryConstPtr odomMsg; // null
225  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
226  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
227  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
228 }
229 void CommonDataSubscriber::depthDataScan2dInfoCallback(
230  const rtabmap_ros::UserDataConstPtr & userDataMsg,
231  const sensor_msgs::ImageConstPtr& imageMsg,
232  const sensor_msgs::ImageConstPtr& depthMsg,
233  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
234  const sensor_msgs::LaserScanConstPtr& scanMsg,
235  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
236 {
237  nav_msgs::OdometryConstPtr odomMsg; // null
238  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
239  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
240 }
241 void CommonDataSubscriber::depthDataScan3dInfoCallback(
242  const rtabmap_ros::UserDataConstPtr & userDataMsg,
243  const sensor_msgs::ImageConstPtr& imageMsg,
244  const sensor_msgs::ImageConstPtr& depthMsg,
245  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
246  const sensor_msgs::PointCloud2ConstPtr& scanMsg,
247  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
248 {
249  nav_msgs::OdometryConstPtr odomMsg; // null
250  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
251  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
252 }
253 
254 // RGB + Depth + Odom + User Data
255 void CommonDataSubscriber::depthOdomDataCallback(
256  const nav_msgs::OdometryConstPtr & odomMsg,
257  const rtabmap_ros::UserDataConstPtr & userDataMsg,
258  const sensor_msgs::ImageConstPtr& imageMsg,
259  const sensor_msgs::ImageConstPtr& depthMsg,
260  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
261 {
262  sensor_msgs::LaserScanConstPtr scanMsg; // Null
263  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
264  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
265  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
266 }
267 void CommonDataSubscriber::depthOdomDataScan2dCallback(
268  const nav_msgs::OdometryConstPtr & odomMsg,
269  const rtabmap_ros::UserDataConstPtr & userDataMsg,
270  const sensor_msgs::ImageConstPtr& imageMsg,
271  const sensor_msgs::ImageConstPtr& depthMsg,
272  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
273  const sensor_msgs::LaserScanConstPtr& scanMsg)
274 {
275  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
276  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
277  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
278 }
279 void CommonDataSubscriber::depthOdomDataScan3dCallback(
280  const nav_msgs::OdometryConstPtr & odomMsg,
281  const rtabmap_ros::UserDataConstPtr & userDataMsg,
282  const sensor_msgs::ImageConstPtr& imageMsg,
283  const sensor_msgs::ImageConstPtr& depthMsg,
284  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
285  const sensor_msgs::PointCloud2ConstPtr& scanMsg)
286 {
287  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
288  rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
289  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
290 }
291 void CommonDataSubscriber::depthOdomDataInfoCallback(
292  const nav_msgs::OdometryConstPtr & odomMsg,
293  const rtabmap_ros::UserDataConstPtr & userDataMsg,
294  const sensor_msgs::ImageConstPtr& imageMsg,
295  const sensor_msgs::ImageConstPtr& depthMsg,
296  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
297  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
298 {
299  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
300  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
301  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
302 }
303 void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
304  const nav_msgs::OdometryConstPtr & odomMsg,
305  const rtabmap_ros::UserDataConstPtr & userDataMsg,
306  const sensor_msgs::ImageConstPtr& imageMsg,
307  const sensor_msgs::ImageConstPtr& depthMsg,
308  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
309  const sensor_msgs::LaserScanConstPtr& scanMsg,
310  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
311 {
312  sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
313  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
314 }
315 void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
316  const nav_msgs::OdometryConstPtr & odomMsg,
317  const rtabmap_ros::UserDataConstPtr & userDataMsg,
318  const sensor_msgs::ImageConstPtr& imageMsg,
319  const sensor_msgs::ImageConstPtr& depthMsg,
320  const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
321  const sensor_msgs::PointCloud2ConstPtr& scanMsg,
322  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
323 {
324  sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
325  commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
326 }
327 
328 void CommonDataSubscriber::setupDepthCallbacks(
329  ros::NodeHandle & nh,
330  ros::NodeHandle & pnh,
331  bool subscribeOdom,
332  bool subscribeUserData,
333  bool subscribeScan2d,
334  bool subscribeScan3d,
335  bool subscribeOdomInfo,
336  int queueSize,
337  bool approxSync)
338 {
339  ROS_INFO("Setup depth callback");
340 
341  std::string rgbPrefix = "rgb";
342  std::string depthPrefix = "depth";
343  ros::NodeHandle rgb_nh(nh, rgbPrefix);
344  ros::NodeHandle depth_nh(nh, depthPrefix);
345  ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
346  ros::NodeHandle depth_pnh(pnh, depthPrefix);
347  image_transport::ImageTransport rgb_it(rgb_nh);
348  image_transport::ImageTransport depth_it(depth_nh);
349  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
350  image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
351 
352  imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
353  imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
354  cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
355 
356  if(subscribeOdom && subscribeUserData)
357  {
358  odomSub_.subscribe(nh, "odom", 1);
359  userDataSub_.subscribe(nh, "user_data", 1);
360 
361  if(subscribeScan2d)
362  {
363  subscribedToScan2d_ = true;
364  scanSub_.subscribe(nh, "scan", 1);
365  if(subscribeOdomInfo)
366  {
367  subscribedToOdomInfo_ = true;
368  odomInfoSub_.subscribe(nh, "odom_info", 1);
369  SYNC_DECL7(depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
370  }
371  else
372  {
373  SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
374  }
375  }
376  else if(subscribeScan3d)
377  {
378  subscribedToScan3d_ = true;
379  scan3dSub_.subscribe(nh, "scan_cloud", 1);
380  if(subscribeOdomInfo)
381  {
382  subscribedToOdomInfo_ = true;
383  odomInfoSub_.subscribe(nh, "odom_info", 1);
384  SYNC_DECL7(depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
385  }
386  else
387  {
388  SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
389  }
390  }
391  else if(subscribeOdomInfo)
392  {
393  subscribedToOdomInfo_ = true;
394  odomInfoSub_.subscribe(nh, "odom_info", 1);
395  SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
396  }
397  else
398  {
399  SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
400  }
401  }
402  else if(subscribeOdom)
403  {
404  odomSub_.subscribe(nh, "odom", 1);
405 
406  if(subscribeScan2d)
407  {
408  subscribedToScan2d_ = true;
409  scanSub_.subscribe(nh, "scan", 1);
410  if(subscribeOdomInfo)
411  {
412  subscribedToOdomInfo_ = true;
413  odomInfoSub_.subscribe(nh, "odom_info", 1);
414  SYNC_DECL6(depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
415  }
416  else
417  {
418  SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
419  }
420  }
421  else if(subscribeScan3d)
422  {
423  subscribedToScan3d_ = true;
424  scan3dSub_.subscribe(nh, "scan_cloud", 1);
425  if(subscribeOdomInfo)
426  {
427  subscribedToOdomInfo_ = true;
428  odomInfoSub_.subscribe(nh, "odom_info", 1);
429  SYNC_DECL6(depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
430  }
431  else
432  {
433  SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
434  }
435  }
436  else if(subscribeOdomInfo)
437  {
438  subscribedToOdomInfo_ = true;
439  odomInfoSub_.subscribe(nh, "odom_info", 1);
440  SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
441  }
442  else
443  {
444  SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
445  }
446  }
447  else if(subscribeUserData)
448  {
449  userDataSub_.subscribe(nh, "user_data", 1);
450 
451  if(subscribeScan2d)
452  {
453  subscribedToScan2d_ = true;
454  scanSub_.subscribe(nh, "scan", 1);
455 
456  if(subscribeOdomInfo)
457  {
458  subscribedToOdomInfo_ = true;
459  odomInfoSub_.subscribe(nh, "odom_info", 1);
460  SYNC_DECL6(depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
461  }
462  else
463  {
464  SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
465  }
466  }
467  else if(subscribeScan3d)
468  {
469  subscribedToScan3d_ = true;
470  scan3dSub_.subscribe(nh, "scan_cloud", 1);
471  if(subscribeOdomInfo)
472  {
473  subscribedToOdomInfo_ = true;
474  odomInfoSub_.subscribe(nh, "odom_info", 1);
475  SYNC_DECL6(depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
476  }
477  else
478  {
479  SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
480  }
481  }
482  else if(subscribeOdomInfo)
483  {
484  subscribedToOdomInfo_ = true;
485  odomInfoSub_.subscribe(nh, "odom_info", 1);
486  SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
487  }
488  else
489  {
490  SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
491  }
492  }
493  else
494  {
495  if(subscribeScan2d)
496  {
497  subscribedToScan2d_ = true;
498  scanSub_.subscribe(nh, "scan", 1);
499  if(subscribeOdomInfo)
500  {
501  subscribedToOdomInfo_ = true;
502  odomInfoSub_.subscribe(nh, "odom_info", 1);
503  SYNC_DECL5(depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
504  }
505  else
506  {
507  SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
508  }
509  }
510  else if(subscribeScan3d)
511  {
512  subscribedToScan3d_ = true;
513  scan3dSub_.subscribe(nh, "scan_cloud", 1);
514  if(subscribeOdomInfo)
515  {
516  subscribedToOdomInfo_ = true;
517  odomInfoSub_.subscribe(nh, "odom_info", 1);
518  SYNC_DECL5(depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
519  }
520  else
521  {
522  SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
523  }
524  }
525  else if(subscribeOdomInfo)
526  {
527  subscribedToOdomInfo_ = true;
528  odomInfoSub_.subscribe(nh, "odom_info", 1);
529  SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
530  }
531  else
532  {
533  SYNC_DECL3(depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
534  }
535  }
536 }
537 
538 } /* namespace rtabmap_ros */
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
rtabmap_ros::CommonDataSubscriber::commonSingleDepthCallback
void commonSingleDepthCallback(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::LaserScanConstPtr &scanMsg, const sensor_msgs::PointCloud2ConstPtr &scan3dMsg, const rtabmap_ros::OdomInfoConstPtr &odomInfoMsg)
Definition: CommonDataSubscriber.cpp:587
ros::NodeHandle
SYNC_DECL7
#define SYNC_DECL7(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6)
Definition: CommonDataSubscriberDefines.h:213
SYNC_DECL5
#define SYNC_DECL5(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4)
Definition: CommonDataSubscriberDefines.h:168
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
SYNC_DECL3
#define SYNC_DECL3(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2)
Definition: CommonDataSubscriberDefines.h:127
rtabmap_ros::CommonDataSubscriber::userDataSub_
message_filters::Subscriber< rtabmap_ros::UserData > userDataSub_
Definition: CommonDataSubscriber.h:199
rtabmap_ros::CommonDataSubscriber::subscribedToOdomInfo_
bool subscribedToOdomInfo_
Definition: CommonDataSubscriber.h:180
rtabmap_ros
Definition: CommonDataSubscriber.h:55
SYNC_DECL6
#define SYNC_DECL6(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5)
Definition: CommonDataSubscriberDefines.h:190
rtabmap_ros::CommonDataSubscriber::odomInfoSub_
message_filters::Subscriber< rtabmap_ros::OdomInfo > odomInfoSub_
Definition: CommonDataSubscriber.h:202
rtabmap_ros::CommonDataSubscriber::subscribedToScan2d_
bool subscribedToScan2d_
Definition: CommonDataSubscriber.h:178
rtabmap_ros::CommonDataSubscriber::imageDepthSub_
image_transport::SubscriberFilter imageDepthSub_
Definition: CommonDataSubscriber.h:185
ROS_INFO
#define ROS_INFO(...)
rtabmap_ros::CommonDataSubscriber::imageSub_
image_transport::SubscriberFilter imageSub_
Definition: CommonDataSubscriber.h:184
rtabmap_ros::CommonDataSubscriber::scanSub_
message_filters::Subscriber< sensor_msgs::LaserScan > scanSub_
Definition: CommonDataSubscriber.h:200
SYNC_DECL4
#define SYNC_DECL4(PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3)
Definition: CommonDataSubscriberDefines.h:147
rtabmap_ros::CommonDataSubscriber::odomSub_
message_filters::Subscriber< nav_msgs::Odometry > odomSub_
Definition: CommonDataSubscriber.h:198
rtabmap_ros::CommonDataSubscriber::subscribedToScan3d_
bool subscribedToScan3d_
Definition: CommonDataSubscriber.h:179
image_transport::SubscriberFilter::subscribe
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
rtabmap_ros::CommonDataSubscriber::scan3dSub_
message_filters::Subscriber< sensor_msgs::PointCloud2 > scan3dSub_
Definition: CommonDataSubscriber.h:201
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)
image_transport::TransportHints
image_transport::ImageTransport
rtabmap_ros::CommonDataSubscriber::cameraInfoSub_
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoSub_
Definition: CommonDataSubscriber.h:186
rtabmap_ros::CommonDataSubscriber::setupDepthCallbacks
void setupDepthCallbacks(ros::NodeHandle &nh, ros::NodeHandle &pnh, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, bool subscribeScan3d, bool subscribeOdomInfo, int queueSize, bool approxSync)
Definition: CommonDataSubscriberDepth.cpp:328

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:03