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, boost::placeholders::_1, boost::placeholders::_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_);
93  {
94  NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
95  return;
96  }
97  }
98 
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
nodelet_topic_tools::NodeletThrottle::onInit
virtual void onInit()
Definition: nodelet_throttle.h:65
nodelet_topic_tools::NodeletThrottle::nh_
ros::NodeHandle nh_
Definition: nodelet_throttle.h:124
ros::Publisher
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
boost::shared_ptr< void >
ros.h
nodelet_topic_tools::NodeletThrottle
Definition: nodelet_throttle.h:46
nodelet_topic_tools::NodeletThrottle::sub_
ros::Subscriber sub_
Definition: nodelet_throttle.h:126
nodelet_topic_tools::NodeletThrottle::connect_mutex_
boost::mutex connect_mutex_
Definition: nodelet_throttle.h:62
TimeBase< Time, Duration >::toSec
double toSec() const
ros::Subscriber::shutdown
void shutdown()
nodelet_topic_tools::NodeletThrottle::srv_
dynamic_reconfigure::Server< nodelet_topic_tools::NodeletThrottleConfig > * srv_
Definition: nodelet_throttle.h:63
nodelet_topic_tools::NodeletThrottle::reconfigure
void reconfigure(nodelet_topic_tools::NodeletThrottleConfig &config, uint32_t level)
Definition: nodelet_throttle.h:103
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::AdvertiseOptions
f
f
nodelet_topic_tools::NodeletThrottle::disconnectCB
void disconnectCB()
Definition: nodelet_throttle.h:116
nodelet_topic_tools::NodeletThrottle::max_update_rate_
double max_update_rate_
Definition: nodelet_throttle.h:61
nodelet_topic_tools
Definition: nodelet_lazy.h:47
ros::NodeHandle::getCallbackQueue
CallbackQueueInterface * getCallbackQueue() const
nodelet_topic_tools::NodeletThrottle::NodeletThrottle
NodeletThrottle()
Definition: nodelet_throttle.h:50
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
nodelet_topic_tools::NodeletThrottle::pub_
ros::Publisher pub_
Definition: nodelet_throttle.h:125
nodelet::Nodelet
ros::Time
nodelet.h
nodelet_topic_tools::NodeletThrottle::last_update_
ros::Time last_update_
Definition: nodelet_throttle.h:60
class_list_macros.hpp
nodelet_topic_tools::NodeletThrottle::~NodeletThrottle
~NodeletThrottle()
Definition: nodelet_throttle.h:54
nodelet_topic_tools::NodeletThrottle::connectCB
void connectCB()
Definition: nodelet_throttle.h:108
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
ros::Duration
ros::NodeHandle
ros::Subscriber
nodelet_topic_tools::NodeletThrottle::callback
void callback(const boost::shared_ptr< const M > &cloud)
Definition: nodelet_throttle.h:87
NODELET_DEBUG
#define NODELET_DEBUG(...)
ros::Time::now
static Time now()


nodelet_topic_tools
Author(s): Radu Bogdan Rusu, Tully Foote, Michael Carroll
autogenerated on Fri Jan 12 2024 03:40:41