TOPIC TOOLS: add initial_topic param (#1199)

* TOPIC TOOLS: add initial_topic param

* TOPIC TOOLS: add tests for initial_topic param

tools/topic_tools/CMakeLists.txt

@@ -103,6 +103,8 @@ if(CATKIN_ENABLE_TESTING)
   add_rostest(test/relay.test)
   add_rostest(test/relay_stealth.test)
   add_rostest(test/lazy_transport.test)
+  add_rostest(test/mux_initial_none.test)
+  add_rostest(test/mux_initial_other.test)
   ## Latched test disabled until underlying issue in roscpp is resolved,
   ## #3385, #3434.
   #rosbuild_add_rostest(test/relay_latched.test)

tools/topic_tools/src/mux.cpp

@@ -317,9 +317,44 @@ int main(int argc, char **argv)
 
     g_subs.push_back(sub_info);
   }
-  g_selected = g_subs.begin(); // select first topic to start
+
+  // Set initial input topic from optional param, defaults to second argument
+  std::string initial_topic;
+  pnh.getParam("initial_topic", initial_topic);
   std_msgs::String t;
-  t.data = g_selected->topic_name;
+  if (initial_topic.empty()) // If param is not set, default to first in list
+  {
+    g_selected = g_subs.begin();
+    t.data = g_selected->topic_name;
+  }
+  else if (initial_topic == g_none_topic) // Set no initial input if param was __none
+  {
+    ROS_INFO("mux selected to no input.");
+    g_selected = g_subs.end();
+    t.data = g_none_topic;
+  }
+  else // Attempt to set initial topic if it is in the list
+  {
+    ROS_INFO("trying to switch mux to %s", initial_topic.c_str());
+    // 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();
+	 ++it)
+    {
+      if (ros::names::resolve(it->topic_name) == ros::names::resolve(initial_topic))
+      {
+        g_selected = it;
+        t.data = initial_topic;
+        ROS_INFO("mux selected input: [%s]", it->topic_name.c_str());
+        break;
+      }
+    }
+    if (t.data.empty()) // If it wasn't in the list, default to no input. Or should we crash here?
+    {
+      g_selected = g_subs.end();
+      t.data = g_none_topic;
+    }
+  }
   g_pub_selected.publish(t);
 
   // Backward compatibility

tools/topic_tools/test/mux_initial_none.test

@@ -0,0 +1,20 @@
+<launch>
+  <node pkg="rostopic" type="rostopic" name="rostopic_pub1"
+        args="pub -r 10 input1 std_msgs/String input1"/>
+  <node pkg="rostopic" type="rostopic" name="rostopic_pub2"
+        args="pub -r 5 input2 std_msgs/String input2"/>
+
+  <!-- Initial topic is __none -->
+  <node pkg="topic_tools" type="mux" name="mux_initial_none" output="screen"
+        args="output input1 input2">
+    <param name="initial_topic" value="__none" />
+  </node>
+
+  <!-- Test that mux initially has no input because initial_topic param is set to __none -->
+  <test test-name="mux_initial_topic_none_test" pkg="rostest" type="hztest">
+    <param name="topic" value="output"/>
+    <param name="hz" value="0"/>
+    <param name="hzerror" value="0"/>
+    <param name="test_duration" value="1.0" />
+  </test>
+</launch>

tools/topic_tools/test/mux_initial_other.test

@@ -0,0 +1,21 @@
+<launch>
+  <node pkg="rostopic" type="rostopic" name="rostopic_pub1"
+        args="pub -r 10 input1 std_msgs/String input1"/>
+  <node pkg="rostopic" type="rostopic" name="rostopic_pub2"
+        args="pub -r 5 input2 std_msgs/String input2"/>
+
+  <!-- Initial topic non default -->
+  <node pkg="topic_tools" type="mux" name="mux_explicit" output="screen"
+        args="output input1 input2">
+    <param name="initial_topic" value="input2" />
+  </node>
+
+  <!-- Test that initialy mux chooses input2 as specified in param -->
+  <test test-name="mux_hztest_explicit" pkg="rostest" type="hztest">
+    <param name="topic" value="output"/>
+    <param name="hz" value="5.0"/>
+    <param name="hzerror" value="0.5"/>
+    <param name="test_duration" value="1.0" />
+  </test>
+
+</launch>