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

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