CommonDataSubscriberRGBD6.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_sync/CommonDataSubscriber.h>
29 #include <rtabmap/utilite/UConversion.h>
30 #include <rtabmap/core/Compression.h>
31 #include <rtabmap_conversions/MsgConversion.h>
32 #include <cv_bridge/cv_bridge.h>
33 
34 namespace rtabmap_sync {
35 
36 #define IMAGE_CONVERSION() \
37  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6); \
38  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6); \
39  rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
40  rtabmap_conversions::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
41  rtabmap_conversions::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
42  rtabmap_conversions::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
43  rtabmap_conversions::toCvShare(image5Msg, imageMsgs[4], depthMsgs[4]); \
44  rtabmap_conversions::toCvShare(image6Msg, imageMsgs[5], depthMsgs[5]); \
45  if(!depthMsgs[0].get()) \
46  depthMsgs.clear(); \
47  std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
48  cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
49  cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
50  cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
51  cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
52  cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
53  cameraInfoMsgs.push_back(image6Msg->rgb_camera_info); \
54  std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
55  depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
56  depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
57  depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
58  depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
59  depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
60  depthCameraInfoMsgs.push_back(image6Msg->depth_camera_info); \
61  std::vector<rtabmap_msgs::GlobalDescriptor> globalDescriptorMsgs; \
62  std::vector<std::vector<rtabmap_msgs::KeyPoint> > localKeyPoints; \
63  std::vector<std::vector<rtabmap_msgs::Point3f> > localPoints3d; \
64  std::vector<cv::Mat> localDescriptors; \
65  if(!image1Msg->global_descriptor.data.empty()) \
66  globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
67  if(!image2Msg->global_descriptor.data.empty()) \
68  globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
69  if(!image3Msg->global_descriptor.data.empty()) \
70  globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
71  if(!image4Msg->global_descriptor.data.empty()) \
72  globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
73  if(!image5Msg->global_descriptor.data.empty()) \
74  globalDescriptorMsgs.push_back(image5Msg->global_descriptor); \
75  if(!image6Msg->global_descriptor.data.empty()) \
76  globalDescriptorMsgs.push_back(image6Msg->global_descriptor); \
77  localKeyPoints.push_back(image1Msg->key_points); \
78  localKeyPoints.push_back(image2Msg->key_points); \
79  localKeyPoints.push_back(image3Msg->key_points); \
80  localKeyPoints.push_back(image4Msg->key_points); \
81  localKeyPoints.push_back(image5Msg->key_points); \
82  localKeyPoints.push_back(image6Msg->key_points); \
83  localPoints3d.push_back(image1Msg->points); \
84  localPoints3d.push_back(image2Msg->points); \
85  localPoints3d.push_back(image3Msg->points); \
86  localPoints3d.push_back(image4Msg->points); \
87  localPoints3d.push_back(image5Msg->points); \
88  localPoints3d.push_back(image6Msg->points); \
89  localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
90  localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
91  localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
92  localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors)); \
93  localDescriptors.push_back(rtabmap::uncompressData(image5Msg->descriptors)); \
94  localDescriptors.push_back(rtabmap::uncompressData(image6Msg->descriptors));
95 
96 // 6 RGBD
97 void CommonDataSubscriber::rgbd6Callback(
98  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
99  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
100  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
101  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
102  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
103  const rtabmap_msgs::RGBDImageConstPtr& image6Msg)
104 {
105  IMAGE_CONVERSION();
106 
107  nav_msgs::OdometryConstPtr odomMsg; // Null
108  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
109  sensor_msgs::LaserScan scanMsg; // Null
110  sensor_msgs::PointCloud2 scan3dMsg; // Null
111  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
112  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
113 }
114 void CommonDataSubscriber::rgbd6Scan2dCallback(
115  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
116  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
117  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
118  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
119  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
120  const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
121  const sensor_msgs::LaserScanConstPtr& scanMsg)
122 {
123  IMAGE_CONVERSION();
124 
125  nav_msgs::OdometryConstPtr odomMsg; // Null
126  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
127  sensor_msgs::PointCloud2 scan3dMsg; // Null
128  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
129  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
130 }
131 void CommonDataSubscriber::rgbd6Scan3dCallback(
132  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
133  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
134  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
135  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
136  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
137  const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
138  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
139 {
140  IMAGE_CONVERSION();
141 
142  nav_msgs::OdometryConstPtr odomMsg; // Null
143  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
144  sensor_msgs::LaserScan scanMsg; // Null
145  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
146  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
147 }
148 void CommonDataSubscriber::rgbd6ScanDescCallback(
149  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
150  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
151  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
152  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
153  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
154  const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
155  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
156 {
157  IMAGE_CONVERSION();
158 
159  nav_msgs::OdometryConstPtr odomMsg; // Null
160  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
161  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
162  if(!scanDescMsg->global_descriptor.data.empty())
163  {
164  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
165  }
166  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
167 }
168 void CommonDataSubscriber::rgbd6InfoCallback(
169  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
170  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
171  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
172  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
173  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
174  const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
175  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
176 {
177  IMAGE_CONVERSION();
178 
179  nav_msgs::OdometryConstPtr odomMsg; // Null
180  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
181  sensor_msgs::LaserScan scanMsg; // Null
182  sensor_msgs::PointCloud2 scan3dMsg; // Null
183  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
184 }
185 
186 // 6 RGBD + Odom
187 void CommonDataSubscriber::rgbd6OdomCallback(
188  const nav_msgs::OdometryConstPtr & odomMsg,
189  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
190  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
191  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
192  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
193  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
194  const rtabmap_msgs::RGBDImageConstPtr& image6Msg)
195 {
196  IMAGE_CONVERSION();
197 
198  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
199  sensor_msgs::LaserScan scanMsg; // Null
200  sensor_msgs::PointCloud2 scan3dMsg; // Null
201  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
202  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
203 }
204 void CommonDataSubscriber::rgbd6OdomScan2dCallback(
205  const nav_msgs::OdometryConstPtr & odomMsg,
206  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
207  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
208  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
209  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
210  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
211  const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
212  const sensor_msgs::LaserScanConstPtr& scanMsg)
213 {
214  IMAGE_CONVERSION();
215 
216  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
217  sensor_msgs::PointCloud2 scan3dMsg; // Null
218  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
219  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
220 }
221 void CommonDataSubscriber::rgbd6OdomScan3dCallback(
222  const nav_msgs::OdometryConstPtr & odomMsg,
223  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
224  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
225  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
226  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
227  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
228  const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
229  const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
230 {
231  IMAGE_CONVERSION();
232 
233  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
234  sensor_msgs::LaserScan scanMsg; // Null
235  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
236  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
237 }
238 void CommonDataSubscriber::rgbd6OdomScanDescCallback(
239  const nav_msgs::OdometryConstPtr & odomMsg,
240  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
241  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
242  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
243  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
244  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
245  const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
246  const rtabmap_msgs::ScanDescriptorConstPtr& scanDescMsg)
247 {
248  IMAGE_CONVERSION();
249 
250  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
251  rtabmap_msgs::OdomInfoConstPtr odomInfoMsg; // null
252  if(!scanDescMsg->global_descriptor.data.empty())
253  {
254  globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
255  }
256  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
257 }
258 void CommonDataSubscriber::rgbd6OdomInfoCallback(
259  const nav_msgs::OdometryConstPtr & odomMsg,
260  const rtabmap_msgs::RGBDImageConstPtr& image1Msg,
261  const rtabmap_msgs::RGBDImageConstPtr& image2Msg,
262  const rtabmap_msgs::RGBDImageConstPtr& image3Msg,
263  const rtabmap_msgs::RGBDImageConstPtr& image4Msg,
264  const rtabmap_msgs::RGBDImageConstPtr& image5Msg,
265  const rtabmap_msgs::RGBDImageConstPtr& image6Msg,
266  const rtabmap_msgs::OdomInfoConstPtr& odomInfoMsg)
267 {
268  IMAGE_CONVERSION();
269 
270  rtabmap_msgs::UserDataConstPtr userDataMsg; // Null
271  sensor_msgs::LaserScan scanMsg; // Null
272  sensor_msgs::PointCloud2 scan3dMsg; // Null
273  commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
274 }
275 
276 void CommonDataSubscriber::setupRGBD6Callbacks(
277  ros::NodeHandle & nh,
278  ros::NodeHandle & pnh,
279  bool subscribeOdom,
280  bool subscribeUserData,
281  bool subscribeScan2d,
282  bool subscribeScan3d,
283  bool subscribeScanDesc,
284  bool subscribeOdomInfo)
285 {
286  ROS_INFO("Setup rgbd6 callback");
287 
288  rgbdSubs_.resize(6);
289  for(int i=0; i<6; ++i)
290  {
291  rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>;
292  rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_);
293  }
294  if(subscribeOdom)
295  {
296  odomSub_.subscribe(nh, "odom", topicQueueSize_);
297  if(subscribeScanDesc)
298  {
299  subscribedToScanDescriptor_ = true;
300  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
301  if(subscribeOdomInfo)
302  {
303  subscribedToOdomInfo_ = false;
304  ROS_WARN("subscribe_odom_info ignored...");
305  }
306  SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
307  }
308  else if(subscribeScan2d)
309  {
310  subscribedToScan2d_ = true;
311  scanSub_.subscribe(nh, "scan", topicQueueSize_);
312  if(subscribeOdomInfo)
313  {
314  subscribedToOdomInfo_ = false;
315  ROS_WARN("subscribe_odom_info ignored...");
316  }
317  SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
318  }
319  else if(subscribeScan3d)
320  {
321  subscribedToScan3d_ = true;
322  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
323  if(subscribeOdomInfo)
324  {
325  subscribedToOdomInfo_ = false;
326  ROS_WARN("subscribe_odom_info ignored...");
327  }
328  SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
329  }
330  else if(subscribeOdomInfo)
331  {
332  subscribedToOdomInfo_ = true;
333  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
334  SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
335  }
336  else
337  {
338  SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
339  }
340  }
341  else
342  {
343  if(subscribeScanDesc)
344  {
345  subscribedToScanDescriptor_ = true;
346  scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
347  if(subscribeOdomInfo)
348  {
349  subscribedToOdomInfo_ = false;
350  ROS_WARN("subscribe_odom_info ignored...");
351  }
352  SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
353  }
354  else if(subscribeScan2d)
355  {
356  subscribedToScan2d_ = true;
357  scanSub_.subscribe(nh, "scan", topicQueueSize_);
358  if(subscribeOdomInfo)
359  {
360  subscribedToOdomInfo_ = false;
361  ROS_WARN("subscribe_odom_info ignored...");
362  }
363  SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
364  }
365  else if(subscribeScan3d)
366  {
367  subscribedToScan3d_ = true;
368  scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
369  if(subscribeOdomInfo)
370  {
371  subscribedToOdomInfo_ = false;
372  ROS_WARN("subscribe_odom_info ignored...");
373  }
374  SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
375  }
376  else if(subscribeOdomInfo)
377  {
378  subscribedToOdomInfo_ = true;
379  odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
380  SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
381  }
382  else
383  {
384  SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
385  }
386  }
387 }
388 
389 } /* namespace rtabmap_sync */
SYNC_DECL6
#define SYNC_DECL6(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5)
Definition: CommonDataSubscriberDefines.h:190
rtabmap_sync
Definition: CommonDataSubscriber.h:59
SYNC_DECL8
#define SYNC_DECL8(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7)
Definition: CommonDataSubscriberDefines.h:237
uFormat
std::string uFormat(const char *fmt,...)
Compression.h
rtabmap_sync::CommonDataSubscriber::scan3dSub_
message_filters::Subscriber< sensor_msgs::PointCloud2 > scan3dSub_
Definition: CommonDataSubscriber.h:286
rtabmap_sync::CommonDataSubscriber::odomInfoSub_
message_filters::Subscriber< rtabmap_msgs::OdomInfo > odomInfoSub_
Definition: CommonDataSubscriber.h:288
CommonDataSubscriber.h
rtabmap_sync::CommonDataSubscriber::syncQueueSize_
int syncQueueSize_
Definition: CommonDataSubscriber.h:246
message_filters::Subscriber
rtabmap_sync::CommonDataSubscriber::scanDescSub_
message_filters::Subscriber< rtabmap_msgs::ScanDescriptor > scanDescSub_
Definition: CommonDataSubscriber.h:287
rtabmap_sync::CommonDataSubscriber::topicQueueSize_
int topicQueueSize_
Definition: CommonDataSubscriber.h:245
rtabmap_sync::CommonDataSubscriber::CommonDataSubscriber
CommonDataSubscriber(bool gui)
Definition: CommonDataSubscriber.cpp:32
rtabmap_sync::CommonDataSubscriber::subscribedToScan3d_
bool subscribedToScan3d_
Definition: CommonDataSubscriber.h:257
rtabmap_sync::CommonDataSubscriber::scanSub_
message_filters::Subscriber< sensor_msgs::LaserScan > scanSub_
Definition: CommonDataSubscriber.h:285
IMAGE_CONVERSION
#define IMAGE_CONVERSION()
Definition: CommonDataSubscriberRGBD6.cpp:36
rtabmap_sync::CommonDataSubscriber::subscribedToScanDescriptor_
bool subscribedToScanDescriptor_
Definition: CommonDataSubscriber.h:258
ROS_WARN
#define ROS_WARN(...)
message_filters::Subscriber::subscribe
void subscribe()
rtabmap_sync::CommonDataSubscriber::approxSync_
bool approxSync_
Definition: CommonDataSubscriber.h:249
rtabmap_sync::CommonDataSubscriber::rgbdSubs_
std::vector< message_filters::Subscriber< rtabmap_msgs::RGBDImage > * > rgbdSubs_
Definition: CommonDataSubscriber.h:269
rtabmap_sync::CommonDataSubscriber::odomSub_
message_filters::Subscriber< nav_msgs::Odometry > odomSub_
Definition: CommonDataSubscriber.h:283
cv_bridge.h
rtabmap_sync::CommonDataSubscriber::subscribedToOdomInfo_
bool subscribedToOdomInfo_
Definition: CommonDataSubscriber.h:259
rtabmap_sync::CommonDataSubscriber::subscribedToScan2d_
bool subscribedToScan2d_
Definition: CommonDataSubscriber.h:256
ROS_INFO
#define ROS_INFO(...)
UConversion.h
MsgConversion.h
rtabmap_sync::CommonDataSubscriber::commonMultiCameraCallback
virtual void commonMultiCameraCallback(const nav_msgs::OdometryConstPtr &odomMsg, const rtabmap_msgs::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_msgs::OdomInfoConstPtr &odomInfoMsg, const std::vector< rtabmap_msgs::GlobalDescriptor > &globalDescriptorMsgs=std::vector< rtabmap_msgs::GlobalDescriptor >(), const std::vector< std::vector< rtabmap_msgs::KeyPoint > > &localKeyPoints=std::vector< std::vector< rtabmap_msgs::KeyPoint > >(), const std::vector< std::vector< rtabmap_msgs::Point3f > > &localPoints3d=std::vector< std::vector< rtabmap_msgs::Point3f > >(), const std::vector< cv::Mat > &localDescriptors=std::vector< cv::Mat >())=0
i
int i
ros::NodeHandle
SYNC_DECL7
#define SYNC_DECL7(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6)
Definition: CommonDataSubscriberDefines.h:213

rtabmap_sync
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:39:12