$search
00001 #!/usr/bin/env python 00002 00003 # Software License Agreement (BSD License) 00004 # 00005 # Copyright (c) 2008, Willow Garage, Inc. 00006 # All rights reserved. 00007 # 00008 # Redistribution and use in source and binary forms, with or without 00009 # modification, are permitted provided that the following conditions 00010 # are met: 00011 # 00012 # * Redistributions of source code must retain the above copyright 00013 # notice, this list of conditions and the following disclaimer. 00014 # * Redistributions in binary form must reproduce the above 00015 # copyright notice, this list of conditions and the following 00016 # disclaimer in the documentation and/or other materials provided 00017 # with the distribution. 00018 # * Neither the name of Willow Garage, Inc. nor the names of its 00019 # contributors may be used to endorse or promote products derived 00020 # from this software without specific prior written permission. 00021 # 00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 # POSSIBILITY OF SUCH DAMAGE. 00034 # 00035 # Author: Brian Gerkey 00036 00037 PKG = 'topic_tools' 00038 import roslib; roslib.load_manifest(PKG) 00039 00040 import unittest 00041 import rospy 00042 00043 from topic_tools.srv import MuxAdd 00044 from topic_tools.srv import MuxDelete 00045 from topic_tools.srv import MuxList 00046 from topic_tools.srv import MuxSelect 00047 00048 class MuxServiceTestCase(unittest.TestCase): 00049 def make_srv_proxies(self): 00050 try: 00051 rospy.wait_for_service('mux/add', 5) 00052 rospy.wait_for_service('mux/delete', 5) 00053 rospy.wait_for_service('mux/list', 5) 00054 rospy.wait_for_service('mux/select', 5) 00055 except rospy.ROSException, e: 00056 self.fail('failed to find a required service: ' + `e`) 00057 00058 add_srv = rospy.ServiceProxy('mux/add', MuxAdd) 00059 delete_srv = rospy.ServiceProxy('mux/delete', MuxDelete) 00060 list_srv = rospy.ServiceProxy('mux/list', MuxList) 00061 select_srv = rospy.ServiceProxy('mux/select', MuxSelect) 00062 00063 return (add_srv, delete_srv, list_srv, select_srv) 00064 00065 def test_add_delete_list(self): 00066 add_srv, delete_srv, list_srv, select_srv = self.make_srv_proxies() 00067 # Check initial condition 00068 topics = list_srv().topics 00069 self.assertEquals(set(topics), set(['/input'])) 00070 # Add a topic and make sure it's there 00071 add_srv('/new_input') 00072 topics = list_srv().topics 00073 self.assertEquals(set(topics), set(['/input', '/new_input'])) 00074 # Try to add the same topic again, make sure it fails, and that 00075 # nothing changes. 00076 try: 00077 add_srv('/new_input') 00078 except rospy.ServiceException, e: 00079 pass 00080 else: 00081 self.fail('service call should have thrown an exception') 00082 topics = list_srv().topics 00083 self.assertEquals(set(topics), set(['/input', '/new_input'])) 00084 # Select a topic, then try to delete it, make sure it fails, and 00085 # that nothing changes. 00086 select_srv('/input') 00087 try: 00088 delete_srv('/input') 00089 except rospy.ServiceException, e: 00090 pass 00091 else: 00092 self.fail('service call should have thrown an exception') 00093 topics = list_srv().topics 00094 self.assertEquals(set(topics), set(['/input', '/new_input'])) 00095 # Select nothing, to allow deletion 00096 select_srv('__none') 00097 # Delete topics 00098 delete_srv('/input') 00099 topics = list_srv().topics 00100 self.assertEquals(set(topics), set(['/new_input'])) 00101 delete_srv('/new_input') 00102 topics = list_srv().topics 00103 self.assertEquals(set(topics), set([])) 00104 00105 if __name__ == "__main__": 00106 import rostest 00107 rostest.unitrun(PKG, 'mux_services', MuxServiceTestCase) 00108