Fixing reconnect bug by reinitializing the reception queue.

src/network_interfaces/tcp_interface.cpp

@@ -59,6 +59,7 @@ void TcpInterface::open()
   shutting_down_ = false;
   failed_ = false;
   ready_ = false;
+  stream_.reset();
   io_context_.restart();
   if (role_ == "server") {
     setup_server();

src/subscription_manager.cpp

@@ -55,7 +55,7 @@ void SubscriptionManager::setup_subscription()
   std::string topic = subscribe_namespace_ + topic_;
 
   if (all_topics_and_types.find(topic) == all_topics_and_types.end()) {
-    if (topic_found_) {
+    if (topic_found_) { // If we thought is was found but we cannot find it.
       RCLCPP_WARN(node_->get_logger(), "Topic %s not found", topic.c_str());
     }
     topic_found_ = false;
@@ -113,6 +113,9 @@ void SubscriptionManager::create_subscription(
       const std::shared_ptr<const rclcpp::SerializedMessage> & serialized_msg) {
       this->callback(serialized_msg);
     });
+  RCLCPP_INFO(
+    node_->get_logger(),
+    "Created generic subscriber for topic %s", topic.c_str());
 }
 
 void SubscriptionManager::callback(

src/subscription_manager_tf.cpp

@@ -60,6 +60,9 @@ void SubscriptionManagerTF::create_subscription(
         const std::shared_ptr<const tf2_msgs::msg::TFMessage> & tfmsg) {
         this->tf2_callback(tfmsg);
       });
+    RCLCPP_INFO(
+      node_->get_logger(),
+      "Created static TF subscriber for topic %s", topic.c_str());
   } else {
     tf2_ros::DynamicListenerQoS dynamic_qos;
     tf2_subscriber_ = node_->create_subscription<tf2_msgs::msg::TFMessage>(
@@ -68,6 +71,9 @@ void SubscriptionManagerTF::create_subscription(
         const std::shared_ptr<const tf2_msgs::msg::TFMessage> & tfmsg) {
         this->tf2_callback(tfmsg);
       });
+    RCLCPP_INFO(
+      node_->get_logger(),
+      "Created generic TF subscriber for topic %s", topic.c_str());
   }
 }
 
@@ -97,9 +103,7 @@ void SubscriptionManagerTF::tf2_callback(
   const std::shared_ptr<const tf2_msgs::msg::TFMessage> & tfmsg)
 {
   bool new_tf = false;
-  size_t i = 0;
-  // Not using a for loop to allow a clean reset.
-  while (i < tfmsg->transforms.size()) {
+  for (size_t i = 0; i < tfmsg->transforms.size(); i++) {
     const geometry_msgs::msg::TransformStamped t = tfmsg->transforms[i];
     if (!exclude_pattern.empty()) {
       bool matched = false;
@@ -116,7 +120,6 @@ void SubscriptionManagerTF::tf2_callback(
       }
       if (matched) {
         // Ignore this transform, it's in the exclude list
-        i++;
         continue;
       }
     }
@@ -135,7 +138,6 @@ void SubscriptionManagerTF::tf2_callback(
       }
       if (!matched) {
         // Ignore this transform, it's not in the matched list
-        i++;
         continue;
       }
     }
@@ -162,7 +164,11 @@ void SubscriptionManagerTF::tf2_callback(
       // Known TF
       tfs_.transforms[it->second] = t;
     }
-    i++;
+  }
+  if (static_tf_) {
+    for (size_t i = 0; i < tfs_.transforms.size(); i++) {
+      tfs_.transforms[i].header.stamp = rclcpp::Time();
+    }
   }
   if (new_tf) {
     RCLCPP_INFO(