Add remove/unbridge services (remove_subscribe, remove_advertise)

udp_bridge/CMakeLists.txt

@@ -146,6 +146,9 @@ if(BUILD_TESTING)
   # test_publish_queue (issue #10) exercises the bounded publish-worker
   # queue in isolation via an injected sink — no UDPBridge / node needed.
   add_udp_bridge_gtest(test_publish_queue test/test_publish_queue.cpp)
+  # test_subscriber_registry (issue #30) exercises the remove-forwarding map
+  # logic (remove_subscribe / remove_advertise) on a plain map — no node needed.
+  add_udp_bridge_gtest(test_subscriber_registry test/test_subscriber_registry.cpp)
 endif()
 
 ament_package()

udp_bridge/include/udp_bridge/subscriber_registry.h

@@ -0,0 +1,61 @@
+#ifndef UDP_BRIDGE_SUBSCRIBER_REGISTRY_H
+#define UDP_BRIDGE_SUBSCRIBER_REGISTRY_H
+
+#include <map>
+#include <string>
+#include <utility>
+
+#include "rclcpp/generic_subscription.hpp"
+#include "udp_bridge/types.h"
+
+namespace udp_bridge
+{
+
+// Result of removeSubscriberConnectionFrom. `changed` is true when something was
+// actually removed (so the caller republishes bridge info). `expired_subscription`
+// holds the local generic subscription moved out of the registry when a source
+// topic lost its last remote — the caller resets it OFF the registry lock as
+// hygiene (subscription teardown does rmw work better kept off the hot mutex; an
+// in-flight forwarding callback keeps its own strong ref via the executor, so
+// the reset is safe regardless). Pure data + map logic, no node / clock / mutex
+// here.
+struct SubscriberRemoval
+{
+  bool changed = false;
+  rclcpp::GenericSubscription::SharedPtr expired_subscription;
+};
+
+// Remove the (remote_node, connection_id) forwarding for source_topic from a
+// subscriber registry. Drops the remote when it has no connections left, and the
+// whole source entry — moving out its subscription for off-lock teardown — when
+// it has no remotes left. Removing something absent is a no-op (changed=false),
+// so repeated calls are idempotent.
+inline SubscriberRemoval removeSubscriberConnectionFrom(
+  std::map<std::string, SubscriberDetails>& subscribers,
+  const std::string& source_topic,
+  const std::string& remote_node,
+  const std::string& connection_id)
+{
+  SubscriberRemoval result;
+  auto sub_it = subscribers.find(source_topic);
+  if(sub_it == subscribers.end())
+    return result;
+  auto rd_it = sub_it->second.remote_details.find(remote_node);
+  if(rd_it == sub_it->second.remote_details.end())
+    return result;
+  if(rd_it->second.connection_rates.erase(connection_id) == 0)
+    return result;
+  result.changed = true;
+  if(rd_it->second.connection_rates.empty())
+    sub_it->second.remote_details.erase(rd_it);
+  if(sub_it->second.remote_details.empty())
+  {
+    result.expired_subscription = std::move(sub_it->second.subscription);
+    subscribers.erase(sub_it);
+  }
+  return result;
+}
+
+}  // namespace udp_bridge
+
+#endif

udp_bridge/include/udp_bridge/udp_bridge.h

@@ -152,6 +152,18 @@ class UDPBridge: public rclcpp_lifecycle::LifecycleNode
     const std::shared_ptr<udp_bridge_interfaces::srv::Subscribe::Request> request,
     std::shared_ptr<udp_bridge_interfaces::srv::Subscribe::Response> response);
 
+  /// Service handler for local request to cancel a remote subscription: tells
+  /// the remote to stop pushing source_topic to us (mirror of remoteSubscribe).
+  void removeSubscribe(
+    const std::shared_ptr<udp_bridge_interfaces::srv::Subscribe::Request> request,
+    std::shared_ptr<udp_bridge_interfaces::srv::Subscribe::Response> response);
+
+  /// Service handler to stop advertising a local topic to a remote: removes the
+  /// local source->remote forwarding (mirror of remoteAdvertise).
+  void removeAdvertise(
+    const std::shared_ptr<udp_bridge_interfaces::srv::Subscribe::Request> request,
+    std::shared_ptr<udp_bridge_interfaces::srv::Subscribe::Response> response);
+
   /// Service handler to add a named remote
   void addRemote(
     const std::shared_ptr<udp_bridge_interfaces::srv::AddRemote::Request> request,
@@ -268,8 +280,20 @@ class UDPBridge: public rclcpp_lifecycle::LifecycleNode
                                std::string durability = "",
                                uint32_t history_depth = 0);
 
+  /// @brief Remove a source->remote forwarding added by addSubscriberConnection.
+  ///
+  /// Drops the (connection_id) rate entry for (source_topic, remote_node); when
+  /// that leaves the remote with no connections it is removed, and when the
+  /// source topic has no remaining remotes its local generic subscription is
+  /// torn down. Removing a non-existent forwarding is a no-op (idempotent). The
+  /// subscription is destroyed off the subscribers_ lock so it cannot race the
+  /// forwarding callback.
+  void removeSubscriberConnection(std::string const &source_topic,
+                                  std::string const &remote_node,
+                                  std::string const &connection_id);
+
   /// @brief Checks configured local topics and attempts to subscribe
-  void updateLocalSubscriptions();  
+  void updateLocalSubscriptions();
 
 
 
@@ -291,6 +315,8 @@ class UDPBridge: public rclcpp_lifecycle::LifecycleNode
 
   rclcpp::Service<udp_bridge_interfaces::srv::Subscribe>::SharedPtr subscribe_service_;
   rclcpp::Service<udp_bridge_interfaces::srv::Subscribe>::SharedPtr advertise_service_;
+  rclcpp::Service<udp_bridge_interfaces::srv::Subscribe>::SharedPtr remove_subscribe_service_;
+  rclcpp::Service<udp_bridge_interfaces::srv::Subscribe>::SharedPtr remove_advertise_service_;
   rclcpp::Service<udp_bridge_interfaces::srv::AddRemote>::SharedPtr add_remote_service_;
   rclcpp::Service<udp_bridge_interfaces::srv::ListRemotes>::SharedPtr list_remotes_service_;
 

udp_bridge/src/udp_bridge.cpp

@@ -60,6 +60,7 @@
 #include "udp_bridge/remote_node.h"
 #include "udp_bridge/resend_constants.h"
 #include "udp_bridge/giveup_diagnostic.h"
+#include "udp_bridge/subscriber_registry.h"
 #include "udp_bridge/types.h"
 #include "udp_bridge/utilities.h"
 #include "lifecycle_msgs/msg/state.hpp"
@@ -326,6 +327,14 @@ UDPBridge::CallbackReturn UDPBridge::on_configure(const rclcpp_lifecycle::State
     node_name+"/remote_advertise",
     std::bind(&UDPBridge::remoteAdvertise, this, _1, _2),
     service_qos, periodic_group_);
+  remove_subscribe_service_ = create_service<Subscribe>(
+    node_name+"/remove_subscribe",
+    std::bind(&UDPBridge::removeSubscribe, this, _1, _2),
+    service_qos, periodic_group_);
+  remove_advertise_service_ = create_service<Subscribe>(
+    node_name+"/remove_advertise",
+    std::bind(&UDPBridge::removeAdvertise, this, _1, _2),
+    service_qos, periodic_group_);
 
   add_remote_service_ = create_service<AddRemote>(
     node_name+"/add_remote",
@@ -635,7 +644,14 @@ void UDPBridge::callback(std::string topic_name, std::string topic_type, std::sh
   std::map<std::string, DestinationConfig> destination_config_by_remote;
   {
     std::lock_guard<std::mutex> lock(subscribers_mutex_);
-    auto& sub = subscribers_[topic_name];
+    // Use find(), not operator[]: a message can still arrive on a subscription
+    // that removeSubscriberConnection is tearing down. operator[] would
+    // re-insert an empty entry, which updateLocalSubscriptions would then
+    // resurrect as a zombie subscription forwarding to nobody.
+    auto sub_it = subscribers_.find(topic_name);
+    if(sub_it == subscribers_.end())
+      return;
+    auto& sub = sub_it->second;
     for(auto& remote_details: sub.remote_details)
     {
       std::unordered_set<float> periods; // group the sending to connections with same period
@@ -706,7 +722,9 @@ void UDPBridge::callback(std::string topic_name, std::string topic_type, std::sh
     size_data.message_size = message->size();
     {
       std::lock_guard<std::mutex> lock(subscribers_mutex_);
-      subscribers_[topic_name].statistics.add(size_data);
+      auto sub_it = subscribers_.find(topic_name);
+      if(sub_it != subscribers_.end())
+        sub_it->second.statistics.add(size_data);
     }
   }
 }
@@ -1032,6 +1050,27 @@ void UDPBridge::addSubscriberConnection(std::string const &source_topic,
   }
 }
 
+void UDPBridge::removeSubscriberConnection(std::string const &source_topic,
+                                           std::string const &remote_node,
+                                           std::string const &connection_id)
+{
+  // The map manipulation (extracted to removeSubscriberConnectionFrom so it can
+  // be unit-tested) runs under subscribers_mutex_. The local generic
+  // subscription it hands back is reset OFF the lock as hygiene — destroying a
+  // subscription does rmw teardown we don't want to run under the hot
+  // subscribers_mutex_. It is safe either way: an in-flight forwarding callback
+  // holds its own strong ref to the subscription via the executor, so reset()
+  // never frees one mid-callback and never blocks on it.
+  SubscriberRemoval removal;
+  {
+    std::lock_guard<std::mutex> lock(subscribers_mutex_);
+    removal = removeSubscriberConnectionFrom(subscribers_, source_topic, remote_node, connection_id);
+  }
+  removal.expired_subscription.reset();  // destroys the generic subscription, off-lock
+  if(removal.changed)
+    sendBridgeInfo();
+}
+
 void UDPBridge::updateLocalSubscriptions()
 {
   // Three-phase lookup so get_publishers_info_by_topic() and
@@ -1099,6 +1138,11 @@ void UDPBridge::decodeSubscribeRequest(std::vector<uint8_t> const &message, cons
 {
   auto remote_request = deserialize<RemoteSubscribeInternal>(message);
 
+  if(remote_request.operation == RemoteSubscribeInternal::OPERATION_UNSUBSCRIBE)
+  {
+    removeSubscriberConnection(remote_request.source_topic, source_info.node_name, remote_request.connection_id);
+    return;
+  }
   addSubscriberConnection(remote_request.source_topic, remote_request.destination_topic, remote_request.queue_size, remote_request.period, source_info.node_name, remote_request.connection_id);
 }
 
@@ -1452,6 +1496,36 @@ void UDPBridge::remoteAdvertise(
   addSubscriberConnection(request->source_topic, request->destination_topic, request->queue_size, request->period, request->remote, request->connection_id);
 }
 
+void UDPBridge::removeSubscribe(
+  const std::shared_ptr<udp_bridge::Subscribe::Request> request,
+  std::shared_ptr<udp_bridge::Subscribe::Response> response)
+{
+  (void)response;
+  RCLCPP_INFO_STREAM(get_logger(), "remove subscribe: remote: " << request->remote << " connection: " << request->connection_id << " source topic: " << request->source_topic);
+
+  // Mirror of remoteSubscribe: tell the remote to stop pushing source_topic to
+  // us. The remote's decodeSubscribeRequest sees OPERATION_UNSUBSCRIBE and calls
+  // removeSubscriberConnection on its side.
+  udp_bridge::RemoteSubscribeInternal remote_request;
+  remote_request.source_topic = request->source_topic;
+  remote_request.connection_id = request->connection_id;
+  remote_request.operation = udp_bridge::RemoteSubscribeInternal::OPERATION_UNSUBSCRIBE;
+
+  send(remote_request, request->remote, true);
+}
+
+void UDPBridge::removeAdvertise(
+  const std::shared_ptr<udp_bridge::Subscribe::Request> request,
+  std::shared_ptr<udp_bridge::Subscribe::Response> response)
+{
+  (void)response;
+  RCLCPP_INFO_STREAM(get_logger(), "remove advertise: remote: " << request->remote << " connection: " << request->connection_id << " source topic: " << request->source_topic);
+
+  // Mirror of remoteAdvertise: stop forwarding our local source_topic to the
+  // remote. Purely local; idempotent if the forwarding is already gone.
+  removeSubscriberConnection(request->source_topic, request->remote, request->connection_id);
+}
+
 void UDPBridge::statsReportCallback()
 {
   if(get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)

udp_bridge/test/test_subscriber_registry.cpp

@@ -0,0 +1,107 @@
+// Unit tests for removeSubscriberConnectionFrom — the registry map logic behind
+// the remove_subscribe / remove_advertise services (issue #30). Pure map logic,
+// so it runs without bringing up a node: entries carry a null subscription and
+// we assert the connection/remote/source erase cascade + idempotency.
+
+#include <map>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#include "udp_bridge/subscriber_registry.h"
+#include "udp_bridge/types.h"
+
+namespace
+{
+
+using udp_bridge::removeSubscriberConnectionFrom;
+using udp_bridge::SubscriberDetails;
+
+// Add a (source -> remote -> connection) forwarding entry (null subscription).
+void addConn(std::map<std::string, SubscriberDetails>& subs,
+             const std::string& source, const std::string& remote,
+             const std::string& conn)
+{
+  subs[source].remote_details[remote].connection_rates[conn].period = 0.0f;
+}
+
+TEST(SubscriberRegistry, RemoveLastConnectionErasesSource)
+{
+  std::map<std::string, SubscriberDetails> subs;
+  addConn(subs, "/state", "boat", "default");
+
+  auto r = removeSubscriberConnectionFrom(subs, "/state", "boat", "default");
+  EXPECT_TRUE(r.changed);
+  EXPECT_EQ(subs.count("/state"), 0u);  // last remote gone -> source entry erased
+}
+
+TEST(SubscriberRegistry, RemoveOneConnectionKeepsRemote)
+{
+  std::map<std::string, SubscriberDetails> subs;
+  addConn(subs, "/state", "boat", "wifi");
+  addConn(subs, "/state", "boat", "vpn");
+
+  auto r = removeSubscriberConnectionFrom(subs, "/state", "boat", "wifi");
+  EXPECT_TRUE(r.changed);
+  ASSERT_EQ(subs.count("/state"), 1u);
+  auto& rd = subs["/state"].remote_details.at("boat");
+  EXPECT_EQ(rd.connection_rates.count("wifi"), 0u);
+  EXPECT_EQ(rd.connection_rates.count("vpn"), 1u);
+}
+
+TEST(SubscriberRegistry, RemoveOneRemoteKeepsOthers)
+{
+  std::map<std::string, SubscriberDetails> subs;
+  addConn(subs, "/state", "boatA", "default");
+  addConn(subs, "/state", "boatB", "default");
+
+  auto r = removeSubscriberConnectionFrom(subs, "/state", "boatA", "default");
+  EXPECT_TRUE(r.changed);
+  ASSERT_EQ(subs.count("/state"), 1u);
+  EXPECT_EQ(subs["/state"].remote_details.count("boatA"), 0u);
+  EXPECT_EQ(subs["/state"].remote_details.count("boatB"), 1u);
+}
+
+TEST(SubscriberRegistry, RemoveAbsentSourceIsNoop)
+{
+  std::map<std::string, SubscriberDetails> subs;
+  addConn(subs, "/state", "boat", "default");
+
+  auto r = removeSubscriberConnectionFrom(subs, "/other", "boat", "default");
+  EXPECT_FALSE(r.changed);
+  EXPECT_EQ(subs.count("/state"), 1u);
+}
+
+TEST(SubscriberRegistry, RemoveAbsentRemoteIsNoop)
+{
+  std::map<std::string, SubscriberDetails> subs;
+  addConn(subs, "/state", "boat", "default");
+
+  auto r = removeSubscriberConnectionFrom(subs, "/state", "ghost", "default");
+  EXPECT_FALSE(r.changed);
+  EXPECT_EQ(subs["/state"].remote_details.count("boat"), 1u);
+}
+
+TEST(SubscriberRegistry, RemoveAbsentConnectionIsNoop)
+{
+  std::map<std::string, SubscriberDetails> subs;
+  addConn(subs, "/state", "boat", "default");
+
+  auto r = removeSubscriberConnectionFrom(subs, "/state", "boat", "ghost");
+  EXPECT_FALSE(r.changed);
+  EXPECT_EQ(subs["/state"].remote_details.at("boat").connection_rates.count("default"), 1u);
+}
+
+TEST(SubscriberRegistry, SecondRemoveIsIdempotentNoop)
+{
+  std::map<std::string, SubscriberDetails> subs;
+  addConn(subs, "/state", "boat", "default");
+
+  auto first = removeSubscriberConnectionFrom(subs, "/state", "boat", "default");
+  EXPECT_TRUE(first.changed);
+  auto second = removeSubscriberConnectionFrom(subs, "/state", "boat", "default");
+  EXPECT_FALSE(second.changed);
+  EXPECT_EQ(subs.count("/state"), 0u);
+}
+
+}  // namespace

udp_bridge_interfaces/msg/RemoteSubscribeInternal.msg

@@ -1,6 +1,19 @@
-# Internal message used to request a topic from a remote over a specific connection.
+# Internal message used to request (or cancel) a topic from a remote over a
+# specific connection.
 string  source_topic
 string  destination_topic
 uint32  queue_size
 float32 period
-string  connection_id
+string  connection_id
+
+# Operation: subscribe (set up the remote's push to us) or unsubscribe (tear it
+# down). Same-version peers default to 0 (subscribe), so the common path is
+# unchanged. NOTE: appending this field is a wire-format change, not a
+# backward-compatible one. A new receiver deserializing a pre-operation (old)
+# subscribe packet throws on the missing trailing byte; udp_bridge's decode()
+# catch-all logs and drops it (recoverable, not a crash and not silently
+# misread). Deploy the bridge fleet together rather than mixing versions across
+# a link.
+uint8 OPERATION_SUBSCRIBE   = 0
+uint8 OPERATION_UNSUBSCRIBE = 1
+uint8 operation