From 94bb020d19e786ae060cf4f098a56f466fafa858 Mon Sep 17 00:00:00 2001 From: Claude Code Agent Date: Sun, 14 Jun 2026 23:14:28 -0400 Subject: [PATCH] Add remove/unbridge services: remove_subscribe, remove_advertise (#30) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit udp_bridge could add a runtime bridge (remote_subscribe / remote_advertise) but not remove one. Disconnecting required tearing down the node or silencing a topic with a negative rate period (left the forwarding in place). This adds real teardown, needed for marine_control's dynamic device control (ADR-0003 D7-dyn). - RemoteSubscribeInternal.msg: add OPERATION_SUBSCRIBE=0 / OPERATION_UNSUBSCRIBE=1 + uint8 operation (mirrors the ConnectionInternal.operation idiom). - removeSubscriberConnectionFrom (new testable header subscriber_registry.h): pure map logic — drop a (remote, connection) forwarding, drop the remote when it has no connections, drop the source entry (handing back its subscription for off-lock teardown) when it has no remotes. Idempotent. - removeSubscriberConnection: runs the map logic under subscribers_mutex_, resets the freed subscription off-lock (hygiene; an in-flight forwarding callback keeps its own executor ref so the reset is always safe), then sendBridgeInfo. - ~/remove_advertise: local removal of a local->remote push (mirror of remote_advertise). ~/remove_subscribe: sends OPERATION_UNSUBSCRIBE so the remote drops its push to us (mirror of remote_subscribe); decodeSubscribeRequest branches on operation. - callback(): use subscribers_.find() instead of operator[] (both the forward and the stats paths) so an in-flight message during teardown can't re-insert an empty entry that updateLocalSubscriptions would resurrect as a zombie sub. - 7 unit tests for the removal cascade + idempotency. 102 tests pass. Reviewed by two Deep adversarial passes (protocol/logic + concurrency): logic, key symmetry, direction, erase cascade, and teardown safety confirmed sound. Wire-format note: appending `operation` is not backward-compatible across versions; a new receiver throws on an old-format subscribe and decode()'s existing catch-all logs+drops it (recoverable). Deploy the fleet together. Closes #30 Co-Authored-By: Claude Fable 5 --- udp_bridge/CMakeLists.txt | 3 + .../include/udp_bridge/subscriber_registry.h | 61 ++++++++++ udp_bridge/include/udp_bridge/udp_bridge.h | 28 ++++- udp_bridge/src/udp_bridge.cpp | 78 ++++++++++++- udp_bridge/test/test_subscriber_registry.cpp | 107 ++++++++++++++++++ .../msg/RemoteSubscribeInternal.msg | 17 ++- 6 files changed, 289 insertions(+), 5 deletions(-) create mode 100644 udp_bridge/include/udp_bridge/subscriber_registry.h create mode 100644 udp_bridge/test/test_subscriber_registry.cpp diff --git a/udp_bridge/CMakeLists.txt b/udp_bridge/CMakeLists.txt index d17f346..e6bf388 100644 --- a/udp_bridge/CMakeLists.txt +++ b/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() diff --git a/udp_bridge/include/udp_bridge/subscriber_registry.h b/udp_bridge/include/udp_bridge/subscriber_registry.h new file mode 100644 index 0000000..22cd4e4 --- /dev/null +++ b/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 +#include +#include + +#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& 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 diff --git a/udp_bridge/include/udp_bridge/udp_bridge.h b/udp_bridge/include/udp_bridge/udp_bridge.h index e01b64c..fb448f7 100644 --- a/udp_bridge/include/udp_bridge/udp_bridge.h +++ b/udp_bridge/include/udp_bridge/udp_bridge.h @@ -152,6 +152,18 @@ class UDPBridge: public rclcpp_lifecycle::LifecycleNode const std::shared_ptr request, std::shared_ptr 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 request, + std::shared_ptr 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 request, + std::shared_ptr response); + /// Service handler to add a named remote void addRemote( const std::shared_ptr 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::SharedPtr subscribe_service_; rclcpp::Service::SharedPtr advertise_service_; + rclcpp::Service::SharedPtr remove_subscribe_service_; + rclcpp::Service::SharedPtr remove_advertise_service_; rclcpp::Service::SharedPtr add_remote_service_; rclcpp::Service::SharedPtr list_remotes_service_; diff --git a/udp_bridge/src/udp_bridge.cpp b/udp_bridge/src/udp_bridge.cpp index 1e5f772..a5361b4 100644 --- a/udp_bridge/src/udp_bridge.cpp +++ b/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( + node_name+"/remove_subscribe", + std::bind(&UDPBridge::removeSubscribe, this, _1, _2), + service_qos, periodic_group_); + remove_advertise_service_ = create_service( + node_name+"/remove_advertise", + std::bind(&UDPBridge::removeAdvertise, this, _1, _2), + service_qos, periodic_group_); add_remote_service_ = create_service( node_name+"/add_remote", @@ -635,7 +644,14 @@ void UDPBridge::callback(std::string topic_name, std::string topic_type, std::sh std::map destination_config_by_remote; { std::lock_guard 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 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 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 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 const &message, cons { auto remote_request = deserialize(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 request, + std::shared_ptr 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 request, + std::shared_ptr 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) diff --git a/udp_bridge/test/test_subscriber_registry.cpp b/udp_bridge/test/test_subscriber_registry.cpp new file mode 100644 index 0000000..20dd0a2 --- /dev/null +++ b/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 +#include + +#include + +#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& 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 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 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 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 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 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 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 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 diff --git a/udp_bridge_interfaces/msg/RemoteSubscribeInternal.msg b/udp_bridge_interfaces/msg/RemoteSubscribeInternal.msg index e5a127a..fbe3ebe 100644 --- a/udp_bridge_interfaces/msg/RemoteSubscribeInternal.msg +++ b/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 \ No newline at end of file +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