Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef NODELET_NODELET_DEMUX_H_
00038 #define NODELET_NODELET_DEMUX_H_
00039
00040 #include <ros/ros.h>
00041 #include <nodelet/nodelet.h>
00042 #include <message_filters/time_synchronizer.h>
00043 #include <message_filters/subscriber.h>
00044
00045 namespace nodelet
00046 {
00050 template <typename T, typename Subscriber = message_filters::Subscriber<T> >
00051 class NodeletDEMUX: public Nodelet
00052 {
00053 typedef typename boost::shared_ptr<const T> TConstPtr;
00054 public:
00056
00057 void
00058 onInit ()
00059 {
00060 private_nh_ = getPrivateNodeHandle ();
00061 sub_input_.subscribe (private_nh_, "input", 1, bind (&NodeletDEMUX<T,Subscriber>::input_callback, this, _1));
00062
00063 if (!private_nh_.getParam ("output_topics", output_topics_))
00064 {
00065 ROS_ERROR ("[nodelet::NodeletDEMUX::init] Need a 'output_topics' parameter to be set before continuing!");
00066 return;
00067 }
00068
00069 switch (output_topics_.getType ())
00070 {
00071 case XmlRpc::XmlRpcValue::TypeArray:
00072 {
00073 if (output_topics_.size () == 1)
00074 {
00075 ROS_ERROR ("[nodelet::NodeletDEMUX::init] Only one topic given. Does it make sense to passthrough?");
00076 return;
00077 }
00078
00079 if (output_topics_.size () > 8)
00080 {
00081 ROS_ERROR ("[nodelet::NodeletDEMUX::init] More than 8 topics passed!");
00082 return;
00083 }
00084
00085 ROS_INFO_STREAM ("[nodelet::NodeletDEMUX::init] Publishing to " << output_topics_.size () << " user given topics as outputs:");
00086 for (int d = 0; d < output_topics_.size (); ++d)
00087 ROS_INFO_STREAM (" - " << (std::string)(output_topics_[d]));
00088
00089 pubs_output_.resize (output_topics_.size ());
00090 for (int d = 0; d < output_topics_.size (); ++d)
00091 *pubs_output_[d] = private_nh_.template advertise<T> ((std::string)(output_topics_[d]), 1);
00092 break;
00093 }
00094 default:
00095 {
00096 ROS_ERROR ("[nodelet::NodeletDEMUX::init] Invalid 'output_topics' parameter given!");
00097 return;
00098 }
00099 }
00100 }
00101
00102 private:
00103
00105 void
00106 input_callback (const TConstPtr &input)
00107 {
00108 for (size_t d = 0; d < pubs_output_.size (); ++d)
00109 pubs_output_[d]->publish (input);
00110 }
00111
00113 ros::NodeHandle private_nh_;
00115 std::vector<boost::shared_ptr <ros::Publisher> > pubs_output_;
00117 Subscriber sub_input_;
00118
00119
00121 XmlRpc::XmlRpcValue output_topics_;
00122 };
00123
00127 template <typename T>
00128 class NodeletDEMUX<T, message_filters::Subscriber<T> >: public Nodelet
00129 {
00130 typedef typename boost::shared_ptr<const T> TConstPtr;
00131 public:
00133
00134 void
00135 onInit ()
00136 {
00137 private_nh_ = getPrivateNodeHandle ();
00138
00139 sub_input_ = private_nh_.subscribe ("input", 1, &NodeletDEMUX<T>::input_callback, this);
00140
00141 if (!private_nh_.getParam ("output_topics", output_topics_))
00142 {
00143 ROS_ERROR ("[nodelet::NodeletDEMUX::init] Need a 'output_topics' parameter to be set before continuing!");
00144 return;
00145 }
00146
00147 switch (output_topics_.getType ())
00148 {
00149 case XmlRpc::XmlRpcValue::TypeArray:
00150 {
00151 if (output_topics_.size () == 1)
00152 {
00153 ROS_ERROR ("[nodelet::NodeletDEMUX::init] Only one topic given. Does it make sense to passthrough?");
00154 return;
00155 }
00156
00157 if (output_topics_.size () > 8)
00158 {
00159 ROS_ERROR ("[nodelet::NodeletDEMUX::init] More than 8 topics passed!");
00160 return;
00161 }
00162
00163 ROS_INFO_STREAM ("[nodelet::NodeletDEMUX::init] Publishing to " << output_topics_.size () << " user given topics as outputs:");
00164 for (int d = 0; d < output_topics_.size (); ++d)
00165 ROS_INFO_STREAM (" - " << (std::string)(output_topics_[d]));
00166
00167 pubs_output_.resize (output_topics_.size ());
00168 for (int d = 0; d < output_topics_.size (); ++d)
00169 *pubs_output_[d] = private_nh_.template advertise<T> ((std::string)(output_topics_[d]), 1);
00170 break;
00171 }
00172 default:
00173 {
00174 ROS_ERROR ("[nodelet::NodeletDEMUX::init] Invalid 'output_topics' parameter given!");
00175 return;
00176 }
00177 }
00178 }
00179
00180 private:
00181
00183 void
00184 input_callback (const TConstPtr &input)
00185 {
00186 for (size_t d = 0; d < pubs_output_.size (); ++d)
00187 pubs_output_[d]->publish (input);
00188 }
00189
00191 ros::NodeHandle private_nh_;
00193 std::vector<boost::shared_ptr <ros::Publisher> > pubs_output_;
00195 ros::Subscriber sub_input_;
00196
00197
00199 XmlRpc::XmlRpcValue output_topics_;
00200 };
00201
00202 }
00203 #endif