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 #include <boost/thread/locks.hpp>
00040 #include <boost/thread/mutex.hpp>
00041 
00042 namespace nodelet_topic_tools
00043 {
00044 
00045 template<typename M>
00046 class NodeletThrottle : public nodelet::Nodelet
00047 {
00048 public:
00049   //Constructor
00050   NodeletThrottle(): max_update_rate_(0)
00051   {
00052   };
00053 
00054   ~NodeletThrottle()
00055   {
00056       delete srv_;
00057   }
00058 
00059 private:
00060   ros::Time last_update_;
00061   double max_update_rate_;
00062   boost::mutex connect_mutex_;
00063   dynamic_reconfigure::Server<nodelet_topic_tools::NodeletThrottleConfig>* srv_;
00064 
00065   virtual void onInit()
00066   {
00067     nh_ = getNodeHandle();
00068     ros::NodeHandle& private_nh = getPrivateNodeHandle();
00069 
00070     srv_ = new dynamic_reconfigure::Server<nodelet_topic_tools::NodeletThrottleConfig>(private_nh);
00071     dynamic_reconfigure::Server<nodelet_topic_tools::NodeletThrottleConfig>::CallbackType f = boost::bind(&NodeletThrottle::reconfigure, this, _1, _2);
00072     srv_->setCallback(f);
00073 
00074     // Lazy subscription to topic
00075     ros::AdvertiseOptions publisher_ao = ros::AdvertiseOptions::create<M>(
00076       "topic_out", 10,
00077       boost::bind( &NodeletThrottle::connectCB, this),
00078       boost::bind( &NodeletThrottle::disconnectCB, this), ros::VoidPtr(), nh_.getCallbackQueue());
00079 
00080     // Need to create the publisher with connection mutex
00081     // connectCB can be called before the publisher is created in nodelet
00082     // which means no topics will connect
00083     boost::lock_guard<boost::mutex> lock(connect_mutex_);
00084     pub_ = nh_.advertise(publisher_ao);
00085   };
00086 
00087   void callback(const boost::shared_ptr<const M>& cloud)
00088   {
00089     if (max_update_rate_ > 0.0)
00090     {
00091       NODELET_DEBUG("update set to %f", max_update_rate_);
00092       if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
00093       {
00094         NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
00095         return;
00096       }
00097     }
00098 
00099     last_update_ = ros::Time::now();
00100     pub_.publish(cloud);
00101   }
00102 
00103   void reconfigure(nodelet_topic_tools::NodeletThrottleConfig &config, uint32_t level)
00104   {
00105       max_update_rate_ = config.update_rate;
00106   }
00107 
00108   void connectCB() {
00109       boost::lock_guard<boost::mutex> lock(connect_mutex_);
00110       if (pub_.getNumSubscribers() > 0) {
00111           NODELET_DEBUG("Connecting to topic");
00112           sub_ = nh_.subscribe<M>("topic_in", 10, &NodeletThrottle::callback, this);
00113       }
00114   }
00115 
00116   void disconnectCB() {
00117       boost::lock_guard<boost::mutex> lock(connect_mutex_);
00118       if (pub_.getNumSubscribers() == 0) {
00119           NODELET_DEBUG("Unsubscribing from topic.");
00120           sub_.shutdown();
00121       }
00122   }
00123 
00124   ros::NodeHandle nh_;
00125   ros::Publisher  pub_;
00126   ros::Subscriber sub_;
00127 };
00128 
00129 } // namespace
00130 
00131 #endif // guard


nodelet_topic_tools
Author(s): Radu Bogdan Rusu, Tully Foote
autogenerated on Wed Aug 26 2015 14:56:48