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