Demux modes

nodelet_topic_tools/include/nodelet_topic_tools/nodelet_demux.h

@@ -41,24 +41,32 @@
 #include <nodelet/nodelet.h>
 #include <message_filters/time_synchronizer.h>
 #include <message_filters/subscriber.h>
+#include <std_msgs/UInt8.h>
 
 namespace nodelet
 {
-  /** \brief @b NodeletDEMUX represent a demux nodelet for topics: it takes 1 input topic, and publishes on N (<=8) output topics.
-    * \author Radu Bogdan Rusu
-    */
-  template <typename T, typename Subscriber = message_filters::Subscriber<T> >
-  class NodeletDEMUX: public Nodelet
+  template <typename T>
+  class NodeletBaseDEMUX: public Nodelet
   {
+    // TODO(lucasw) would ShapeShifter work as well?
     typedef typename boost::shared_ptr<const T> TConstPtr;
     public:
-      //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-      /** \brief Nodelet initialization routine. */
-      void
-        onInit ()
+      virtual void onInit() = 0;
+
+      virtual void finishInit ()
       {
-        private_nh_ = getPrivateNodeHandle ();
-        sub_input_.subscribe (private_nh_, "input", 1, bind (&NodeletDEMUX<T,Subscriber>::input_callback, this, _1));
+        // auto_index_ will evenly redistribute the input topic over the n output topics
+        auto_index_ = false;
+        private_nh_.getParam ("auto_index", auto_index_);
+        index_ = 0;
+        if (!auto_index_)
+        {
+          std_msgs::UInt8::Ptr maskp(new std_msgs::UInt8);
+          maskp->data = 0xff;
+          mask_ = maskp;
+          select_sub_ = private_nh_.subscribe ("select", 1,
+              &NodeletBaseDEMUX::select_callback, this);
+        }
 
         if (!private_nh_.getParam ("output_topics", output_topics_))
         {
@@ -88,7 +96,8 @@ namespace nodelet
 
             pubs_output_.resize (output_topics_.size ());
             for (int d = 0; d < output_topics_.size (); ++d)
-              *pubs_output_[d] = private_nh_.template advertise<T> ((std::string)(output_topics_[d]), 1);
+              pubs_output_[d] = boost::make_shared<ros::Publisher>(
+                  private_nh_.template advertise<T> ((std::string)(output_topics_[d]), 1));
             break;
           }
           default:
@@ -99,23 +108,43 @@ namespace nodelet
         }
       }
 
-    private:
-
-      //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-      void
+    protected:
+      virtual void
         input_callback (const TConstPtr &input)
       {
+        if (auto_index_ && (pubs_output_.size () > 0))
+        {
+          index_ %= pubs_output_.size ();
+          pubs_output_[index_]->publish (input);
+          index_++;
+          return;
+        }
+
         for (size_t d = 0; d < pubs_output_.size (); ++d)
-          pubs_output_[d]->publish (input);
+        {
+          if ((1 << d) & mask_->data)
+            pubs_output_[d]->publish (input);
+        }
       }
 
+      virtual void
+        select_callback (const std_msgs::UInt8::ConstPtr& msg)
+      {
+        mask_ = msg;
+      }
+
+      bool auto_index_;
+      unsigned int index_;
+      std_msgs::UInt8::ConstPtr mask_;
+
       /** \brief ROS local node handle. */
       ros::NodeHandle private_nh_;
       /** \brief The output list of publishers. */
       std::vector<boost::shared_ptr <ros::Publisher> > pubs_output_;
       /** \brief The input subscriber. */
-      Subscriber sub_input_;
-
+      ros::Subscriber sub_input_;
+      /** \brief The output topic select subscriber. */
+      ros::Subscriber select_sub_;
 
       /** \brief The list of output topics passed as a parameter. */
       XmlRpc::XmlRpcValue output_topics_;
@@ -124,79 +153,45 @@ namespace nodelet
   /** \brief @b NodeletDEMUX represent a demux nodelet for topics: it takes 1 input topic, and publishes on N (<=8) output topics.
     * \author Radu Bogdan Rusu
     */
-  template <typename T>
-  class NodeletDEMUX<T, message_filters::Subscriber<T> >: public Nodelet
+  template <typename T, typename Subscriber = message_filters::Subscriber<T> >
+  class NodeletDEMUX: public NodeletBaseDEMUX<T>
   {
     typedef typename boost::shared_ptr<const T> TConstPtr;
     public:
       //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
       /** \brief Nodelet initialization routine. */
-      void
+      virtual void
         onInit ()
       {
-        private_nh_ = getPrivateNodeHandle ();
-
-        sub_input_ = private_nh_.subscribe ("input", 1, &NodeletDEMUX<T>::input_callback, this);
-
-        if (!private_nh_.getParam ("output_topics", output_topics_))
-        {
-          ROS_ERROR ("[nodelet::NodeletDEMUX::init] Need a 'output_topics' parameter to be set before continuing!");
-          return;
-        }
-        // Check the type
-        switch (output_topics_.getType ())
-        {
-          case XmlRpc::XmlRpcValue::TypeArray:
-          {
-            if (output_topics_.size () == 1)
-            {
-              ROS_ERROR ("[nodelet::NodeletDEMUX::init] Only one topic given. Does it make sense to passthrough?");
-              return;
-            }
-
-            if (output_topics_.size () > 8)
-            {
-              ROS_ERROR ("[nodelet::NodeletDEMUX::init] More than 8 topics passed!");
-              return;
-             }
-
-            ROS_INFO_STREAM ("[nodelet::NodeletDEMUX::init] Publishing to " << output_topics_.size () << " user given topics as outputs:");
-            for (int d = 0; d < output_topics_.size (); ++d)
-              ROS_INFO_STREAM (" - " << (std::string)(output_topics_[d]));
-
-            pubs_output_.resize (output_topics_.size ());
-            for (int d = 0; d < output_topics_.size (); ++d)
-              *pubs_output_[d] = private_nh_.template advertise<T> ((std::string)(output_topics_[d]), 1);
-            break;
-          }
-          default:
-          {
-            ROS_ERROR ("[nodelet::NodeletDEMUX::init] Invalid 'output_topics' parameter given!");
-            return;
-          }
-        }
+        this->private_nh_ = this->getPrivateNodeHandle ();
+        this->sub_input_.subscribe (this->private_nh_, "input", 1, bind (&NodeletDEMUX<T,Subscriber>::input_callback, this, _1));
+        this->finishInit();
       }
+  };
 
-    private:
-
+  /** \brief @b NodeletDEMUX represent a demux nodelet for topics: it takes 1 input topic, and publishes on N (<=8) output topics.
+    * \author Radu Bogdan Rusu
+    */
+  template <typename T>
+  class NodeletDEMUX<T, message_filters::Subscriber<T> >: public NodeletBaseDEMUX<T>
+  {
+    typedef typename boost::shared_ptr<const T> TConstPtr;
+    public:
       //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-      void
-        input_callback (const TConstPtr &input)
+      /** \brief Nodelet initialization routine. */
+      virtual void
+        onInit ()
       {
-        for (size_t d = 0; d < pubs_output_.size (); ++d)
-          pubs_output_[d]->publish (input);
+        this->private_nh_ = this->getPrivateNodeHandle ();
+        this->sub_input_ = this->private_nh_.subscribe ("input", 1, &NodeletDEMUX::input_callback, this);
+        this->finishInit();
       }
 
-      /** \brief ROS local node handle. */
-      ros::NodeHandle private_nh_;
-      /** \brief The output list of publishers. */
-      std::vector<boost::shared_ptr <ros::Publisher> > pubs_output_;
-      /** \brief The input subscriber. */
-      ros::Subscriber sub_input_;
-
-
-      /** \brief The list of output topics passed as a parameter. */
-      XmlRpc::XmlRpcValue output_topics_;
+      virtual void
+        input_callback (const TConstPtr &input)
+      {
+        NodeletBaseDEMUX<T>::input_callback(input);
+      }
   };
 
 }

nodelet_topic_tools/package.xml

@@ -24,4 +24,5 @@
   <build_export_depend>nodelet</build_export_depend>
   <build_export_depend>pluginlib</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
 </package>

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()

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 <[email protected]>
+ */
+
+#include <nodelet_topic_tools/nodelet_demux.h>
+#include <pluginlib/class_list_macros.h>
+#include <std_msgs/String.h>
+
+typedef nodelet::NodeletDEMUX<std_msgs::String> NodeletDemuxString;
+PLUGINLIB_EXPORT_CLASS(NodeletDemuxString, nodelet::Nodelet);

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)

test_nodelet_topic_tools/test/test_nodelet_demux.launch

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<launch>
+
+  <node pkg="nodelet" name="nodelet_manager" type="nodelet" args="manager"/>
+
+  <param name="verbose_connection" value="true" />
+
+  <node name="string_nodelet_demux"
+        pkg="nodelet" type="nodelet"
+        args="load NodeletDemuxString nodelet_manager"
+        output="screen">
+    <remap from="~input"  to="input"/>
+    <param name="auto_index" value="true" />
+    <rosparam>
+      output_topics: [/input1, /input2]
+    </rosparam>
+  </node>
+
+  <test test-name="test_demux"
+        name="test_demux"
+        pkg="test_nodelet_topic_tools" type="test_demux.py"
+        retry="3">
+    <rosparam>
+      output_topics: [/input1, /input2]
+    </rosparam>
+  </test>
+
+</launch>

test_nodelet_topic_tools/test/test_nodelets.xml

@@ -9,4 +9,10 @@
       Lazy transport strings in nodelet (testing only).
     </description>
   </class>
+  <!-- TODO(lucas) not sure how to put this into test_nodelet_topic_tools -->
+  <class name="NodeletDemuxString" type="NodeletDemuxString" base_class_type="nodelet::Nodelet">
+    <description>
+      Nodelet demultiplexer (testing only).
+    </description>
+  </class>
 </library>