stereo_throttle.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "ros/ros.h"
00029 #include "pluginlib/class_list_macros.h"
00030 #include "nodelet/nodelet.h"
00031 
00032 #include <message_filters/subscriber.h>
00033 #include <message_filters/time_synchronizer.h>
00034 #include <message_filters/sync_policies/approximate_time.h>
00035 
00036 #include <image_transport/image_transport.h>
00037 #include <image_transport/subscriber_filter.h>
00038 
00039 #include <sensor_msgs/CameraInfo.h>
00040 
00041 #include <cv_bridge/cv_bridge.h>
00042 
00043 #include <rtabmap/core/util2d.h>
00044 
00045 namespace rtabmap_ros
00046 {
00047 
00048 class StereoThrottleNodelet : public nodelet::Nodelet
00049 {
00050 public:
00051         //Constructor
00052         StereoThrottleNodelet():
00053                 rate_(0),
00054                 approxSync_(0),
00055                 exactSync_(0),
00056                 decimation_(1)
00057         {
00058         }
00059 
00060         virtual ~StereoThrottleNodelet()
00061         {
00062                 if(approxSync_)
00063                 {
00064                         delete approxSync_;
00065                 }
00066                 if(exactSync_)
00067                 {
00068                         delete exactSync_;
00069                 }
00070         }
00071 
00072 private:
00073         ros::Time last_update_;
00074         double rate_;
00075         virtual void onInit()
00076         {
00077                 ros::NodeHandle& nh = getNodeHandle();
00078                 ros::NodeHandle& pnh = getPrivateNodeHandle();
00079 
00080                 ros::NodeHandle left_nh(nh, "left");
00081                 ros::NodeHandle right_nh(nh, "right");
00082                 ros::NodeHandle left_pnh(pnh, "left");
00083                 ros::NodeHandle right_pnh(pnh, "right");
00084                 image_transport::ImageTransport left_it(left_nh);
00085                 image_transport::ImageTransport right_it(right_nh);
00086                 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
00087                 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
00088 
00089                 int queueSize = 5;
00090                 bool approxSync = false;
00091                 pnh.param("approx_sync", approxSync, approxSync);
00092                 pnh.param("rate", rate_, rate_);
00093                 pnh.param("queue_size", queueSize, queueSize);
00094                 pnh.param("decimation", decimation_, decimation_);
00095                 ROS_ASSERT(decimation_ >= 1);
00096                 NODELET_INFO("Rate=%f Hz", rate_);
00097                 NODELET_INFO("Decimation=%d", decimation_);
00098                 NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
00099 
00100                 if(approxSync)
00101                 {
00102                         approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00103                         approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
00104                 }
00105                 else
00106                 {
00107                         exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
00108                         exactSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
00109                 }
00110 
00111                 imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
00112                 imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
00113                 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
00114                 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
00115 
00116                 imageLeftPub_ = left_it.advertise(left_nh.resolveName("image")+"_throttle", 1);
00117                 imageRightPub_ = right_it.advertise(right_nh.resolveName("image")+"_throttle", 1);
00118                 infoLeftPub_ = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info")+"_throttle", 1);
00119                 infoRightPub_ = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info")+"_throttle", 1);
00120         };
00121 
00122         void callback(const sensor_msgs::ImageConstPtr& imageLeft,
00123                         const sensor_msgs::ImageConstPtr& imageRight,
00124                         const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
00125                         const sensor_msgs::CameraInfoConstPtr& camInfoRight)
00126         {
00127                 if (rate_ > 0.0)
00128                 {
00129                         NODELET_DEBUG("update set to %f", rate_);
00130                         if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
00131                         {
00132                                 NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
00133                                 return;
00134                         }
00135                 }
00136                 else
00137                         NODELET_DEBUG("rate unset continuing");
00138 
00139                 last_update_ = ros::Time::now();
00140 
00141                 if(imageLeftPub_.getNumSubscribers())
00142                 {
00143                         if(decimation_ > 1)
00144                         {
00145                                 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
00146                                 cv_bridge::CvImage out;
00147                                 out.header = imagePtr->header;
00148                                 out.encoding = imagePtr->encoding;
00149                                 out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
00150                                 imageLeftPub_.publish(out.toImageMsg());
00151                         }
00152                         else
00153                         {
00154                                 imageLeftPub_.publish(imageLeft);
00155                         }
00156                 }
00157                 if(imageRightPub_.getNumSubscribers())
00158                 {
00159                         if(decimation_ > 1)
00160                         {
00161                                 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
00162                                 cv_bridge::CvImage out;
00163                                 out.header = imagePtr->header;
00164                                 out.encoding = imagePtr->encoding;
00165                                 out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
00166                                 imageRightPub_.publish(out.toImageMsg());
00167                         }
00168                         else
00169                         {
00170                                 imageRightPub_.publish(imageRight);
00171                         }
00172                 }
00173                 if(infoLeftPub_.getNumSubscribers())
00174                 {
00175                         if(decimation_ > 1)
00176                         {
00177                                 sensor_msgs::CameraInfo info = *camInfoLeft;
00178                                 info.height /= decimation_;
00179                                 info.width /= decimation_;
00180                                 info.roi.height /= decimation_;
00181                                 info.roi.width /= decimation_;
00182                                 info.K[2]/=float(decimation_); // cx
00183                                 info.K[5]/=float(decimation_); // cy
00184                                 info.K[0]/=float(decimation_); // fx
00185                                 info.K[4]/=float(decimation_); // fy
00186                                 info.P[2]/=float(decimation_); // cx
00187                                 info.P[6]/=float(decimation_); // cy
00188                                 info.P[0]/=float(decimation_); // fx
00189                                 info.P[5]/=float(decimation_); // fy
00190                                 info.P[3]/=float(decimation_); // Tx
00191                                 infoLeftPub_.publish(info);
00192                         }
00193                         else
00194                         {
00195                                 infoLeftPub_.publish(camInfoLeft);
00196                         }
00197                 }
00198                 if(infoRightPub_.getNumSubscribers())
00199                 {
00200                         if(decimation_ > 1)
00201                         {
00202                                 sensor_msgs::CameraInfo info = *camInfoRight;
00203                                 info.height /= decimation_;
00204                                 info.width /= decimation_;
00205                                 info.roi.height /= decimation_;
00206                                 info.roi.width /= decimation_;
00207                                 info.K[2]/=float(decimation_); // cx
00208                                 info.K[5]/=float(decimation_); // cy
00209                                 info.K[0]/=float(decimation_); // fx
00210                                 info.K[4]/=float(decimation_); // fy
00211                                 info.P[2]/=float(decimation_); // cx
00212                                 info.P[6]/=float(decimation_); // cy
00213                                 info.P[0]/=float(decimation_); // fx
00214                                 info.P[5]/=float(decimation_); // fy
00215                                 info.P[3]/=float(decimation_); // Tx
00216                                 infoRightPub_.publish(info);
00217                         }
00218                         else
00219                         {
00220                                 infoRightPub_.publish(camInfoRight);
00221                         }
00222                 }
00223         }
00224 
00225         image_transport::Publisher imageLeftPub_;
00226         image_transport::Publisher imageRightPub_;
00227         ros::Publisher infoLeftPub_;
00228         ros::Publisher infoRightPub_;
00229 
00230         image_transport::SubscriberFilter imageLeft_;
00231         image_transport::SubscriberFilter imageRight_;
00232         message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
00233         message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
00234 
00235         typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00236         message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00237         typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00238         message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00239 
00240         int decimation_;
00241 
00242 };
00243 
00244 
00245 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoThrottleNodelet, nodelet::Nodelet);
00246 }


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Sun Jul 24 2016 03:49:08