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
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]);
00097 ros::spin();
00098 return 0;
00099 }
00100