diff --git a/test_nodelet_topic_tools/CMakeLists.txt b/test_nodelet_topic_tools/CMakeLists.txt index 11ab1a84..adba28fd 100644 --- a/test_nodelet_topic_tools/CMakeLists.txt +++ b/test_nodelet_topic_tools/CMakeLists.txt @@ -10,10 +10,15 @@ catkin_package( if(CATKIN_ENABLE_TESTING) include_directories(SYSTEM ${catkin_INCLUDE_DIRS}) - add_library(test_nodelet_topic_tools test/string_nodelet_lazy.cpp test/string_nodelet_throttle.cpp) + add_library(test_nodelet_topic_tools + test/string_nodelet_demux.cpp + test/string_nodelet_lazy.cpp + test/string_nodelet_throttle.cpp + ) target_link_libraries(test_nodelet_topic_tools ${catkin_LIBRARIES}) add_dependencies(test_nodelet_topic_tools ${catkin_EXPORTED_TARGETS}) + add_rostest(test/test_nodelet_demux.launch) add_rostest(test/test_nodelet_lazy.launch) add_rostest(test/test_nodelet_throttle.launch) endif() diff --git a/test_nodelet_topic_tools/test/string_nodelet_demux.cpp b/test_nodelet_topic_tools/test/string_nodelet_demux.cpp new file mode 100644 index 00000000..a8491132 --- /dev/null +++ b/test_nodelet_topic_tools/test/string_nodelet_demux.cpp @@ -0,0 +1,43 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, JSK Lab, University of Tokyo. + * 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/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab 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. + *********************************************************************/ + +/* Author: Kentaro Wada + */ + +#include +#include +#include + +typedef nodelet::NodeletDEMUX NodeletDemuxString; +PLUGINLIB_EXPORT_CLASS(NodeletDemuxString, nodelet::Nodelet); diff --git a/test_nodelet_topic_tools/test/test_demux.py b/test_nodelet_topic_tools/test/test_demux.py new file mode 100755 index 00000000..81cdc48b --- /dev/null +++ b/test_nodelet_topic_tools/test/test_demux.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python + +import rospy +import threading +import unittest + +from std_msgs.msg import String + + +class TestNodeletDemux(unittest.TestCase): + def __init__(self, *args): + super(TestNodeletDemux, self).__init__(*args) + + self._lock = threading.RLock() + + self._msgs_rec = {} + + self._pub = rospy.Publisher('input', String, queue_size=4) + self.output_topics = rospy.get_param('~output_topics', []) + rospy.loginfo(self.output_topics) + self._sub = {} + for key in self.output_topics: + self._sub[key] = rospy.Subscriber(key, String, self._cb, (key)) + + def _cb(self, msg, key): + with self._lock: + if key not in self._msgs_rec.keys(): + self._msgs_rec[key] = 0 + self._msgs_rec[key] += 1 + + def test_nodelet_demux(self): + self._msgs_rec = {} + num_each_to_receive = 4 + num_to_send = len(self.output_topics) * num_each_to_receive + self.assert_(num_to_send > 0, "No output topics to test with") + + # TODO(lucasw) is this a really brittle test, maybe a few messags will be missed + # and the assert below should be within 10-20% of expected? + rospy.sleep(1.0) + for i in range(num_to_send): + self._pub.publish(String('hello, world')) + rospy.sleep(0.2) + rospy.sleep(1.0) + + rospy.loginfo(self._msgs_rec) + for key in self.output_topics: + self.assert_(key in self._msgs_rec.keys(), "No messages received on " + + key + "\" " + str(num_each_to_receive)) + if key in self._msgs_rec.keys(): + self.assert_(self._msgs_rec[key] == num_each_to_receive, + "Wrong messages received from nodelet demux on topic \"" + + key + "\" " + str(self._msgs_rec[key]) + " != " + str(num_each_to_receive)) + +if __name__ == '__main__': + rospy.init_node('test_nodelet_demux') + + import rostest + rostest.rosrun('test_nodelet_topic_tools', 'test_nodelet_demux', TestNodeletDemux) diff --git a/test_nodelet_topic_tools/test/test_nodelet_demux.launch b/test_nodelet_topic_tools/test/test_nodelet_demux.launch new file mode 100644 index 00000000..04066631 --- /dev/null +++ b/test_nodelet_topic_tools/test/test_nodelet_demux.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + output_topics: [/input1, /input2] + + + + + + output_topics: [/input1, /input2] + + + + diff --git a/test_nodelet_topic_tools/test/test_nodelets.xml b/test_nodelet_topic_tools/test/test_nodelets.xml index aacb21f4..6e4003e6 100644 --- a/test_nodelet_topic_tools/test/test_nodelets.xml +++ b/test_nodelet_topic_tools/test/test_nodelets.xml @@ -9,4 +9,10 @@ Lazy transport strings in nodelet (testing only). + + + + Nodelet demultiplexer (testing only). + +