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 and topic subscription are provided
00004 // to select between the outputs.
00005 //
00006 // Copyright (C) 2009, Morgan Quigley
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 #include "ros/node.h"
00033 #include "std_msgs/String.h"
00034 #include "topic_tools/DemuxSelect.h"
00035 #include "topic_tools/shape_shifter.h"
00036 
00037 using std::string;
00038 using std::vector;
00039 using std::map;
00040 using namespace topic_tools;
00041 
00042 static std_msgs::String g_sel_msg;
00043 static vector<string> g_output_topics, g_selected;
00044 static bool g_advertised = false;
00045 static ros::Node *g_node = NULL;
00046 static ShapeShifter g_in_msg;
00047 
00048 bool sel_srv_cb( topic_tools::DemuxSelect::Request  &req,
00049                  topic_tools::DemuxSelect::Response &res )
00050 {
00051   res.prev_topics = g_selected;
00052   g_selected = req.topics;
00053   printf("enabling demux on these topics:\n");
00054   for (size_t i = 0; i < g_selected.size(); ++i)
00055     printf("  %s\n", g_selected[i].c_str());
00056   return true;
00057 }
00058 
00059 void in_cb()
00060 {
00061   if (!ShapeShifter::typed)
00062   {
00063     printf("reading first message header, setting datatype\n");
00064     ShapeShifter::datatype = (*(g_in_msg.__connection_header))["type"];
00065     ShapeShifter::md5 = (*(g_in_msg.__connection_header))["md5sum"];
00066     ShapeShifter::msg_def = (*(g_in_msg.__connection_header))["message_definition"];
00067     ShapeShifter::typed = true;
00068   }
00069   if (!g_advertised)
00070   {
00071     printf("advertising\n");
00072     for (size_t i = 0; i < g_output_topics.size(); i++)
00073       g_node->advertise<ShapeShifter>(g_output_topics[i], 10);
00074     g_advertised = true;
00075   }
00076   for (size_t i = 0; i < g_selected.size(); i++)
00077     g_node->publish(g_selected[i], g_in_msg);
00078 }
00079 
00080 int main(int argc, char **argv)
00081 {
00082   if (argc < 4)
00083   {
00084     printf("\nusage: demux IN_TOPIC OUT_TOPIC1 OUT_TOPIC2 [...]\n");
00085     return 1;
00086   }
00087   string in_topic = string(argv[1]);
00088   ros::init(argc, argv, in_topic + string("_demux"));
00089   for (int i = 2; i < argc; i++)
00090     g_output_topics.push_back(argv[i]);
00091   ros::Node n(in_topic + string("_demux"));
00092   g_node = &n;
00093   n.advertiseService(in_topic + string("_demux_select"), sel_srv_cb);
00094   g_in_msg.topic = in_topic;
00095   n.subscribe(in_topic, g_in_msg, in_cb, 10);
00096   g_selected.push_back(g_output_topics[0]); // select the first one to start
00097   ros::spin();
00098   return 0;
00099 }
00100 


topic_tools
Author(s): Morgan Quigley, Brian Gerkey
autogenerated on Fri Aug 28 2015 12:33:48