demux.cpp
Go to the documentation of this file.
00001 
00002 // demux is a generic ROS topic demultiplexer: one input topic is fanned out
00003 // to 1 of N output topics. A service is provided to select between the outputs
00004 //
00005 // Copyright (C) 2009, Morgan Quigley
00006 // Copyright (C) 2014, Andreas Hermann
00007 //
00008 // Redistribution and use in source and binary forms, with or without
00009 // modification, are permitted provided that the following conditions are met:
00010 //   * Redistributions of source code must retain the above copyright notice,
00011 //     this list of conditions and the following disclaimer.
00012 //   * Redistributions in binary form must reproduce the above copyright
00013 //     notice, this list of conditions and the following disclaimer in the
00014 //     documentation and/or other materials provided with the distribution.
00015 //   * Neither the name of Stanford University nor the names of its
00016 //     contributors may be used to endorse or promote products derived from
00017 //     this software without specific prior written permission.
00018 //
00019 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 // POSSIBILITY OF SUCH DAMAGE.
00031 
00032 
00033 #include <cstdio>
00034 #include <vector>
00035 #include <list>
00036 #include "ros/console.h"
00037 #include "std_msgs/String.h"
00038 #include "topic_tools/DemuxSelect.h"
00039 #include "topic_tools/DemuxAdd.h"
00040 #include "topic_tools/DemuxList.h"
00041 #include "topic_tools/DemuxDelete.h"
00042 #include "topic_tools/shape_shifter.h"
00043 #include "topic_tools/parse.h"
00044 
00045 using std::string;
00046 using std::vector;
00047 using std::list;
00048 using namespace topic_tools;
00049 
00050 const static string g_none_topic = "__none";
00051 
00052 static ros::NodeHandle *g_node = NULL;
00053 
00054 static string g_input_topic;
00055 static ros::Subscriber g_sub; // the input toppic
00056 static ros::Publisher g_pub_selected; // publishes name of selected publisher toppic
00057 
00058 struct pub_info_t
00059 {
00060   std::string topic_name;
00061   ros::Publisher *pub;
00062 };
00063 
00064 void in_cb(const boost::shared_ptr<ShapeShifter const>& msg);
00065 
00066 static list<struct pub_info_t> g_pubs; // the list of publishers
00067 static list<struct pub_info_t>::iterator g_selected = g_pubs.end();
00068 
00069 bool sel_srv_cb( topic_tools::DemuxSelect::Request  &req,
00070                  topic_tools::DemuxSelect::Response &res )
00071 {
00072   bool ret = false;
00073   if (g_selected != g_pubs.end()) {
00074     res.prev_topic = g_selected->topic_name;
00075 
00076     // Unregister old topic
00077     ROS_INFO("Unregistering %s", res.prev_topic.c_str());
00078     if (g_selected->pub)
00079           g_selected->pub->shutdown();
00080     delete g_selected->pub;
00081     g_selected->pub = NULL;
00082   }
00083   else
00084     res.prev_topic = string("");
00085 
00086   // see if it's the magical '__none' topic, in which case we open the circuit
00087   if (req.topic == g_none_topic)
00088   {
00089     ROS_INFO("demux selected to no output.");
00090 
00091     g_selected = g_pubs.end();
00092     ret = true;
00093   }
00094   else
00095   {
00096     ROS_INFO("trying to switch demux to %s", req.topic.c_str());
00097     // spin through our vector of inputs and find this guy
00098     for (list<struct pub_info_t>::iterator it = g_pubs.begin();
00099          it != g_pubs.end();
00100          ++it)
00101     {
00102       if (ros::names::resolve(it->topic_name) == ros::names::resolve(req.topic))
00103       {
00104         g_selected = it;
00105         ROS_INFO("demux selected output: [%s]", it->topic_name.c_str());
00106         ret = true;
00107       }
00108     }
00109     if(!ret)
00110     {
00111         ROS_WARN("Failed to switch to non-existing topic %s in demux.", req.topic.c_str());
00112     }
00113   }
00114 
00115   if(ret)
00116   {
00117     std_msgs::String t;
00118     t.data = req.topic;
00119     g_pub_selected.publish(t);
00120   }
00121 
00122   return ret;
00123 }
00124 
00125 void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
00126 {
00127   ROS_DEBUG("Received an incoming msg ...");
00128   // when a message is incoming, check, if the requested publisher is already existing.
00129   // if not, create it with the information available from the incoming message.
00130   if(!g_selected->pub)
00131   {
00132           try
00133           {
00134                   g_selected->pub = new ros::Publisher(msg->advertise(*g_node, g_selected->topic_name, 10, false));
00135           }
00136           catch(ros::InvalidNameException& e)
00137           {
00138                 ROS_WARN("failed to add topic %s to demux, because it's an invalid name: %s",
00139                                 g_selected->topic_name.c_str(), e.what());
00140                 return;
00141           }
00142 
00143           ROS_INFO("Added publisher %s to demux! Sleeping for 0.5 secs.", g_selected->topic_name.c_str());
00144           // This is needed, because it takes some time before publisher is registered and can send out messages.
00145           ros::Duration(0.5).sleep();
00146   }
00147 
00148   // finally: send out the message over the active publisher
00149   g_selected->pub->publish(msg);
00150   ROS_DEBUG("... and sent it out again!");
00151 }
00152 
00153 bool list_topic_cb(topic_tools::DemuxList::Request& req,
00154                    topic_tools::DemuxList::Response& res)
00155 {
00156   (void)req;
00157   for (list<struct pub_info_t>::iterator it = g_pubs.begin();
00158        it != g_pubs.end();
00159        ++it)
00160   {
00161     res.topics.push_back(it->topic_name);
00162   }
00163 
00164   return true;
00165 }
00166 
00167 bool add_topic_cb(topic_tools::DemuxAdd::Request& req,
00168                   topic_tools::DemuxAdd::Response& res)
00169 {
00170   (void)res;
00171   // Check that it's not already in our list
00172   ROS_INFO("trying to add %s to demux", req.topic.c_str());
00173 
00174   // Can't add the __none topic
00175   if(req.topic == g_none_topic)
00176   {
00177     ROS_WARN("failed to add topic %s to demux, because it's reserved for special use",
00178              req.topic.c_str());
00179     return false;
00180   }
00181 
00182   // spin through our vector of inputs and find this guy
00183   for (list<struct pub_info_t>::iterator it = g_pubs.begin();
00184        it != g_pubs.end();
00185        ++it)
00186   {
00187     if (ros::names::resolve(it->topic_name) == ros::names::resolve(req.topic))
00188     {
00189       ROS_WARN("tried to add a topic that demux was already publishing: [%s]",
00190                it->topic_name.c_str());
00191       return false;
00192     }
00193   }
00194 
00195   struct pub_info_t pub_info;
00196   pub_info.topic_name = ros::names::resolve(req.topic);
00197   pub_info.pub = NULL;
00198   g_pubs.push_back(pub_info);
00199 
00200   ROS_INFO("PRE added %s to demux", req.topic.c_str());
00201 
00202   return true;
00203 }
00204 
00205 bool del_topic_cb(topic_tools::DemuxDelete::Request& req,
00206                   topic_tools::DemuxDelete::Response& res)
00207 {
00208   (void)res;
00209   // Check that it's in our list
00210   ROS_INFO("trying to delete %s from demux", req.topic.c_str());
00211   // spin through our vector of inputs and find this guy
00212   for (list<struct pub_info_t>::iterator it = g_pubs.begin();
00213        it != g_pubs.end();
00214        ++it)
00215   {
00216     if (ros::names::resolve(it->topic_name) == ros::names::resolve(req.topic))
00217     {
00218       // Can't delete the currently selected input, #2863
00219       if(it == g_selected)
00220       {
00221         ROS_WARN("tried to delete currently selected topic %s from demux", req.topic.c_str());
00222         return false;
00223       }
00224       if (it->pub)
00225         it->pub->shutdown();
00226       delete it->pub;
00227       g_pubs.erase(it);
00228       ROS_INFO("deleted topic %s from demux", req.topic.c_str());
00229       return true;
00230     }
00231   }
00232 
00233   ROS_WARN("tried to delete non-published topic %s from demux", req.topic.c_str());
00234   return false;
00235 }
00236 
00237 int main(int argc, char **argv)
00238 {
00239   vector<string> args;
00240   ros::removeROSArgs(argc, (const char**)argv, args);
00241 
00242   if (args.size() < 3)
00243   {
00244     printf("\nusage: demux IN_TOPIC OUT_TOPIC1 [OUT_TOPIC2 [...]]\n\n");
00245     return 1;
00246   }
00247   std::string topic_name;
00248   if(!getBaseName(args[1], topic_name))
00249     return 1;
00250   ros::init(argc, argv, topic_name + string("_demux"),
00251             ros::init_options::AnonymousName);
00252   vector<string> topics;
00253   for (unsigned int i = 2; i < args.size(); i++)
00254     topics.push_back(args[i]);
00255   ros::NodeHandle n;
00256   g_node = &n;
00257   g_input_topic = args[1];
00258   // Put our API into the "demux" namespace, which the user should usually remap
00259   ros::NodeHandle demux_nh("demux"), pnh("~");
00260 
00261   // Latched publisher for selected output topic name
00262   g_pub_selected = demux_nh.advertise<std_msgs::String>(string("selected"), 1, true);
00263 
00264   for (size_t i = 0; i < topics.size(); i++)
00265   {
00266     struct pub_info_t pub_info;
00267     pub_info.topic_name = ros::names::resolve(topics[i]);
00268     pub_info.pub = NULL;
00269     g_pubs.push_back(pub_info);
00270     ROS_INFO("PRE added %s to demux", topics[i].c_str());
00271   }
00272   g_selected = g_pubs.begin(); // select first topic to start
00273   std_msgs::String t;
00274   t.data = g_selected->topic_name;
00275   g_pub_selected.publish(t);
00276 
00277   // Create the one subscriber
00278   g_sub = ros::Subscriber(n.subscribe<ShapeShifter>(g_input_topic, 10, boost::bind(in_cb, _1)));
00279 
00280 
00281   // New service
00282   ros::ServiceServer ss_select = demux_nh.advertiseService(string("select"), sel_srv_cb);
00283   ros::ServiceServer ss_add = demux_nh.advertiseService(string("add"), add_topic_cb);
00284   ros::ServiceServer ss_list = demux_nh.advertiseService(string("list"), list_topic_cb);
00285   ros::ServiceServer ss_del = demux_nh.advertiseService(string("delete"), del_topic_cb);
00286 
00287   // Run
00288   ros::spin();
00289 
00290   // Destruction
00291   for (list<struct pub_info_t>::iterator it = g_pubs.begin();
00292        it != g_pubs.end();
00293        ++it)
00294   {
00295     if (it->pub)
00296       it->pub->shutdown();
00297     delete it->pub;
00298   }
00299 
00300   g_pubs.clear();
00301   return 0;
00302 }
00303 


topic_tools
Author(s): Morgan Quigley, Brian Gerkey
autogenerated on Thu Jun 6 2019 21:10:59