nodelet_throttle.h
Go to the documentation of this file.
1 #ifndef _NODELET_TOPIC_TOOLS_NODELET_THROTTLE_H_
2 #define _NODELET_TOPIC_TOOLS_NODELET_THROTTLE_H_
3 
4 /*
5  * Copyright (c) 2011, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above copyright
14  * notice, this list of conditions and the following disclaimer in the
15  * documentation and/or other materials provided with the distribution.
16  * * Neither the name of the Willow Garage, Inc. nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #include <ros/ros.h>
35 #include <nodelet/nodelet.h>
36 #include <dynamic_reconfigure/server.h>
37 #include <nodelet_topic_tools/NodeletThrottleConfig.h>
38 
39 #include <boost/thread/locks.hpp>
40 #include <boost/thread/mutex.hpp>
41 
42 namespace nodelet_topic_tools
43 {
44 
45 template<typename M>
47 {
48 public:
49  //Constructor
51  {
52  };
53 
55  {
56  delete srv_;
57  }
58 
59 private:
62  boost::mutex connect_mutex_;
63  dynamic_reconfigure::Server<nodelet_topic_tools::NodeletThrottleConfig>* srv_;
64 
65  virtual void onInit()
66  {
67  nh_ = getNodeHandle();
68  ros::NodeHandle& private_nh = getPrivateNodeHandle();
69 
70  srv_ = new dynamic_reconfigure::Server<nodelet_topic_tools::NodeletThrottleConfig>(private_nh);
71  dynamic_reconfigure::Server<nodelet_topic_tools::NodeletThrottleConfig>::CallbackType f = boost::bind(&NodeletThrottle::reconfigure, this, _1, _2);
72  srv_->setCallback(f);
73 
74  // Lazy subscription to topic
75  ros::AdvertiseOptions publisher_ao = ros::AdvertiseOptions::create<M>(
76  "topic_out", 10,
77  boost::bind( &NodeletThrottle::connectCB, this),
79 
80  // Need to create the publisher with connection mutex
81  // connectCB can be called before the publisher is created in nodelet
82  // which means no topics will connect
83  boost::lock_guard<boost::mutex> lock(connect_mutex_);
84  pub_ = nh_.advertise(publisher_ao);
85  };
86 
88  {
89  if (max_update_rate_ > 0.0)
90  {
91  NODELET_DEBUG("update set to %f", max_update_rate_);
92  if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
93  {
94  NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
95  return;
96  }
97  }
98 
99  last_update_ = ros::Time::now();
100  pub_.publish(cloud);
101  }
102 
103  void reconfigure(nodelet_topic_tools::NodeletThrottleConfig &config, uint32_t level)
104  {
105  max_update_rate_ = config.update_rate;
106  }
107 
108  void connectCB() {
109  boost::lock_guard<boost::mutex> lock(connect_mutex_);
110  if (pub_.getNumSubscribers() > 0) {
111  NODELET_DEBUG("Connecting to topic");
112  sub_ = nh_.subscribe<M>("topic_in", 10, &NodeletThrottle::callback, this);
113  }
114  }
115 
116  void disconnectCB() {
117  boost::lock_guard<boost::mutex> lock(connect_mutex_);
118  if (pub_.getNumSubscribers() == 0) {
119  NODELET_DEBUG("Unsubscribing from topic.");
120  sub_.shutdown();
121  }
122  }
123 
127 };
128 
129 } // namespace
130 
131 #endif // guard
dynamic_reconfigure::Server< nodelet_topic_tools::NodeletThrottleConfig > * srv_
f
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::NodeHandle & getNodeHandle() const
ros::NodeHandle & getPrivateNodeHandle() const
void publish(const boost::shared_ptr< M > &message) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
CallbackQueueInterface * getCallbackQueue() const
static Time now()
void callback(const boost::shared_ptr< const M > &cloud)
uint32_t getNumSubscribers() const
void reconfigure(nodelet_topic_tools::NodeletThrottleConfig &config, uint32_t level)
boost::shared_ptr< void > VoidPtr
#define NODELET_DEBUG(...)


nodelet_topic_tools
Author(s): Radu Bogdan Rusu, Tully Foote
autogenerated on Mon Feb 28 2022 22:57:14