connection_based_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, 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 
38 
39 namespace jsk_topic_tools
40 {
42  {
44  bool use_multithread;
45  ros::param::param<bool>("~use_multithread_callback", use_multithread, true);
46  if (use_multithread) {
47  NODELET_DEBUG("use multithread callback");
48  nh_.reset (new ros::NodeHandle (getMTNodeHandle ()));
50  } else {
51  NODELET_DEBUG("use singlethread callback");
52  nh_.reset (new ros::NodeHandle (getNodeHandle ()));
53  pnh_.reset (new ros::NodeHandle (getPrivateNodeHandle ()));
54  }
55  pnh_->param("always_subscribe", always_subscribe_, false);
56  pnh_->param("verbose_connection", verbose_connection_, false);
57  if (!verbose_connection_) {
58  nh_->param("verbose_connection", verbose_connection_, false);
59  }
60 
61  // timer to warn when onInitProcess is not called
62  pnh_->param("no_warn_on_init_post_process", on_init_post_process_called_, false);
67  this,
68  /*oneshot=*/true);
69  }
70 
71  // timer to warn when no connection in a few seconds
72  ever_subscribed_ = false;
73  timer_warn_never_subscribed_ = nh_->createWallTimer(
76  this,
77  /*oneshot=*/true);
78  }
79 
81  {
83  if (always_subscribe_) {
84  boost::mutex::scoped_lock lock(connection_mutex_);
85  ever_subscribed_ = true;
86  subscribe();
87  }
88  }
89 
91  {
93  }
94 
95 
97  {
99  NODELET_WARN("[%s] onInitPostProcess is not yet called.", nodelet::Nodelet::getName().c_str());
100  }
101  }
102 
104  {
105  if (!ever_subscribed_) {
106  NODELET_WARN("'%s' subscribes topics only with child subscribers.", nodelet::Nodelet::getName().c_str());
107  }
108  }
109 
111  {
112  if (verbose_connection_) {
113  NODELET_INFO("New connection or disconnection is detected");
114  }
115  if (!always_subscribe_) {
116  boost::mutex::scoped_lock lock(connection_mutex_);
117  for (size_t i = 0; i < publishers_.size(); i++) {
118  ros::Publisher pub = publishers_[i];
119  if (pub.getNumSubscribers() > 0) {
120  if (!ever_subscribed_) {
121  ever_subscribed_ = true;
122  }
124  if (verbose_connection_) {
125  NODELET_INFO("Subscribe input topics");
126  }
127  subscribe();
129  }
130  return;
131  }
132  }
134  if (verbose_connection_) {
135  NODELET_INFO("Unsubscribe input topics");
136  }
137  unsubscribe();
139  }
140  }
141  }
142 
145  {
146  if (verbose_connection_) {
147  NODELET_INFO("New image connection or disconnection is detected");
148  }
149  if (!always_subscribe_) {
150  boost::mutex::scoped_lock lock(connection_mutex_);
151  for (size_t i = 0; i < image_publishers_.size(); i++) {
153  if (pub.getNumSubscribers() > 0) {
154  if (!ever_subscribed_) {
155  ever_subscribed_ = true;
156  }
158  if (verbose_connection_) {
159  NODELET_INFO("Subscribe input topics");
160  }
161  subscribe();
163  }
164  return;
165  }
166  }
168  if (verbose_connection_) {
169  NODELET_INFO("Unsubscribe input topics");
170  }
171  unsubscribe();
173  }
174  }
175  }
176 
179  {
181  }
182 
185  {
187  }
188 
190  {
191  if (verbose_connection_) {
192  NODELET_INFO("New image connection or disconnection is detected");
193  }
194  if (!always_subscribe_) {
195  boost::mutex::scoped_lock lock(connection_mutex_);
196  for (size_t i = 0; i < camera_publishers_.size(); i++) {
198  if (pub.getNumSubscribers() > 0) {
199  if (!ever_subscribed_) {
200  ever_subscribed_ = true;
201  }
203  if (verbose_connection_) {
204  NODELET_INFO("Subscribe input topics");
205  }
206  subscribe();
208  }
209  return;
210  }
211  }
213  if (verbose_connection_) {
214  NODELET_INFO("Unsubscribe input topics");
215  }
216  unsubscribe();
218  }
219  }
220  }
221 }
ConnectionStatus connection_status_
Status of connection.
pub
#define NODELET_WARN(...)
std::vector< image_transport::Publisher > image_publishers_
List of watching image publishers.
uint32_t getNumSubscribers() const
const std::string & getName() const
virtual void cameraInfoConnectionCallback(const ros::SingleSubscriberPublisher &pub)
callback function which is called when new subscriber come for camera info publisher ...
ros::WallTimer timer_warn_never_subscribed_
WallTimer instance for warning about no connection.
ros::NodeHandle & getMTNodeHandle() const
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
virtual void connectionCallback(const ros::SingleSubscriberPublisher &pub)
callback function which is called when new subscriber come
uint32_t getNumSubscribers() const
ros::NodeHandle & getPrivateNodeHandle() const
virtual void onInitPostProcess()
Post processing of initialization of nodelet. You need to call this method in order to use always_sub...
ros::NodeHandle & getMTPrivateNodeHandle() const
virtual void warnNeverSubscribedCallback(const ros::WallTimerEvent &event)
callback function which is called when walltimer duration run out.
virtual void cameraConnectionCallback(const image_transport::SingleSubscriberPublisher &pub)
callback function which is called when new subscriber come for camera image publisher ...
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
virtual void unsubscribe()=0
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
bool ever_subscribed_
A flag to check if the node has been ever subscribed or not.
ros::NodeHandle & getNodeHandle() const
virtual void imageConnectionCallback(const image_transport::SingleSubscriberPublisher &pub)
callback function which is called when new subscriber come for image publisher
std::vector< image_transport::CameraPublisher > camera_publishers_
List of watching camera publishers.
#define NODELET_INFO(...)
uint32_t getNumSubscribers() const
ros::WallTimer timer_warn_on_init_post_process_called_
WallTimer instance for warning about no connection.
virtual void warnOnInitPostProcessCalledCallback(const ros::WallTimerEvent &event)
callback function which is called when walltimer duration run out.
virtual void cameraConnectionBaseCallback()
callback function which is called when new subscriber come for camera image publisher or camera info ...
virtual bool isSubscribed()
Returns true when this nodelet subscribes topics, false otherwise.
boost::mutex connection_mutex_
mutex to call subscribe() and unsubscribe() in critical section.
bool verbose_connection_
true if ~verbose_connection or verbose_connection parameter is true.
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
std::vector< ros::Publisher > publishers_
List of watching publishers.
bool on_init_post_process_called_
Never warning on not calling onInitPostProcess if true.
#define NODELET_DEBUG(...)
virtual void subscribe()=0
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...


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