block_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 #include <ros/advertise_options.h>
38 
39 namespace jsk_topic_tools
40 {
41 
42 
43 
45  {
49  pnh_.param("check_rate", check_rate_, 1.0); // defaults to 1Hz
53  this);
56  "output_original", 1,
58  this);
60  &Block::timerCallback, this);
61  }
62 
64  {
65  NODELET_DEBUG("timerCallback");
67  // if the publishers are not advertised, we need to wait until
68  // the subscribers get the first messages
69 
70  // next, check the number of the publisher
71  if (pub_output_.getNumSubscribers() > 0) {
72  NODELET_DEBUG("subscribe input");
73  // we need to run all the subscribers
76  "input", 1,
78  this);
80  }
82  NODELET_DEBUG("subscribe output original");
85  "output_original", 1,
87  this);
89  }
90  }
91  else {
92  // we need to shutdown the subscribers
94  NODELET_DEBUG("shutdown input");
96  sub_input_subscribing_ = false;
97  }
99  NODELET_DEBUG("shutdown output_original");
102  }
103  }
104  }
105  }
106 
109  {
110  NODELET_DEBUG("inputCallback");
111  // first we need to check is the publisher is advertised or not
113  NODELET_DEBUG("advertise input_original");
114  pub_input_original_ = msg->advertise(pnh_, "input_original", 1);
116  // shutdown subscriber
118  NODELET_DEBUG("shutdown input");
120  }
121  else {
122  NODELET_DEBUG("publish input_original");
124  }
125  }
126  else {
127  NODELET_DEBUG("publish input_original");
129  }
130  }
131 
134  {
135  NODELET_DEBUG("outputOriginalCallback");
136  // first we need to check is the publisher is advertised or not
137  if (!pub_output_advertised_) {
138  NODELET_DEBUG("advertise output");;
139  pub_output_ = msg->advertise(pnh_, "output", 1);
140  pub_output_advertised_ = true;
141  // shutdown subscriber
144  NODELET_DEBUG("shutdown input");
146  }
147  else {
148  NODELET_DEBUG("publish output");
149  pub_output_.publish(msg);
150  }
151  }
152  else {
153  NODELET_DEBUG("publish output");
154  pub_output_.publish(msg);
155  }
156  }
157 }
158 
161 
ros::Publisher pub_input_original_
Definition: block_nodelet.h:68
void publish(const boost::shared_ptr< M > &message) const
ros::Subscriber sub_input_
Definition: block_nodelet.h:67
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 void outputOriginalCallback(const boost::shared_ptr< topic_tools::ShapeShifter const > &msg)
ros::Publisher pub_output_
Definition: block_nodelet.h:68
virtual void timerCallback(const ros::TimerEvent &event)
ros::NodeHandle & getPrivateNodeHandle() const
jsk_topic_tools::Block Block
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::NodeHandle pnh_
Definition: block_nodelet.h:64
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ros::Subscriber sub_output_original_
Definition: block_nodelet.h:67
uint32_t getNumSubscribers() const
virtual void inputCallback(const boost::shared_ptr< topic_tools::ShapeShifter const > &msg)
#define NODELET_DEBUG(...)


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