nodelet_mux.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
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/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. 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 OWNERff 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  *
34  * $Id$
35  *
36  */
37 #ifndef NODELET_NODELET_MUX_H_
38 #define NODELET_NODELET_MUX_H_
39 
40 #include <ros/ros.h>
41 #include <nodelet/nodelet.h>
44 
45 namespace nodelet
46 {
51  template <typename T, typename Filter>
52  class NodeletMUX: public Nodelet
53  {
54  typedef typename boost::shared_ptr<T> TPtr;
56 
57  public:
59 
61 
62  virtual void
63  onInit ()
64  {
66  pub_output_ = private_nh_.template advertise<T> ("output", 1);
67 
68  XmlRpc::XmlRpcValue input_topics;
69  if (!private_nh_.getParam ("input_topics", input_topics))
70  {
71  ROS_ERROR ("[nodelet::NodeletMUX::init] Need a 'input_topics' parameter to be set before continuing!");
72  return;
73  }
74 
75  private_nh_.getParam ("max_queue_size", maximum_queue_size_);
76 
77  // Check the type
78  switch (input_topics.getType ())
79  {
81  {
82  if (input_topics.size () == 1)
83  {
84  ROS_ERROR ("[nodelet::NodeletMUX::init] Only one topic given. Does it make sense to passthrough?");
85  return;
86  }
87 
88  if (input_topics.size () > 8)
89  {
90  ROS_ERROR ("[nodelet::NodeletMUX::init] More than 8 topics passed!");
91  return;
92  }
93 
94  ROS_INFO_STREAM ("[nodelet::NodeletMUX::init] Subscribing to " << input_topics.size () << " user given topics as inputs:");
95  for (int d = 0; d < input_topics.size (); ++d)
96  ROS_INFO_STREAM (" - " << (std::string)(input_topics[d]));
97 
98  // Subscribe to the filters
99  filters_.resize (input_topics.size ());
100  for (int d = 0; d < input_topics.size (); ++d)
101  {
102  filters_[d].reset (new Filter ());
103  filters_[d]->subscribe (private_nh_, (std::string)(input_topics[d]), 1);
104  }
105 
106  // Subscribe to 1 callback to fill in the passthrough
107  filters_[0]->registerCallback (boost::bind (&NodeletMUX<T,Filter>::filter_cb, this, _1));
108 
110 
111  switch (input_topics.size ())
112  {
113  case 2:
114  {
115  ts_->connectInput (*filters_[0], *filters_[1], nf_, nf_, nf_, nf_, nf_, nf_);
116  break;
117  }
118  case 3:
119  {
120  ts_->connectInput (*filters_[0], *filters_[1], *filters_[2], nf_, nf_, nf_, nf_, nf_);
121  break;
122  }
123  case 4:
124  {
125  ts_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], nf_, nf_, nf_, nf_);
126  break;
127  }
128  case 5:
129  {
130  ts_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], nf_, nf_, nf_);
131  break;
132  }
133  case 6:
134  {
135  ts_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], nf_, nf_);
136  break;
137  }
138  case 7:
139  {
140  ts_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], nf_);
141  break;
142  }
143  case 8:
144  {
145  ts_->connectInput (*filters_[0], *filters_[1], *filters_[2], *filters_[3], *filters_[4], *filters_[5], *filters_[6], *filters_[7]);
146  break;
147  }
148  default:
149  {
150  ROS_ERROR ("[nodelet::NodeletMUX::init] Invalid 'input_topics' parameter given!");
151  return;
152  }
153  }
154  break;
155  }
156  default:
157  {
158  ROS_ERROR ("[nodelet::NodeletMUX::init] Invalid 'input_topics' parameter given!");
159  return;
160  }
161  }
162 
163  ts_->registerCallback (boost::bind (&NodeletMUX<T,Filter>::input, this, _1, _2, _3, _4, _5, _6, _7, _8));
164  }
165 
166  private:
167 
168  void
169  filter_cb (const TConstPtr &input)
170  {
171  nf_.add (input);
172  }
173 
174  void
175  input (const TConstPtr &in1, const TConstPtr &in2, const TConstPtr &in3, const TConstPtr &in4,
176  const TConstPtr &in5, const TConstPtr &in6, const TConstPtr &in7, const TConstPtr &in8)
177  {
180  }
181 
186 
189 
192 
194  std::vector<boost::shared_ptr<Filter> > filters_;
195 
198  };
199 
200 }
201 #endif
d
boost::shared_ptr< T > TPtr
Definition: nodelet_mux.h:54
void publish(const boost::shared_ptr< M > &message) const
NodeletMUX represent a mux nodelet for topics: it takes N (<=8) input topics, and publishes all of th...
Definition: nodelet_mux.h:52
int maximum_queue_size_
The maximum number of messages that we can store in the queue.
Definition: nodelet_mux.h:191
void filter_cb(const TConstPtr &input)
Definition: nodelet_mux.h:169
ros::NodeHandle & getMTPrivateNodeHandle() const
virtual void onInit()
Nodelet initialization routine.
Definition: nodelet_mux.h:63
std::vector< boost::shared_ptr< Filter > > filters_
A vector of message filters.
Definition: nodelet_mux.h:194
ros::Publisher pub_output_
The output ROS publisher.
Definition: nodelet_mux.h:185
ros::NodeHandle private_nh_
ROS local node handle.
Definition: nodelet_mux.h:183
void input(const TConstPtr &in1, const TConstPtr &in2, const TConstPtr &in3, const TConstPtr &in4, const TConstPtr &in5, const TConstPtr &in6, const TConstPtr &in7, const TConstPtr &in8)
Definition: nodelet_mux.h:175
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
boost::shared_ptr< const T > TConstPtr
Definition: nodelet_mux.h:55
void add(const MConstPtr &msg)
message_filters::PassThrough< T > nf_
Null filter.
Definition: nodelet_mux.h:188
#define ROS_ERROR(...)
boost::shared_ptr< message_filters::TimeSynchronizer< T, T, T, T, T, T, T, T > > ts_
Synchronizer object.
Definition: nodelet_mux.h:197


nodelet_topic_tools
Author(s): Radu Bogdan Rusu, Tully Foote
autogenerated on Sat Jul 18 2020 03:17:55