passthrough_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, JSK Lab
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * 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
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/o2r other materials provided
16  * with the distribution.
17  * * Neither the name of the JSK Lab nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
36 
37 namespace jsk_topic_tools
38 {
54  {
55  advertised_ = false;
56  publish_requested_ = false;
58  subscribing_ = true;
59  pnh_.param("default_duration", default_duration_, 10.0);
61  "input", 1,
64  "request_duration", &Passthrough::requestDurationCallback, this);
66  "request", &Passthrough::requestCallback, this);
68  "stop", &Passthrough::stopCallback, this);
69  }
70 
72  std_srvs::Empty::Request &req,
73  std_srvs::Empty::Response &res)
74  {
75  boost::mutex::scoped_lock lock(mutex_);
76  // force to stop publishing
77  if (!publish_requested_) {
78  NODELET_DEBUG("already stoppped");
79  }
80  publish_requested_ = false;
81  return true;
82  }
83 
85  const ros::Duration& duration)
86  {
87  boost::mutex::scoped_lock lock(mutex_);
88  // special case of ros::Duration(0), it means eternal
89  if (duration == ros::Duration(0)) {
90  end_time_ = ros::Time(0);
91  publish_requested_ = true;
92  }
93  else {
94  ros::Time now = ros::Time::now();
95  if (publish_requested_) {
96  // check need to update end_time or not
97  if (end_time_ < now + duration) {
98  end_time_ = now + duration;
99  }
100  }
101  else {
102  publish_requested_ = true;
103  end_time_ = now + duration;
104  }
105  }
106  if (!subscribing_) {
107  NODELET_DEBUG("suscribe");
109  "input", 1,
111  subscribing_ = true;
112  }
113  }
114 
116  jsk_topic_tools::PassthroughDuration::Request &req,
117  jsk_topic_tools::PassthroughDuration::Response &res)
118  {
119  requestDurationCallbackImpl(req.duration);
120  return true;
121  }
122 
124  std_srvs::Empty::Request &req,
125  std_srvs::Empty::Response &res)
126  {
128  return true;
129  }
130 
133  {
134  boost::mutex::scoped_lock lock(mutex_);
135  if (!advertised_) {
136  // this block is called only once
137  // in order to advertise topic.
138  // we need to subscribe at least one message
139  // in order to detect message definition.
140  pub_ = advertise(msg, "output");
141  advertised_ = true;
142  }
143  if (publish_requested_) {
144  ros::Time now = ros::Time::now();
145  if (end_time_ == ros::Time(0) || // ros::Time(0) means eternal publishing
146  end_time_ > now) {
147  pub_.publish(msg);
148  }
149  // check it goes over end_time_
150  if (end_time_ != ros::Time(0) && end_time_ < now) {
151  publish_requested_ = false;
152  }
153  }
154  if (!publish_requested_) {
155  // Unsubscribe input anyway
156  sub_.shutdown();
157  subscribing_ = false;
158  }
159  }
160 
162  {
163  boost::mutex::scoped_lock lock(mutex_);
164  NODELET_DEBUG("connectCB");
165  if (advertised_) {
166  if (pub_.getNumSubscribers() > 0) {
168  NODELET_DEBUG("suscribe");
170  "input", 1,
172  subscribing_ = true;
173  }
174  }
175  }
176  }
177 
179  {
180  boost::mutex::scoped_lock lock(mutex_);
181  NODELET_DEBUG("disconnectCb");
182  if (advertised_) {
183  if (pub_.getNumSubscribers() == 0) {
184  if (subscribing_) {
185  NODELET_DEBUG("disconnect");
186  sub_.shutdown();
187  subscribing_ = false;
188  }
189  }
190  }
191  }
192 
193 
196  const std::string& topic)
197  {
199  = boost::bind( &Passthrough::connectCb, this);
200  ros::SubscriberStatusCallback disconnect_cb
201  = boost::bind( &Passthrough::disconnectCb, this);
202  ros::AdvertiseOptions opts(topic, 1,
203  msg->getMD5Sum(),
204  msg->getDataType(),
205  msg->getMessageDefinition());
206  opts.latch = true;
207  return pnh_.advertise(opts);
208  }
209 }
210 
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void publish(const boost::shared_ptr< M > &message) const
virtual void requestDurationCallbackImpl(const ros::Duration &duration)
Implementation of requestDurationCallback.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
PLUGINLIB_EXPORT_CLASS(jsk_topic_tools::Snapshot, nodelet::Nodelet)
virtual bool requestDurationCallback(jsk_topic_tools::PassthroughDuration::Request &req, jsk_topic_tools::PassthroughDuration::Response &res)
Callback function for ~request_duration service.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::NodeHandle & getPrivateNodeHandle() const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::ServiceServer request_duration_srv_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
virtual ros::Publisher advertise(boost::shared_ptr< topic_tools::ShapeShifter const > msg, const std::string &topic)
Advertise a topic according to a specific message instance.
virtual bool requestCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Callback function for ~request service.
uint32_t getNumSubscribers() const
virtual void inputCallback(const boost::shared_ptr< topic_tools::ShapeShifter const > &msg)
Callback function for ~input. It advertise ~output topic according to the definition of input topic a...
virtual bool stopCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Callback function of ~stop service. Force to stop publishing messages.
jsk_topic_tools::Passthrough Passthrough
static Time now()
virtual void onInit()
Initialization function.
#define NODELET_DEBUG(...)


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Tue Feb 6 2018 03:45:19