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

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:18