data_throttle.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2014, 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 #include <message_filters/sync_policies/exact_time.h>
00036 
00037 #include <image_transport/image_transport.h>
00038 #include <image_transport/subscriber_filter.h>
00039 
00040 #include <sensor_msgs/CameraInfo.h>
00041 
00042 #include <cv_bridge/cv_bridge.h>
00043 
00044 #include <rtabmap/core/util3d.h>
00045 
00046 namespace rtabmap_ros
00047 {
00048 
00049 class DataThrottleNodelet : public nodelet::Nodelet
00050 {
00051 public:
00052         //Constructor
00053         DataThrottleNodelet():
00054                 rate_(0),
00055                 approxSync_(0),
00056                 exactSync_(0),
00057                 decimation_(1)
00058         {
00059         }
00060 
00061         virtual ~DataThrottleNodelet()
00062         {
00063                 if(approxSync_)
00064                 {
00065                         delete approxSync_;
00066                 }
00067                 if(exactSync_)
00068                 {
00069                         delete exactSync_;
00070                 }
00071         }
00072 
00073 private:
00074         ros::Time last_update_;
00075         double rate_;
00076         virtual void onInit()
00077         {
00078                 ros::NodeHandle& nh = getNodeHandle();
00079                 ros::NodeHandle& private_nh = getPrivateNodeHandle();
00080 
00081                 ros::NodeHandle rgb_nh(nh, "rgb");
00082                 ros::NodeHandle depth_nh(nh, "depth");
00083                 ros::NodeHandle rgb_pnh(private_nh, "rgb");
00084                 ros::NodeHandle depth_pnh(private_nh, "depth");
00085                 image_transport::ImageTransport rgb_it(rgb_nh);
00086                 image_transport::ImageTransport depth_it(depth_nh);
00087                 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00088                 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00089 
00090                 int queueSize = 10;
00091                 bool approxSync = true;
00092                 if(private_nh.getParam("max_rate", rate_))
00093                 {
00094                         ROS_WARN("\"max_rate\" is now known as \"rate\".");
00095                 }
00096                 private_nh.param("rate", rate_, rate_);
00097                 private_nh.param("queue_size", queueSize, queueSize);
00098                 private_nh.param("approx_sync", approxSync, approxSync);
00099                 private_nh.param("decimation", decimation_, decimation_);
00100                 ROS_ASSERT(decimation_ >= 1);
00101                 ROS_INFO("Rate=%f Hz", rate_);
00102                 ROS_INFO("Decimation=%d", decimation_);
00103                 ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
00104 
00105                 if(approxSync)
00106                 {
00107                         approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
00108                         approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
00109                 }
00110                 else
00111                 {
00112                         exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
00113                         exactSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
00114                 }
00115 
00116                 image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
00117                 image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
00118                 info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
00119 
00120                 imagePub_ = rgb_it.advertise("image_out", 1);
00121                 imageDepthPub_ = depth_it.advertise("image_out", 1);
00122                 infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 1);
00123         };
00124 
00125         void callback(const sensor_msgs::ImageConstPtr& image,
00126                         const sensor_msgs::ImageConstPtr& imageDepth,
00127                         const sensor_msgs::CameraInfoConstPtr& camInfo)
00128         {
00129                 if (rate_ > 0.0)
00130                 {
00131                         NODELET_DEBUG("update set to %f", rate_);
00132                         if ( last_update_ + ros::Duration(1.0/rate_) > ros::Time::now())
00133                         {
00134                                 NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
00135                                 return;
00136                         }
00137                 }
00138                 else
00139                         NODELET_DEBUG("rate unset continuing");
00140 
00141                 last_update_ = ros::Time::now();
00142 
00143                 if(imagePub_.getNumSubscribers())
00144                 {
00145                         if(decimation_ > 1)
00146                         {
00147                                 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
00148                                 cv_bridge::CvImage out;
00149                                 out.header = imagePtr->header;
00150                                 out.encoding = imagePtr->encoding;
00151                                 out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
00152                                 imagePub_.publish(out.toImageMsg());
00153                         }
00154                         else
00155                         {
00156                                 imagePub_.publish(image);
00157                         }
00158                 }
00159                 if(imageDepthPub_.getNumSubscribers())
00160                 {
00161                         if(decimation_ > 1)
00162                         {
00163                                 cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
00164                                 cv_bridge::CvImage out;
00165                                 out.header = imagePtr->header;
00166                                 out.encoding = imagePtr->encoding;
00167                                 out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
00168                                 imageDepthPub_.publish(out.toImageMsg());
00169                         }
00170                         else
00171                         {
00172                                 imageDepthPub_.publish(imageDepth);
00173                         }
00174                 }
00175                 if(infoPub_.getNumSubscribers())
00176                 {
00177                         if(decimation_ > 1)
00178                         {
00179                                 sensor_msgs::CameraInfo info = *camInfo;
00180                                 info.height /= decimation_;
00181                                 info.width /= decimation_;
00182                                 info.roi.height /= decimation_;
00183                                 info.roi.width /= decimation_;
00184                                 info.K[2]/=float(decimation_); // cx
00185                                 info.K[5]/=float(decimation_); // cy
00186                                 info.K[0]/=float(decimation_); // fx
00187                                 info.K[4]/=float(decimation_); // fy
00188                                 info.P[2]/=float(decimation_); // cx
00189                                 info.P[6]/=float(decimation_); // cy
00190                                 info.P[0]/=float(decimation_); // fx
00191                                 info.P[5]/=float(decimation_); // fy
00192                                 infoPub_.publish(info);
00193                         }
00194                         else
00195                         {
00196                                 infoPub_.publish(camInfo);
00197                         }
00198                 }
00199         }
00200 
00201         image_transport::Publisher imagePub_;
00202         image_transport::Publisher imageDepthPub_;
00203         ros::Publisher infoPub_;
00204 
00205         image_transport::SubscriberFilter image_sub_;
00206         image_transport::SubscriberFilter image_depth_sub_;
00207         message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00208 
00209         typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00210         message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00211         typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00212         message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00213 
00214         int decimation_;
00215 
00216 };
00217 
00218 
00219 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DataThrottleNodelet, nodelet::Nodelet);
00220 }


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Thu Aug 27 2015 15:00:24