stereo_throttle.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 
35 
38 
39 #include <sensor_msgs/CameraInfo.h>
40 
41 #include <cv_bridge/cv_bridge.h>
42 
43 #include <rtabmap/core/util2d.h>
44 
45 namespace rtabmap_ros
46 {
47 
49 {
50 public:
51  //Constructor
53  rate_(0),
54  approxSync_(0),
55  exactSync_(0),
56  decimation_(1)
57  {
58  }
59 
61  {
62  if(approxSync_)
63  {
64  delete approxSync_;
65  }
66  if(exactSync_)
67  {
68  delete exactSync_;
69  }
70  }
71 
72 private:
74  double rate_;
75  virtual void onInit()
76  {
79 
80  ros::NodeHandle left_nh(nh, "left");
81  ros::NodeHandle right_nh(nh, "right");
82  ros::NodeHandle left_pnh(pnh, "left");
83  ros::NodeHandle right_pnh(pnh, "right");
84  image_transport::ImageTransport left_it(left_nh);
85  image_transport::ImageTransport right_it(right_nh);
86  image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
87  image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
88 
89  int queueSize = 5;
90  bool approxSync = false;
91  pnh.param("approx_sync", approxSync, approxSync);
92  pnh.param("rate", rate_, rate_);
93  pnh.param("queue_size", queueSize, queueSize);
94  pnh.param("decimation", decimation_, decimation_);
95  ROS_ASSERT(decimation_ >= 1);
96  NODELET_INFO("Rate=%f Hz", rate_);
97  NODELET_INFO("Decimation=%d", decimation_);
98  NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
99 
100  if(approxSync)
101  {
103  approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
104  }
105  else
106  {
108  exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
109  }
110 
111  imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
112  imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
113  cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
114  cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
115 
116  imageLeftPub_ = left_it.advertise(left_nh.resolveName("image")+"_throttle", 1);
117  imageRightPub_ = right_it.advertise(right_nh.resolveName("image")+"_throttle", 1);
118  infoLeftPub_ = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info")+"_throttle", 1);
119  infoRightPub_ = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info")+"_throttle", 1);
120  };
121 
122  void callback(const sensor_msgs::ImageConstPtr& imageLeft,
123  const sensor_msgs::ImageConstPtr& imageRight,
124  const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
125  const sensor_msgs::CameraInfoConstPtr& camInfoRight)
126  {
127  if (rate_ > 0.0)
128  {
129  NODELET_DEBUG("update set to %f", rate_);
130  if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
131  {
132  NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
133  return;
134  }
135  }
136  else
137  NODELET_DEBUG("rate unset continuing");
138 
139  last_update_ = ros::Time::now();
140 
141  double leftStamp = imageLeft->header.stamp.toSec();
142  double rightStamp = imageRight->header.stamp.toSec();
143  double leftInfoStamp = camInfoLeft->header.stamp.toSec();
144  double rightInfoStamp = camInfoRight->header.stamp.toSec();
145 
147  {
148  if(decimation_ > 1)
149  {
150  sensor_msgs::CameraInfo info = *camInfoLeft;
151  info.height /= decimation_;
152  info.width /= decimation_;
153  info.roi.height /= decimation_;
154  info.roi.width /= decimation_;
155  info.K[2]/=float(decimation_); // cx
156  info.K[5]/=float(decimation_); // cy
157  info.K[0]/=float(decimation_); // fx
158  info.K[4]/=float(decimation_); // fy
159  info.P[2]/=float(decimation_); // cx
160  info.P[6]/=float(decimation_); // cy
161  info.P[0]/=float(decimation_); // fx
162  info.P[5]/=float(decimation_); // fy
163  info.P[3]/=float(decimation_); // Tx
164  infoLeftPub_.publish(info);
165  }
166  else
167  {
168  infoLeftPub_.publish(camInfoLeft);
169  }
170  }
172  {
173  if(decimation_ > 1)
174  {
175  sensor_msgs::CameraInfo info = *camInfoRight;
176  info.height /= decimation_;
177  info.width /= decimation_;
178  info.roi.height /= decimation_;
179  info.roi.width /= decimation_;
180  info.K[2]/=float(decimation_); // cx
181  info.K[5]/=float(decimation_); // cy
182  info.K[0]/=float(decimation_); // fx
183  info.K[4]/=float(decimation_); // fy
184  info.P[2]/=float(decimation_); // cx
185  info.P[6]/=float(decimation_); // cy
186  info.P[0]/=float(decimation_); // fx
187  info.P[5]/=float(decimation_); // fy
188  info.P[3]/=float(decimation_); // Tx
189  infoRightPub_.publish(info);
190  }
191  else
192  {
193  infoRightPub_.publish(camInfoRight);
194  }
195  }
196 
197 
199  {
200  if(decimation_ > 1)
201  {
202  cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
203  cv_bridge::CvImage out;
204  out.header = imagePtr->header;
205  out.encoding = imagePtr->encoding;
206  out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
208  }
209  else
210  {
211  imageLeftPub_.publish(imageLeft);
212  }
213  }
215  {
216  if(decimation_ > 1)
217  {
218  cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
219  cv_bridge::CvImage out;
220  out.header = imagePtr->header;
221  out.encoding = imagePtr->encoding;
222  out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
224  }
225  else
226  {
227  imageRightPub_.publish(imageRight);
228  }
229  }
230 
231  if( leftStamp != imageLeft->header.stamp.toSec() ||
232  rightStamp != imageRight->header.stamp.toSec())
233  {
234  NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
235  "sure the node publishing the topics doesn't override the same data after publishing them. A "
236  "solution is to use this node within another nodelet manager. Stamps: "
237  "left%f->%f right=%f->%f",
238  leftStamp, imageLeft->header.stamp.toSec(),
239  rightStamp, imageRight->header.stamp.toSec());
240  }
241  }
242 
247 
252 
257 
259 
260 };
261 
262 
264 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
image_transport::Publisher imageLeftPub_
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoRight_
image_transport::SubscriberFilter imageRight_
std::string resolveName(const std::string &name, bool remap=true) const
std::string encoding
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo > MyExactSyncPolicy
image_transport::SubscriberFilter imageLeft_
image_transport::Publisher imageRightPub_
uint32_t getNumSubscribers() const
void callback(const sensor_msgs::ImageConstPtr &imageLeft, const sensor_msgs::ImageConstPtr &imageRight, const sensor_msgs::CameraInfoConstPtr &camInfoLeft, const sensor_msgs::CameraInfoConstPtr &camInfoRight)
ros::NodeHandle & getPrivateNodeHandle() const
void publish(const sensor_msgs::Image &message) const
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
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle & getNodeHandle() const
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
#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)
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo > MyApproxSyncPolicy
static Time now()
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
message_filters::Synchronizer< MyApproxSyncPolicy > * approxSync_
#define ROS_ASSERT(cond)
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoLeft_
#define NODELET_DEBUG(...)
message_filters::Synchronizer< MyExactSyncPolicy > * exactSync_
sensor_msgs::ImagePtr toImageMsg() const
std_msgs::Header header


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