Fix unsubscribing when switching to none topic (#111) (#118)

Signed-off-by: Adam Morrissett <[email protected]> (cherry picked from commit ba45cd2) Co-authored-by: Adam Morrissett <[email protected]>
ros-tooling · Oct 14, 2024 · 69c32de · 69c32de
1 parent 342b33c
commit 69c32de
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 1 deletion.
diff --git a/topic_tools/src/mux_node.cpp b/topic_tools/src/mux_node.cpp
@@ -166,6 +166,9 @@ void MuxNode::on_mux_select(
   if (request->topic == NONE_TOPIC) {
     RCLCPP_INFO(get_logger(), "mux selected to no input.");
     input_topic_ = NONE_TOPIC;
+    // Calling the base class's function directly here because NONE_TOPIC is assumed to have no
+    // publishers. Continuously polling through the derived function is unnecessary effort.
+    ToolBaseNode::make_subscribe_unsubscribe_decisions();
     response->success = true;
   } else {
     RCLCPP_INFO(get_logger(), "trying to switch mux to %s", request->topic.c_str());

diff --git a/topic_tools/test/test_mux.cpp b/topic_tools/test/test_mux.cpp
@@ -49,7 +49,11 @@ class MuxTest : public TestTopicToolMultiSub
     using namespace std::chrono_literals;
 
     auto request = std::make_shared<topic_tools_interfaces::srv::MuxSelect::Request>();
-    request->topic = get_target_input_topics()[topic_index];
+    if (topic_index < 0) {
+      request->topic = "__none";
+    } else {
+      request->topic = get_target_input_topics()[topic_index];
+    }
 
     while (!srv_client_->wait_for_service(1s)) {
       if (!rclcpp::ok()) {
@@ -98,3 +102,21 @@ TEST_F(MuxTest, SwitchingTopicsWorks) {
 
   ASSERT_EQ(get_received_msgs(), expect_to_receive);
 }
+
+TEST_F(MuxTest, SwitchingToNoneWorks) {
+  int expect_to_receive = 0;
+
+  publish_and_check("not dropped", 0);
+  expect_to_receive++;
+
+  change_topic(1);
+  publish_and_check("dropped", 0);
+  publish_and_check("not dropped", 1);
+  expect_to_receive++;
+
+  change_topic(-1);
+  publish_and_check("dropped", 0);
+  publish_and_check("not dropped", 1);
+
+  ASSERT_EQ(get_received_msgs(), expect_to_receive);
+}