Select input topic provides a mask to pass to any combination of outp…

…ut topics, defaults to all of them ros#79
lucasw · Jul 30, 2018 · f66a0b5 · f66a0b5
1 parent 25024a4
commit f66a0b5
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 3 deletions.
diff --git a/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_demux.h b/nodelet_topic_tools/include/nodelet_topic_tools/nodelet_demux.h
@@ -41,22 +41,32 @@
 #include <nodelet/nodelet.h>
 #include <message_filters/time_synchronizer.h>
 #include <message_filters/subscriber.h>
+#include <std_msgs/UInt8.h>
 
 namespace nodelet
 {
   template <typename T>
   class NodeletBaseDEMUX: public Nodelet
   {
+    // TODO(lucasw) would ShapeShifter work as well?
     typedef typename boost::shared_ptr<const T> TConstPtr;
     public:
       virtual void onInit() = 0;
 
       virtual void finishInit ()
       {
-        // this will evenly redistribute the input topic over the n output topics
+        // 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_))
         {
@@ -111,18 +121,30 @@ namespace nodelet
         }
 
         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. */
       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_;
@@ -161,7 +183,6 @@ namespace nodelet
         onInit ()
       {
         this->private_nh_ = this->getPrivateNodeHandle ();
-        // this->sub_input_ = this->private_nh_.subscribe ("input", 1, &NodeletBaseDEMUX<T>::input_callback, this);
         this->sub_input_ = this->private_nh_.subscribe ("input", 1, &NodeletDEMUX::input_callback, this);
         this->finishInit();
       }

diff --git a/nodelet_topic_tools/package.xml b/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>