Fixed bug in select service, added check for __none in add service, added

list service.

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)

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)

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)

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)

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<ShapeShifter const>& msg,
     g_pub.publish(msg);
 }
 
+bool list_topic_cb(topic_tools::MuxList::Request& req,
+	 	   topic_tools::MuxList::Response& res)
+{
+  for (list<struct sub_info_t>::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<struct sub_info_t>::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<ShapeShifter>(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<ShapeShifter>(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++)
   {

tools/topic_tools/srv/MuxList.srv

@@ -0,0 +1,2 @@
+---
+string[] topics