rgbd_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>
30 #include <nodelet/nodelet.h>
31 
32 #include <sensor_msgs/Image.h>
33 #include <sensor_msgs/CompressedImage.h>
35 #include <sensor_msgs/CameraInfo.h>
36 
39 
43 
44 #include <cv_bridge/cv_bridge.h>
45 #include <opencv2/highgui/highgui.hpp>
46 
47 #include <boost/thread.hpp>
48 
49 #include "rtabmap_ros/RGBDImage.h"
51 
53 #include "rtabmap/core/util2d.h"
55 
56 namespace rtabmap_ros
57 {
58 
59 class RGBDSync : public nodelet::Nodelet
60 {
61 public:
63  depthScale_(1.0),
64  decimation_(1),
65  compressedRate_(0),
66  warningThread_(0),
70  {}
71 
72  virtual ~RGBDSync()
73  {
75  delete approxSyncDepth_;
76  if(exactSyncDepth_)
77  delete exactSyncDepth_;
78 
79  if(warningThread_)
80  {
81  callbackCalled_=true;
82  warningThread_->join();
83  delete warningThread_;
84  }
85  }
86 
87 private:
88  virtual void onInit()
89  {
92 
93  int queueSize = 10;
94  bool approxSync = true;
95  pnh.param("approx_sync", approxSync, approxSync);
96  pnh.param("queue_size", queueSize, queueSize);
97  pnh.param("depth_scale", depthScale_, depthScale_);
98  pnh.param("decimation", decimation_, decimation_);
99  pnh.param("compressed_rate", compressedRate_, compressedRate_);
100 
101  if(decimation_<1)
102  {
103  decimation_ = 1;
104  }
105 
106  NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
107  NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
108  NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_);
109  NODELET_INFO("%s: decimation = %d", getName().c_str(), decimation_);
110  NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
111 
112  rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
113  rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
114 
115  if(approxSync)
116  {
118  approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
119  }
120  else
121  {
123  exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
124  }
125 
126  ros::NodeHandle rgb_nh(nh, "rgb");
127  ros::NodeHandle depth_nh(nh, "depth");
128  ros::NodeHandle rgb_pnh(pnh, "rgb");
129  ros::NodeHandle depth_pnh(pnh, "depth");
130  image_transport::ImageTransport rgb_it(rgb_nh);
131  image_transport::ImageTransport depth_it(depth_nh);
132  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
133  image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
134 
135  imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
136  imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
137  cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
138 
139  std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s",
140  getName().c_str(),
141  approxSync?"approx":"exact",
142  imageSub_.getTopic().c_str(),
143  imageDepthSub_.getTopic().c_str(),
144  cameraInfoSub_.getTopic().c_str());
145 
146  warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync));
147  NODELET_INFO("%s", subscribedTopicsMsg.c_str());
148  }
149 
150  void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
151  {
152  ros::Duration r(5.0);
153  while(!callbackCalled_)
154  {
155  r.sleep();
156  if(!callbackCalled_)
157  {
158  ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
159  "published (\"$ rostopic hz my_topic\") and the timestamps in their "
160  "header are set. %s%s",
161  getName().c_str(),
162  approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
163  "topics should have all the exact timestamp for the callback to be called.",
164  subscribedTopicsMsg.c_str());
165  }
166  }
167  }
168 
169  void callback(
170  const sensor_msgs::ImageConstPtr& image,
171  const sensor_msgs::ImageConstPtr& depth,
172  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
173  {
174  callbackCalled_ = true;
176  {
177  double rgbStamp = image->header.stamp.toSec();
178  double depthStamp = depth->header.stamp.toSec();
179  double infoStamp = cameraInfo->header.stamp.toSec();
180 
181  rtabmap_ros::RGBDImage msg;
182  msg.header.frame_id = cameraInfo->header.frame_id;
183  msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
184  if(decimation_>1 && !(depth->width % decimation_ == 0 && depth->height % decimation_ == 0))
185  {
186  ROS_WARN("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
187  "Images won't be resized.", decimation_, depth->width, depth->height);
188  decimation_ = 1;
189  }
190  if(decimation_>1)
191  {
193  sensor_msgs::CameraInfo info;
194  rtabmap_ros::cameraModelToROS(model.scaled(1.0f/float(decimation_)), info);
195  info.header = cameraInfo->header;
196  msg.rgb_camera_info = info;
197  msg.depth_camera_info = info;
198  }
199  else
200  {
201  msg.rgb_camera_info = *cameraInfo;
202  msg.depth_camera_info = *cameraInfo;
203  }
204 
205  cv::Mat rgbMat;
206  cv::Mat depthMat;
208  cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
209  rgbMat = imagePtr->image;
210  depthMat = imageDepthPtr->image;
211 
212  if(decimation_>1)
213  {
214  rgbMat = rtabmap::util2d::decimate(rgbMat, decimation_);
215  depthMat = rtabmap::util2d::decimate(depthMat, decimation_);
216  }
217 
218  if(depthScale_ != 1.0)
219  {
220  depthMat*=depthScale_;
221  }
222 
224  {
225  bool publishCompressed = true;
226  if (compressedRate_ > 0.0)
227  {
229  {
230  NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
231  publishCompressed = false;
232  }
233  }
234 
235  if(publishCompressed)
236  {
238 
239  rtabmap_ros::RGBDImage msgCompressed;
240  msgCompressed.header = msg.header;
241  msgCompressed.rgb_camera_info = msg.rgb_camera_info;
242  msgCompressed.depth_camera_info = msg.depth_camera_info;
243 
244  cv_bridge::CvImage cvImg;
245  cvImg.header = image->header;
246  cvImg.image = rgbMat;
247  cvImg.encoding = image->encoding;
248  cvImg.toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
249 
250  msgCompressed.depth_compressed.header = imageDepthPtr->header;
251  msgCompressed.depth_compressed.data = rtabmap::compressImage(depthMat, ".png");
252 
253  msgCompressed.depth_compressed.format = "png";
254 
255  rgbdImageCompressedPub_.publish(msgCompressed);
256  }
257  }
258 
260  {
261  cv_bridge::CvImage cvImg;
262  cvImg.header = image->header;
263  cvImg.image = rgbMat;
264  cvImg.encoding = image->encoding;
265  cvImg.toImageMsg(msg.rgb);
266 
267  cv_bridge::CvImage cvDepth;
268  cvDepth.header = depth->header;
269  cvDepth.image = depthMat;
270  cvDepth.encoding = depth->encoding;
271  cvDepth.toImageMsg(msg.depth);
272 
273  rgbdImagePub_.publish(msg);
274  }
275 
276  if( rgbStamp != image->header.stamp.toSec() ||
277  depthStamp != depth->header.stamp.toSec())
278  {
279  NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
280  "sure the node publishing the topics doesn't override the same data after publishing them. A "
281  "solution is to use this node within another nodelet manager. Stamps: "
282  "rgb=%f->%f depth=%f->%f",
283  rgbStamp, image->header.stamp.toSec(),
284  depthStamp, depth->header.stamp.toSec());
285  }
286  }
287  }
288 
289 private:
290  double depthScale_;
293  boost::thread * warningThread_;
295 
297 
300 
304 
307 
310 };
311 
313 }
314 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
std::string getTopic() const
std::string uFormat(const char *fmt,...)
message_filters::Synchronizer< MyApproxSyncDepthPolicy > * approxSyncDepth_
Definition: rgbd_sync.cpp:306
#define NODELET_ERROR(...)
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > MyExactSyncDepthPolicy
Definition: rgbd_sync.cpp:308
void publish(const boost::shared_ptr< M > &message) const
std::string resolveName(const std::string &name, bool remap=true) const
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > MyApproxSyncDepthPolicy
Definition: rgbd_sync.cpp:305
bool sleep() const
const std::string & getName() const
std::string encoding
void callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &cameraInfo)
Definition: rgbd_sync.cpp:169
image_transport::SubscriberFilter imageSub_
Definition: rgbd_sync.cpp:301
virtual void onInit()
Definition: rgbd_sync.cpp:88
#define ROS_WARN(...)
ros::Publisher rgbdImagePub_
Definition: rgbd_sync.cpp:298
message_filters::Synchronizer< MyExactSyncDepthPolicy > * exactSyncDepth_
Definition: rgbd_sync.cpp:309
ros::NodeHandle & getPrivateNodeHandle() const
std::vector< unsigned char > RTABMAP_EXP compressImage(const cv::Mat &image, const std::string &format=".png")
cv::Mat RTABMAP_EXP decimate(const cv::Mat &image, int d)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void warningLoop(const std::string &subscribedTopicsMsg, bool approxSync)
Definition: rgbd_sync.cpp:150
rtabmap::CameraModel cameraModelFromROS(const sensor_msgs::CameraInfo &camInfo, const rtabmap::Transform &localTransform=rtabmap::Transform::getIdentity())
void cameraModelToROS(const rtabmap::CameraModel &model, sensor_msgs::CameraInfo &camInfo)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle & getNodeHandle() const
sensor_msgs::CompressedImagePtr toCompressedImageMsg(const Format dst_format=JPG) const
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
#define false
#define NODELET_INFO(...)
uint32_t getNumSubscribers() const
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)
ros::Time lastCompressedPublished_
Definition: rgbd_sync.cpp:296
static Time now()
image_transport::SubscriberFilter imageDepthSub_
Definition: rgbd_sync.cpp:302
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
boost::thread * warningThread_
Definition: rgbd_sync.cpp:293
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoSub_
Definition: rgbd_sync.cpp:303
r
#define NODELET_DEBUG(...)
std::string getTopic() const
sensor_msgs::ImagePtr toImageMsg() const
CameraModel scaled(double scale) const
std_msgs::Header header
ros::Publisher rgbdImageCompressedPub_
Definition: rgbd_sync.cpp:299


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