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
00029
00030
00031 #include <cstdio>
00032 #include "ros/console.h"
00033 #include "ros/ros.h"
00034 #include "topic_tools/MuxSelect.h"
00035 #include "topic_tools/parse.h"
00036 using namespace std;
00037 using namespace ros;
00038 using namespace topic_tools;
00039
00040 int main(int argc, char **argv)
00041 {
00042 ROS_WARN("topic_tools/switch_mux is deprecated; please use topic_tools/mux_select instead");
00043 if (argc < 3)
00044 {
00045 printf("usage: switch MUXED_TOPIC SELECT_TOPIC\n");
00046 return 1;
00047 }
00048 std::string topic_name;
00049 if(!getBaseName(string(argv[1]), topic_name))
00050 return 1;
00051 ros::init(argc, argv, topic_name + string("_switcher"));
00052 ros::NodeHandle nh;
00053 string srv_name = string(argv[1]) + "_select";
00054 ROS_INFO("Waiting for service %s...\n", srv_name.c_str());
00055 ros::service::waitForService(srv_name, -1);
00056 ros::ServiceClient client = nh.serviceClient<MuxSelect>(srv_name);
00057 MuxSelect cmd;
00058 cmd.request.topic = argv[2];
00059 if (client.call(cmd))
00060 {
00061 ROS_INFO("muxed topic %s successfully switched from %s to %s",
00062 argv[1], cmd.response.prev_topic.c_str(),
00063 cmd.request.topic.c_str());
00064 return 0;
00065 }
00066 else
00067 {
00068 ROS_ERROR("failed to switch muxed topic %s to %s",
00069 argv[1], cmd.request.topic.c_str());
00070 return 1;
00071 }
00072 }
00073