stealth_relay_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: C++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2017, JSK Lab
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
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 /*
36  * stealth_relay_nodelet.cpp
37  * Author: Furushchev <furushchev@jsk.imi.i.u-tokyo.ac.jp>
38  */
39 
41 
42 
43 namespace jsk_topic_tools
44 {
46  {
47  bool use_multithread;
48  ros::param::param<bool>("~use_multithread_callback", use_multithread, true);
49  if (use_multithread) {
50  NODELET_DEBUG("use multithread callback");
51  nh_.reset (new ros::NodeHandle (getMTNodeHandle ()));
53  } else {
54  NODELET_DEBUG("use singlethread callback");
55  nh_.reset (new ros::NodeHandle (getNodeHandle ()));
56  pnh_.reset (new ros::NodeHandle (getPrivateNodeHandle ()));
57  }
58 
59  subscribed_ = false;
60  advertised_ = false;
61 
62 #ifdef topic_tools_relay_stealth_EXISTS
63  NODELET_WARN("This nodelet is deprecated. Use `topic_tools/Relay` with `stealth_mode`");
64 #endif
65 
66  poll_timer_ = pnh_->createTimer(ros::Duration(1.0),
68  /* oneshot= */false, /* autostart= */ false);
69 
70  srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
71  dynamic_reconfigure::Server<Config>::CallbackType f =
72  boost::bind(&StealthRelay::configCallback, this, _1, _2);
73  srv_->setCallback(f);
74 
75  /* To advertise output topic as the same type of input topic,
76  first this node subscribes input topic until one message is received. */
77  subscribe();
78  }
79 
81  {
82  NODELET_DEBUG("subscribe");
84  ("input", queue_size_, &StealthRelay::inputCallback, this);
85  subscribed_ = true;
86  }
87 
89  {
90  NODELET_DEBUG("unsubscribe");
91  sub_.shutdown();
92  subscribed_ = false;
93  }
94 
96  {
97  return subscribed_;
98  }
99 
100  void StealthRelay::configCallback(Config& config, uint32_t level)
101  {
102  boost::mutex::scoped_lock lock(mutex_);
103  NODELET_DEBUG("configCallback");
104  bool need_resubscribe = (config.queue_size != queue_size_);
105  queue_size_ = config.queue_size;
106 
107  if (config.monitor_topic.empty()) {
108  config.monitor_topic = pnh_->resolveName("input");
109  }
110  monitor_topic_ = config.monitor_topic;
111 
112  if (monitor_rate_ != config.monitor_rate) {
113  monitor_rate_ = config.monitor_rate;
115  }
116 
117  if (enable_monitor_ != config.enable_monitor) {
118  enable_monitor_ = config.enable_monitor;
119  if (enable_monitor_) {
120  poll_timer_.start();
121  } else {
122  poll_timer_.stop();
123  subscribe();
124  }
125  }
126 
127  if (isSubscribed() && need_resubscribe) {
128  unsubscribe();
129  subscribe();
130  }
131  }
132 
134  {
135  const AnyMsgConstPtr& msg = event.getConstMessage();
136  inputCallback(msg);
137  }
138 
140  {
141  boost::mutex::scoped_lock lock(mutex_);
142  NODELET_DEBUG("inputCallback");
143 
144  if (!advertised_)
145  {
146  pub_ = msg->advertise(*pnh_, "output", 1);
147  advertised_ = true;
149  return;
150  }
151 
152  pub_.publish(msg);
153  }
154 
156  {
157  boost::mutex::scoped_lock lock(mutex_);
158  NODELET_DEBUG("timerCallback");
159 
160  if (pub_.getNumSubscribers() == 0 && isSubscribed())
161  {
162  unsubscribe();
163  return;
164  }
165 
166  int subscribing_num = getNumOtherSubscribers(monitor_topic_);
167  if (subscribed_ && subscribing_num == 0)
168  unsubscribe();
169  else if (!subscribed_ && subscribing_num > 0)
170  subscribe();
171  }
172 
173  int StealthRelay::getNumOtherSubscribers(const std::string& name)
174  {
175  int num = 0;
177  bool ok = ros::master::execute("getSystemState", req, res, data, false);
178 
179  XmlRpc::XmlRpcValue& sub_info = data[1];
180  for (size_t i = 0; i < sub_info.size(); ++i)
181  {
182  std::string topic_name = sub_info[i][0];
183  if (topic_name == name)
184  {
185  XmlRpc::XmlRpcValue& subscribers = sub_info[i][1];
186  int cnt = 0;
187  for (size_t j = 0; j < subscribers.size(); ++j)
188  {
189  std::string subscriber = subscribers[j];
190  if (subscriber != ros::this_node::getName()) ++cnt;
191  }
192  num = cnt;
193  break;
194  }
195  }
196  return num;
197  }
198 }
199 
msg
num
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
f
boost::shared_ptr< ros::NodeHandle > nh_
Definition: stealth_relay.h:70
int size() const
void stop()
void setPeriod(const Duration &period, bool reset=true)
PLUGINLIB_EXPORT_CLASS(jsk_topic_tools::Snapshot, nodelet::Nodelet)
void start()
virtual void timerCallback(const ros::TimerEvent &event)
ros::NodeHandle & getMTNodeHandle() const
ROSCPP_DECL const std::string & getName()
data
ros::NodeHandle & getPrivateNodeHandle() const
StealthRelayConfig Config
Definition: stealth_relay.h:57
ros::NodeHandle & getMTPrivateNodeHandle() const
virtual void configCallback(Config &config, uint32_t level)
jsk_topic_tools::StealthRelay StealthRelay
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
virtual int getNumOtherSubscribers(const std::string &name)
ros::NodeHandle & getNodeHandle() const
uint32_t getNumSubscribers() const
boost::shared_ptr< ros::NodeHandle > pnh_
Definition: stealth_relay.h:70
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
Definition: stealth_relay.h:71
virtual void inputCallback(const ros::MessageEvent< topic_tools::ShapeShifter > &event)
#define NODELET_DEBUG(...)


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