nodelet_demux.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNERff OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: nodelet_mux.h 27892 2010-02-25 06:47:29Z rusu $
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         // Check the type
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         // Check the type
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


nodelet_topic_tools
Author(s): Radu Bogdan Rusu, Tully Foote
autogenerated on Wed Aug 26 2015 14:56:48