rgb_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"
50 
53 
54 namespace rtabmap_ros
55 {
56 
57 class RgbSync : public nodelet::Nodelet
58 {
59 public:
60  RgbSync() :
61  compressedRate_(0),
62  warningThread_(0),
64  approxSync_(0),
65  exactSync_(0)
66  {}
67 
68  virtual ~RgbSync()
69  {
70  if(approxSync_)
71  delete approxSync_;
72  if(exactSync_)
73  delete exactSync_;
74 
75  if(warningThread_)
76  {
77  callbackCalled_=true;
78  warningThread_->join();
79  delete warningThread_;
80  }
81  }
82 
83 private:
84  virtual void onInit()
85  {
88 
89  int queueSize = 10;
90  bool approxSync = false;
91  double approxSyncMaxInterval = 0.0;
92  pnh.param("approx_sync", approxSync, approxSync);
93  pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
94  pnh.param("queue_size", queueSize, queueSize);
95  pnh.param("compressed_rate", compressedRate_, compressedRate_);
96 
97  NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
98  if(approxSync)
99  NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
100  NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
101  NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
102 
103  rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
104  rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
105 
106  if(approxSync)
107  {
109  if(approxSyncMaxInterval > 0.0)
110  approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
111  approxSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2));
112  }
113  else
114  {
116  exactSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2));
117  }
118 
119  ros::NodeHandle rgb_nh(nh, "rgb");
120  ros::NodeHandle rgb_pnh(pnh, "rgb");
121  image_transport::ImageTransport rgb_it(rgb_nh);
122  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
123 
124  imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), 1, hintsRgb);
125  cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
126 
127  std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
128  getName().c_str(),
129  approxSync?"approx":"exact",
130  approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
131  imageSub_.getTopic().c_str(),
132  cameraInfoSub_.getTopic().c_str());
133 
134  warningThread_ = new boost::thread(boost::bind(&RgbSync::warningLoop, this, subscribedTopicsMsg, approxSync));
135  NODELET_INFO("%s", subscribedTopicsMsg.c_str());
136  }
137 
138  void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
139  {
140  ros::Duration r(5.0);
141  while(!callbackCalled_)
142  {
143  r.sleep();
144  if(!callbackCalled_)
145  {
146  ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
147  "published (\"$ rostopic hz my_topic\") and the timestamps in their "
148  "header are set. %s%s",
149  getName().c_str(),
150  approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
151  "topics should have all the exact timestamp for the callback to be called.",
152  subscribedTopicsMsg.c_str());
153  }
154  }
155  }
156 
157  void callback(
158  const sensor_msgs::ImageConstPtr& image,
159  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
160  {
161  callbackCalled_ = true;
163  {
164  double stamp = image->header.stamp.toSec();
165 
166  rtabmap_ros::RGBDImage msg;
167  msg.header.frame_id = cameraInfo->header.frame_id;
168  msg.header.stamp = image->header.stamp;
169  msg.rgb_camera_info = *cameraInfo;
170 
172  {
173  bool publishCompressed = true;
174  if (compressedRate_ > 0.0)
175  {
177  {
178  NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
179  publishCompressed = false;
180  }
181  }
182 
183  if(publishCompressed)
184  {
186 
187  rtabmap_ros::RGBDImage msgCompressed = msg;
188 
190  imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG);
191 
192  rgbdImageCompressedPub_.publish(msgCompressed);
193  }
194  }
195 
197  {
198  msg.rgb = *image;
199  rgbdImagePub_.publish(msg);
200  }
201 
202  if( stamp != image->header.stamp.toSec())
203  {
204  NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
205  "sure the node publishing the topics doesn't override the same data after publishing them. A "
206  "solution is to use this node within another nodelet manager. Stamps: "
207  "%f->%f",
208  stamp, image->header.stamp.toSec());
209  }
210  }
211  }
212 
213 private:
215  boost::thread * warningThread_;
218 
221 
224 
227 
230 };
231 
233 }
234 
boost::thread * warningThread_
Definition: rgb_sync.cpp:215
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::CameraInfo > MyApproxSyncPolicy
Definition: rgb_sync.cpp:225
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
std::string uFormat(const char *fmt,...)
#define NODELET_ERROR(...)
void callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::CameraInfoConstPtr &cameraInfo)
Definition: rgb_sync.cpp:157
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::CameraInfo > MyExactSyncPolicy
Definition: rgb_sync.cpp:228
message_filters::Synchronizer< MyExactSyncPolicy > * exactSync_
Definition: rgb_sync.cpp:229
std::string getTopic() const
ros::NodeHandle & getNodeHandle() const
void warningLoop(const std::string &subscribedTopicsMsg, bool approxSync)
Definition: rgb_sync.cpp:138
ros::NodeHandle & getPrivateNodeHandle() const
#define ROS_WARN(...)
virtual ~RgbSync()
Definition: rgb_sync.cpp:68
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoSub_
Definition: rgb_sync.cpp:223
const std::string & getName() const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
std::string resolveName(const std::string &name, bool remap=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
#define false
image_transport::SubscriberFilter imageSub_
Definition: rgb_sync.cpp:222
#define NODELET_INFO(...)
ros::Time lastCompressedPublished_
Definition: rgb_sync.cpp:217
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
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)
message_filters::Synchronizer< MyApproxSyncPolicy > * approxSync_
Definition: rgb_sync.cpp:226
static Time now()
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
virtual void onInit()
Definition: rgb_sync.cpp:84
bool sleep() const
uint32_t getNumSubscribers() const
r
#define NODELET_DEBUG(...)
ros::Publisher rgbdImageCompressedPub_
Definition: rgb_sync.cpp:220
ros::Publisher rgbdImagePub_
Definition: rgb_sync.cpp:219


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