rgbdx_sync.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 <ros/ros.h>
29 #include <pluginlib/class_list_macros.hpp>
30 #include <nodelet/nodelet.h>
31 
32 #include <message_filters/sync_policies/approximate_time.h>
33 #include <message_filters/sync_policies/exact_time.h>
34 #include <message_filters/subscriber.h>
35 
36 #include <boost/thread.hpp>
37 
38 #include "rtabmap_ros/RGBDImages.h"
39 #include "rtabmap_ros/CommonDataSubscriber.h"
40 
41 namespace rtabmap_ros
42 {
43 
44 class RGBDXSync : public nodelet::Nodelet
45 {
46 public:
47  RGBDXSync() :
48  warningThread_(0),
49  callbackCalled_(false),
50  SYNC_INIT(rgbd2),
51  SYNC_INIT(rgbd3),
52  SYNC_INIT(rgbd4),
53  SYNC_INIT(rgbd5),
54  SYNC_INIT(rgbd6),
55  SYNC_INIT(rgbd7),
56  SYNC_INIT(rgbd8)
57  {}
58 
59  virtual ~RGBDXSync()
60  {
61  SYNC_DEL(rgbd2);
62 
63  if(warningThread_)
64  {
65  callbackCalled_=true;
66  warningThread_->join();
67  delete warningThread_;
68  }
69  }
70 
71 private:
72 
73  virtual void onInit()
74  {
75  ros::NodeHandle & nh = getNodeHandle();
76  ros::NodeHandle & pnh = getPrivateNodeHandle();
77 
78  int queueSize = 10;
79  bool approxSync = true;
80  int rgbdCameras = 2;
81  double approxSyncMaxInterval = 0.0;
82  pnh.param("approx_sync", approxSync, approxSync);
83  pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
84  pnh.param("queue_size", queueSize, queueSize);
85  pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
86 
87  NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
88  if(approxSync)
89  NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
90  NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
91  NODELET_INFO("%s: rgbd_cameras = %d", getName().c_str(), rgbdCameras);
92 
93  rgbdImagesPub_ = nh.advertise<rtabmap_ros::RGBDImages>("rgbd_images", 1);
94 
95  ROS_ASSERT(rgbdCameras>=2 && rgbdCameras<=8);
96 
97  rgbdSubs_.resize(rgbdCameras);
98  for(int i=0; i<rgbdCameras; ++i)
99  {
100  rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
101  rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize);
102  }
103 
104  std::string name_ = this->getName();
105  std::string subscribedTopicsMsg_;
106  if(rgbdCameras==2)
107  {
108  SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
109  if(approxSync && approxSyncMaxInterval>0.0)
110  {
111  rgbd2ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
112  }
113  }
114  else if(rgbdCameras==3)
115  {
116  SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
117  if(approxSync && approxSyncMaxInterval>0.0)
118  {
119  rgbd3ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
120  }
121  }
122  else if(rgbdCameras==4)
123  {
124  SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
125  if(approxSync && approxSyncMaxInterval>0.0)
126  {
127  rgbd4ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
128  }
129  }
130  else if(rgbdCameras==5)
131  {
132  SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
133  if(approxSync && approxSyncMaxInterval>0.0)
134  {
135  rgbd5ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
136  }
137  }
138  else if(rgbdCameras==6)
139  {
140  SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
141  if(approxSync && approxSyncMaxInterval>0.0)
142  {
143  rgbd6ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
144  }
145  }
146  else if(rgbdCameras==7)
147  {
148  SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
149  if(approxSync && approxSyncMaxInterval>0.0)
150  {
151  rgbd7ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
152  }
153  }
154  else if(rgbdCameras==8)
155  {
156  SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
157  if(approxSync && approxSyncMaxInterval>0.0)
158  {
159  rgbd8ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
160  }
161  }
162 
163  warningThread_ = new boost::thread(boost::bind(&RGBDXSync::warningLoop, this, subscribedTopicsMsg_, approxSync));
164  NODELET_INFO("%s%s", subscribedTopicsMsg_.c_str(),
165  approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
166  }
167 
168  void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
169  {
170  ros::Duration r(5.0);
171  while(!callbackCalled_)
172  {
173  r.sleep();
174  if(!callbackCalled_)
175  {
176  ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
177  "published (\"$ rostopic hz my_topic\") and the timestamps in their "
178  "header are set. %s%s",
179  getName().c_str(),
180  approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
181  "topics should have all the exact timestamp for the callback to be called.",
182  subscribedTopicsMsg.c_str());
183  }
184  }
185  }
186 
187  DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
188  DATA_SYNCS3(rgbd3, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
189  DATA_SYNCS4(rgbd4, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
190  DATA_SYNCS5(rgbd5, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
191  DATA_SYNCS6(rgbd6, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
192  DATA_SYNCS7(rgbd7, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
193  DATA_SYNCS8(rgbd8, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage);
194 
195 private:
196  boost::thread * warningThread_;
197  bool callbackCalled_;
198 
199  ros::Publisher rgbdImagesPub_;
200 
201  std::vector<message_filters::Subscriber<rtabmap_ros::RGBDImage>*> rgbdSubs_;
202 };
203 
204 
205 void RGBDXSync::rgbd2Callback(
206  const rtabmap_ros::RGBDImageConstPtr& image0,
207  const rtabmap_ros::RGBDImageConstPtr& image1)
208 {
209  callbackCalled_ = true;
210  rtabmap_ros::RGBDImages output;
211  output.header = image0->header;
212  output.rgbd_images.resize(2);
213  output.rgbd_images[0]=(*image0);
214  output.rgbd_images[1]=(*image1);
215  rgbdImagesPub_.publish(output);
216 }
217 
218 void RGBDXSync::rgbd3Callback(
219  const rtabmap_ros::RGBDImageConstPtr& image0,
220  const rtabmap_ros::RGBDImageConstPtr& image1,
221  const rtabmap_ros::RGBDImageConstPtr& image2)
222 {
223  callbackCalled_ = true;
224  rtabmap_ros::RGBDImages output;
225  output.header = image0->header;
226  output.rgbd_images.resize(3);
227  output.rgbd_images[0]=(*image0);
228  output.rgbd_images[1]=(*image1);
229  output.rgbd_images[2]=(*image2);
230  rgbdImagesPub_.publish(output);
231 }
232 
233 void RGBDXSync::rgbd4Callback(
234  const rtabmap_ros::RGBDImageConstPtr& image0,
235  const rtabmap_ros::RGBDImageConstPtr& image1,
236  const rtabmap_ros::RGBDImageConstPtr& image2,
237  const rtabmap_ros::RGBDImageConstPtr& image3)
238 {
239  callbackCalled_ = true;
240  rtabmap_ros::RGBDImages output;
241  output.header = image0->header;
242  output.rgbd_images.resize(4);
243  output.rgbd_images[0]=(*image0);
244  output.rgbd_images[1]=(*image1);
245  output.rgbd_images[2]=(*image2);
246  output.rgbd_images[3]=(*image3);
247  rgbdImagesPub_.publish(output);
248 }
249 
250 void RGBDXSync::rgbd5Callback(
251  const rtabmap_ros::RGBDImageConstPtr& image0,
252  const rtabmap_ros::RGBDImageConstPtr& image1,
253  const rtabmap_ros::RGBDImageConstPtr& image2,
254  const rtabmap_ros::RGBDImageConstPtr& image3,
255  const rtabmap_ros::RGBDImageConstPtr& image4)
256 {
257  callbackCalled_ = true;
258  rtabmap_ros::RGBDImages output;
259  output.header = image0->header;
260  output.rgbd_images.resize(5);
261  output.rgbd_images[0]=(*image0);
262  output.rgbd_images[1]=(*image1);
263  output.rgbd_images[2]=(*image2);
264  output.rgbd_images[3]=(*image3);
265  output.rgbd_images[4]=(*image4);
266  rgbdImagesPub_.publish(output);
267 }
268 
269 void RGBDXSync::rgbd6Callback(
270  const rtabmap_ros::RGBDImageConstPtr& image0,
271  const rtabmap_ros::RGBDImageConstPtr& image1,
272  const rtabmap_ros::RGBDImageConstPtr& image2,
273  const rtabmap_ros::RGBDImageConstPtr& image3,
274  const rtabmap_ros::RGBDImageConstPtr& image4,
275  const rtabmap_ros::RGBDImageConstPtr& image5)
276 {
277  callbackCalled_ = true;
278  rtabmap_ros::RGBDImages output;
279  output.header = image0->header;
280  output.rgbd_images.resize(6);
281  output.rgbd_images[0]=(*image0);
282  output.rgbd_images[1]=(*image1);
283  output.rgbd_images[2]=(*image2);
284  output.rgbd_images[3]=(*image3);
285  output.rgbd_images[4]=(*image4);
286  output.rgbd_images[5]=(*image5);
287  rgbdImagesPub_.publish(output);
288 }
289 
290 void RGBDXSync::rgbd7Callback(
291  const rtabmap_ros::RGBDImageConstPtr& image0,
292  const rtabmap_ros::RGBDImageConstPtr& image1,
293  const rtabmap_ros::RGBDImageConstPtr& image2,
294  const rtabmap_ros::RGBDImageConstPtr& image3,
295  const rtabmap_ros::RGBDImageConstPtr& image4,
296  const rtabmap_ros::RGBDImageConstPtr& image5,
297  const rtabmap_ros::RGBDImageConstPtr& image6)
298 {
299  callbackCalled_ = true;
300  rtabmap_ros::RGBDImages output;
301  output.header = image0->header;
302  output.rgbd_images.resize(7);
303  output.rgbd_images[0]=(*image0);
304  output.rgbd_images[1]=(*image1);
305  output.rgbd_images[2]=(*image2);
306  output.rgbd_images[3]=(*image3);
307  output.rgbd_images[4]=(*image4);
308  output.rgbd_images[5]=(*image5);
309  output.rgbd_images[6]=(*image6);
310  rgbdImagesPub_.publish(output);
311 }
312 
313 void RGBDXSync::rgbd8Callback(
314  const rtabmap_ros::RGBDImageConstPtr& image0,
315  const rtabmap_ros::RGBDImageConstPtr& image1,
316  const rtabmap_ros::RGBDImageConstPtr& image2,
317  const rtabmap_ros::RGBDImageConstPtr& image3,
318  const rtabmap_ros::RGBDImageConstPtr& image4,
319  const rtabmap_ros::RGBDImageConstPtr& image5,
320  const rtabmap_ros::RGBDImageConstPtr& image6,
321  const rtabmap_ros::RGBDImageConstPtr& image7)
322 {
323  callbackCalled_ = true;
324  rtabmap_ros::RGBDImages output;
325  output.header = image0->header;
326  output.rgbd_images.resize(8);
327  output.rgbd_images[0]=(*image0);
328  output.rgbd_images[1]=(*image1);
329  output.rgbd_images[2]=(*image2);
330  output.rgbd_images[3]=(*image3);
331  output.rgbd_images[4]=(*image4);
332  output.rgbd_images[5]=(*image5);
333  output.rgbd_images[6]=(*image6);
334  output.rgbd_images[7]=(*image7);
335  rgbdImagesPub_.publish(output);
336 }
337 
338 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDXSync, nodelet::Nodelet);
339 }
340 
rtabmap_ros::RGBDXSync::warningLoop
void warningLoop(const std::string &subscribedTopicsMsg, bool approxSync)
Definition: rgbdx_sync.cpp:168
uFormat
std::string uFormat(const char *fmt,...)
ros::NodeHandle
name_
std::string name_
SYNC_DECL3
#define SYNC_DECL3(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2)
Definition: CommonDataSubscriberDefines.h:127
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
SYNC_DEL
#define SYNC_DEL(PREFIX)
Definition: CommonDataSubscriberDefines.h:103
rtabmap_ros::RGBDXSync::onInit
virtual void onInit()
Definition: rgbdx_sync.cpp:73
SYNC_DECL8
#define SYNC_DECL8(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6, SUB7)
Definition: CommonDataSubscriberDefines.h:237
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
rtabmap_ros::RGBDXSync::DATA_SYNCS5
DATA_SYNCS5(rgbd5, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage)
rtabmap_ros
Definition: CommonDataSubscriber.h:57
ROS_WARN
#define ROS_WARN(...)
rtabmap_ros::RGBDXSync::DATA_SYNCS3
DATA_SYNCS3(rgbd3, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage)
rtabmap_ros::RGBDXSync::DATA_SYNCS7
DATA_SYNCS7(rgbd7, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage)
rtabmap_ros::RGBDXSync::DATA_SYNCS8
DATA_SYNCS8(rgbd8, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage)
exact_time.h
rtabmap_ros::RGBDXSync::callbackCalled_
bool callbackCalled_
Definition: rgbdx_sync.cpp:197
nodelet::Nodelet::getName
const std::string & getName() const
class_list_macros.hpp
rtabmap_ros::RGBDXSync
Definition: rgbdx_sync.cpp:44
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
rtabmap_ros::RGBDXSync::DATA_SYNCS2
DATA_SYNCS2(rgbd2, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage)
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rtabmap_ros::RGBDXSync::RGBDXSync
RGBDXSync()
Definition: rgbdx_sync.cpp:47
SYNC_DECL5
#define SYNC_DECL5(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4)
Definition: CommonDataSubscriberDefines.h:168
approximate_time.h
rtabmap_ros::RGBDXSync::rgbdSubs_
std::vector< message_filters::Subscriber< rtabmap_ros::RGBDImage > * > rgbdSubs_
Definition: rgbdx_sync.cpp:201
rtabmap_ros::RGBDXSync::DATA_SYNCS6
DATA_SYNCS6(rgbd6, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage)
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
rtabmap_ros::RGBDXSync::DATA_SYNCS4
DATA_SYNCS4(rgbd4, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage)
false
#define false
rtabmap_ros::RGBDXSync::~RGBDXSync
virtual ~RGBDXSync()
Definition: rgbdx_sync.cpp:59
NODELET_INFO
#define NODELET_INFO(...)
SYNC_DECL6
#define SYNC_DECL6(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5)
Definition: CommonDataSubscriberDefines.h:190
SYNC_DECL7
#define SYNC_DECL7(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3, SUB4, SUB5, SUB6)
Definition: CommonDataSubscriberDefines.h:213
rtabmap_ros::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
rtabmap_ros::RGBDXSync::warningThread_
boost::thread * warningThread_
Definition: rgbdx_sync.cpp:196
ros::Duration::sleep
bool sleep() const
message_filters::Subscriber< rtabmap_ros::RGBDImage >
ROS_ASSERT
#define ROS_ASSERT(cond)
r
r
SYNC_DECL4
#define SYNC_DECL4(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1, SUB2, SUB3)
Definition: CommonDataSubscriberDefines.h:147
SYNC_DECL2
#define SYNC_DECL2(CLASS, PREFIX, APPROX, QUEUE_SIZE, SUB0, SUB1)
Definition: CommonDataSubscriberDefines.h:108
SYNC_INIT
#define SYNC_INIT(PREFIX)
Definition: CommonDataSubscriberDefines.h:98
rtabmap_ros::RGBDXSync::rgbdImagesPub_
ros::Publisher rgbdImagesPub_
Definition: rgbdx_sync.cpp:199

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