nodelet_throttle.h
Go to the documentation of this file.
00001 #ifndef _NODELET_TOPIC_TOOLS_NODELET_THROTTLE_H_
00002 #define _NODELET_TOPIC_TOOLS_NODELET_THROTTLE_H_
00003 
00004 /*
00005  * Copyright (c) 2011, Willow Garage, Inc.
00006  * All rights reserved.
00007  *
00008  * Redistribution and use in source and binary forms, with or without
00009  * modification, are permitted provided that the following conditions are met:
00010  *
00011  *     * Redistributions of source code must retain the above copyright
00012  *       notice, this list of conditions and the following disclaimer.
00013  *     * Redistributions in binary form must reproduce the above copyright
00014  *       notice, this list of conditions and the following disclaimer in the
00015  *       documentation and/or other materials provided with the distribution.
00016  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00017  *       contributors may be used to endorse or promote products derived from
00018  *       this software without specific prior written permission.
00019  *
00020  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00021  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00022  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00023  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00024  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00025  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00026  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00027  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00028  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00029  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00030  * POSSIBILITY OF SUCH DAMAGE.
00031  */
00032 
00033 #include <ros/ros.h>
00034 #include <pluginlib/class_list_macros.h>
00035 #include <nodelet/nodelet.h>
00036 #include <dynamic_reconfigure/server.h>
00037 #include <nodelet_topic_tools/NodeletThrottleConfig.h>
00038 
00039 namespace nodelet_topic_tools
00040 {
00041 
00042 template<typename M>
00043 class NodeletThrottle : public nodelet::Nodelet
00044 {
00045 public:
00046   //Constructor
00047   NodeletThrottle(): max_update_rate_(0)
00048   {
00049   };
00050 
00051   ~NodeletThrottle()
00052   {
00053       delete srv_;
00054   }
00055 
00056 private:
00057   ros::Time last_update_;
00058   double max_update_rate_;
00059   boost::mutex connect_mutex_;
00060   dynamic_reconfigure::Server<nodelet_topic_tools::NodeletThrottleConfig>* srv_;
00061 
00062   virtual void onInit()
00063   {
00064     nh_ = getNodeHandle();
00065     ros::NodeHandle& private_nh = getPrivateNodeHandle();
00066 
00067     srv_ = new dynamic_reconfigure::Server<nodelet_topic_tools::NodeletThrottleConfig>(private_nh);
00068     dynamic_reconfigure::Server<nodelet_topic_tools::NodeletThrottleConfig>::CallbackType f = boost::bind(&NodeletThrottle::reconfigure, this, _1, _2);
00069     srv_->setCallback(f);
00070 
00071     // Lazy subscription to topic
00072     ros::AdvertiseOptions publisher_ao = ros::AdvertiseOptions::create<M>(
00073       "topic_out", 10,
00074       boost::bind( &NodeletThrottle::connectCB, this),
00075       boost::bind( &NodeletThrottle::disconnectCB, this), ros::VoidPtr(), nh_.getCallbackQueue());
00076 
00077     // Need to create the publisher with connection mutex
00078     // connectCB can be called before the publisher is created in nodelet
00079     // which means no topics will connect
00080     boost::lock_guard<boost::mutex> lock(connect_mutex_);
00081     pub_ = nh_.advertise(publisher_ao);
00082   };
00083 
00084   void callback(const boost::shared_ptr<const M>& cloud)
00085   {
00086     if (max_update_rate_ > 0.0)
00087     {
00088       NODELET_DEBUG("update set to %f", max_update_rate_);
00089       if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
00090       {
00091         NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
00092         return;
00093       }
00094     }
00095 
00096     last_update_ = ros::Time::now();
00097     pub_.publish(cloud);
00098   }
00099 
00100   void reconfigure(nodelet_topic_tools::NodeletThrottleConfig &config, uint32_t level)
00101   {
00102       max_update_rate_ = config.update_rate;
00103   }
00104 
00105   void connectCB() {
00106       boost::lock_guard<boost::mutex> lock(connect_mutex_);
00107       if (pub_.getNumSubscribers() > 0) {
00108           NODELET_DEBUG("Connecting to topic");
00109           sub_ = nh_.subscribe<M>("topic_in", 10, &NodeletThrottle::callback, this);
00110       }
00111   }
00112 
00113   void disconnectCB() {
00114       boost::lock_guard<boost::mutex> lock(connect_mutex_);
00115       if (pub_.getNumSubscribers() == 0) {
00116           NODELET_DEBUG("Unsubscribing from topic.");
00117           sub_.shutdown();
00118       }
00119   }
00120 
00121   ros::NodeHandle nh_;
00122   ros::Publisher  pub_;
00123   ros::Subscriber sub_;
00124 };
00125 
00126 } // namespace
00127 
00128 #endif // guard


nodelet_topic_tools
Author(s): Radu Bogdan Rusu, Tully Foote
autogenerated on Sat Dec 28 2013 17:14:45