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
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
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
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
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
00078
00079
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 }
00127
00128 #endif // guard