From a4b6c8888a43ecfac71c6d2e3d29167bf94262ed Mon Sep 17 00:00:00 2001 From: Brian Gerkey Date: Thu, 17 Dec 2009 18:22:01 +0000 Subject: [PATCH] Fixed bug in select service, added check for __none in add service, added list service. --- tools/topic_tools/scripts/mux_add | 3 +- tools/topic_tools/scripts/mux_delete | 3 +- tools/topic_tools/scripts/mux_list | 76 ++++++++++++++++++++++++++++ tools/topic_tools/scripts/mux_select | 3 +- tools/topic_tools/src/mux.cpp | 48 ++++++++++++++++-- tools/topic_tools/srv/MuxList.srv | 2 + 6 files changed, 127 insertions(+), 8 deletions(-) create mode 100755 tools/topic_tools/scripts/mux_list create mode 100644 tools/topic_tools/srv/MuxList.srv diff --git a/tools/topic_tools/scripts/mux_add b/tools/topic_tools/scripts/mux_add index 7f6bfa60..4a8a7434 100755 --- a/tools/topic_tools/scripts/mux_add +++ b/tools/topic_tools/scripts/mux_add @@ -49,10 +49,11 @@ from topic_tools.srv import MuxAdd USAGE = 'USAGE: mux_add MUX_NAME TOPIC' def call_srv(m, t): - print "Adding \"%s\" to mux \"%s\""%(t, m) # There's probably a nicer rospy way of doing this s = m + '/add' + print "Waiting for service \"%s\""%(s) rospy.wait_for_service(s) + print "Adding \"%s\" to mux \"%s\""%(t, m) try: srv = rospy.ServiceProxy(s, MuxAdd) return srv(t) diff --git a/tools/topic_tools/scripts/mux_delete b/tools/topic_tools/scripts/mux_delete index 52c482e8..86ba53e2 100755 --- a/tools/topic_tools/scripts/mux_delete +++ b/tools/topic_tools/scripts/mux_delete @@ -49,10 +49,11 @@ from topic_tools.srv import MuxDelete USAGE = 'USAGE: mux_delete MUX_NAME TOPIC' def call_srv(m, t): - print "Deleting \"%s\" from mux \"%s\""%(t, m) # There's probably a nicer rospy way of doing this s = m + '/delete' + print "Waiting for service \"%s\""%(s) rospy.wait_for_service(s) + print "Deleting \"%s\" from mux \"%s\""%(t, m) try: srv = rospy.ServiceProxy(s, MuxDelete) return srv(t) diff --git a/tools/topic_tools/scripts/mux_list b/tools/topic_tools/scripts/mux_list new file mode 100755 index 00000000..94eb2bab --- /dev/null +++ b/tools/topic_tools/scripts/mux_list @@ -0,0 +1,76 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Revision $Id: add_two_ints_client 3804 2009-02-11 02:16:00Z rob_wheeler $ + +PKG = 'topic_tools' # this package name + +import roslib; roslib.load_manifest(PKG) + +import sys +import os +import string + +import rospy + +# imports the MuxList service +from topic_tools.srv import MuxList + +USAGE = 'USAGE: mux_list MUX_NAME' + +def call_srv(m): + # There's probably a nicer rospy way of doing this + s = m + '/list' + print "Waiting for service \"%s\""%(s) + rospy.wait_for_service(s) + print "Listing topics from mux \"%s\""%(m) + try: + srv = rospy.ServiceProxy(s, MuxList) + resp = srv() + if resp: + print resp.topics + return resp + except rospy.ServiceException, e: + print "Service call failed: %s"%e + +def usage(): + return "%s "%sys.argv[0] + +if __name__ == "__main__": + args = rospy.myargv() + if len(args) != 2: + print USAGE + sys.exit(1) + + m = args[1] + call_srv(m) diff --git a/tools/topic_tools/scripts/mux_select b/tools/topic_tools/scripts/mux_select index 39d70189..714ba89f 100755 --- a/tools/topic_tools/scripts/mux_select +++ b/tools/topic_tools/scripts/mux_select @@ -49,10 +49,11 @@ from topic_tools.srv import MuxSelect USAGE = 'USAGE: mux_switch MUX_NAME TOPIC' def call_srv(m, t): - print "Selecting \"%s\" at mux \"%s\""%(t, m) # There's probably a nicer rospy way of doing this s = m + '/select' + print "Waiting for service \"%s\""%(s) rospy.wait_for_service(s) + print "Selecting \"%s\" at mux \"%s\""%(t, m) try: srv = rospy.ServiceProxy(s, MuxSelect) return srv(t) diff --git a/tools/topic_tools/src/mux.cpp b/tools/topic_tools/src/mux.cpp index 45ca017b..025af24f 100644 --- a/tools/topic_tools/src/mux.cpp +++ b/tools/topic_tools/src/mux.cpp @@ -36,6 +36,7 @@ #include "std_msgs/String.h" #include "topic_tools/MuxSelect.h" #include "topic_tools/MuxAdd.h" +#include "topic_tools/MuxList.h" #include "topic_tools/MuxDelete.h" #include "topic_tools/shape_shifter.h" #include "topic_tools/parse.h" @@ -45,6 +46,7 @@ using std::vector; using std::list; using namespace topic_tools; +const static string g_none_topic = "__none"; static ShapeShifter *g_selected = NULL; static ros::NodeHandle *g_node = NULL; static bool g_advertised = false; @@ -69,7 +71,7 @@ bool sel_srv_cb( topic_tools::MuxSelect::Request &req, else res.prev_topic = string(""); // see if it's the magical '__none' topic, in which case we open the circuit - if (req.topic == string("__none")) + if (req.topic == g_none_topic) { ROS_INFO("mux selected to no input."); g_selected = NULL; @@ -92,9 +94,12 @@ bool sel_srv_cb( topic_tools::MuxSelect::Request &req, } } - std_msgs::String t; - t.data = g_selected->topic; - g_pub_selected.publish(t); + if(ret) + { + std_msgs::String t; + t.data = req.topic; + g_pub_selected.publish(t); + } return ret; } @@ -120,11 +125,33 @@ void in_cb(const boost::shared_ptr& msg, g_pub.publish(msg); } +bool list_topic_cb(topic_tools::MuxList::Request& req, + topic_tools::MuxList::Response& res) +{ + for (list::iterator it = g_subs.begin(); + it != g_subs.end(); + ++it) + { + res.topics.push_back(it->msg->topic); + } + + return true; +} + bool add_topic_cb(topic_tools::MuxAdd::Request& req, topic_tools::MuxAdd::Response& res) { // Check that it's not already in our list ROS_INFO("trying to add %s to mux", req.topic.c_str()); + + // Can't add the __none topic + if(req.topic == g_none_topic) + { + ROS_WARN("failed to add topic %s to mux, because it's reserved for special use", + req.topic.c_str()); + return false; + } + // spin through our vector of inputs and find this guy for (list::iterator it = g_subs.begin(); it != g_subs.end(); @@ -139,9 +166,19 @@ bool add_topic_cb(topic_tools::MuxAdd::Request& req, } struct sub_info_t sub_info; + try + { + sub_info.sub = g_node->subscribe(req.topic, 10, boost::bind(in_cb, _1, sub_info.msg)); + } + catch(ros::InvalidNameException& e) + { + ROS_WARN("failed to add topic %s to mux, because it's an invalid name: %s", + req.topic.c_str(), e.what()); + return false; + } + sub_info.msg = new ShapeShifter; sub_info.msg->topic = req.topic; - sub_info.sub = g_node->subscribe(req.topic, 10, boost::bind(in_cb, _1, sub_info.msg)); g_subs.push_back(sub_info); ROS_INFO("added %s to mux", req.topic.c_str()); @@ -203,6 +240,7 @@ int main(int argc, char **argv) // New service ros::ServiceServer ss_select = mux_nh.advertiseService(string("select"), sel_srv_cb); ros::ServiceServer ss_add = mux_nh.advertiseService(string("add"), add_topic_cb); + ros::ServiceServer ss_list = mux_nh.advertiseService(string("list"), list_topic_cb); ros::ServiceServer ss_del = mux_nh.advertiseService(string("delete"), del_topic_cb); for (size_t i = 0; i < topics.size(); i++) { diff --git a/tools/topic_tools/srv/MuxList.srv b/tools/topic_tools/srv/MuxList.srv new file mode 100644 index 00000000..a30d4d45 --- /dev/null +++ b/tools/topic_tools/srv/MuxList.srv @@ -0,0 +1,2 @@ +--- +string[] topics