diff --git a/udp_bridge/CMakeLists.txt b/udp_bridge/CMakeLists.txt index e6bf388..113b172 100644 --- a/udp_bridge/CMakeLists.txt +++ b/udp_bridge/CMakeLists.txt @@ -149,6 +149,11 @@ if(BUILD_TESTING) # 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) + # test_stale_packet_gate exercises RemoteNode::admitForPublish — the + # per-destination-topic high-water gate added on the 2026-06-29 deployment, + # including the remote-restart reset that keeps post-restart packet #0 from + # being dropped as stale. + add_udp_bridge_gtest(test_stale_packet_gate test/test_stale_packet_gate.cpp) endif() ament_package() diff --git a/udp_bridge/include/udp_bridge/remote_node.h b/udp_bridge/include/udp_bridge/remote_node.h index 4def112..103a715 100644 --- a/udp_bridge/include/udp_bridge/remote_node.h +++ b/udp_bridge/include/udp_bridge/remote_node.h @@ -166,6 +166,21 @@ class RemoteNode // re-acquire remote_nodes_mutex_. uint32_t resendGiveupCount() const; + // Stale-packet gate (drop_stale_packets). Returns true if a message + // for `topic` carrying wrapped sequence number `packet_number` should + // be published, false if it is stale — i.e. a strictly-newer message + // for the same destination topic has already been admitted. The first + // message seen for a topic is always admitted. On admit, the per-topic + // high-water mark is advanced to `packet_number`. + // + // packet_number is the sender's per-remote-node monotonic sequence, so + // the high-water marks share this RemoteNode's number space and are + // cleared together with received_packet_times_ on remote-restart + // detection in update(BridgeInfo) — otherwise every post-restart packet + // (numbers reset to 0) would be wrongly dropped as stale. Takes + // state_mutex_. + bool admitForPublish(const std::string& topic, uint64_t packet_number); + private: // Single on-arrival point: record the receive time AND clear any // pending resend tracking for that packet. Both `unwrap()` (the @@ -253,6 +268,17 @@ class RemoteNode std::map received_packet_times_; + // Per-destination-topic high-water mark of the wrapped packet_number + // last admitted for publication (the stale-packet gate; see + // admitForPublish). Keyed on destination topic. A decoded message + // whose packet_number is strictly below its topic's mark is a + // late-arriving resend that a newer message superseded, so it is + // dropped instead of republished out of order. Guarded by state_mutex_. + // Cleared on remote-restart detection in update(BridgeInfo) alongside + // received_packet_times_ — the sender's number space resets to 0 on + // restart, so a stale mark would otherwise reject every fresh packet. + std::map highest_published_packet_number_; + // Per-missing-packet state used by getMissingPackets to apply // exponential backoff and the TTL-bounded give-up condition (issue // #9 failure modes B and the give-up gap). Entries are created when diff --git a/udp_bridge/include/udp_bridge/types.h b/udp_bridge/include/udp_bridge/types.h index b5e4897..8a4e408 100644 --- a/udp_bridge/include/udp_bridge/types.h +++ b/udp_bridge/include/udp_bridge/types.h @@ -53,6 +53,17 @@ struct SourceInfo std::string node_name; std::string host; uint16_t port = 0; + + // Sequencing metadata, populated by UDPBridge::unwrap from the + // SequencedPacketHeader before the inner packet is recursively + // decoded. `sequenced` is false for any packet that did not pass + // through the wrapped-packet layer (so the stale-packet gate in + // decodeData leaves un-sequenced packets untouched). `packet_number` + // is the sender's per-remote-node monotonic sequence number, used to + // drop late-arriving resends that a newer message has already + // superseded on the same destination topic. + bool sequenced = false; + uint64_t packet_number = 0; }; } // namespace udp_bridge diff --git a/udp_bridge/include/udp_bridge/udp_bridge.h b/udp_bridge/include/udp_bridge/udp_bridge.h index fb448f7..7e348b8 100644 --- a/udp_bridge/include/udp_bridge/udp_bridge.h +++ b/udp_bridge/include/udp_bridge/udp_bridge.h @@ -415,6 +415,20 @@ class UDPBridge: public rclcpp_lifecycle::LifecycleNode // cast in on_configure well within range on 32-bit size_t platforms. static constexpr size_t kMaxPublishQueueMaxBytes = 2ull * 1024u * 1024u * 1024u; + // Stale-packet gate (drop_stale_packets parameter, default true). + // When set, decodeData drops a decoded message whose wrapped + // packet_number is older than the newest already published for its + // destination topic — a late-arriving resend the resend protocol + // delivered out of order. Declared in on_configure (declareIfMissing) + // so a deployment can disable it without a rebuild for topics that + // need every message regardless of order. + bool drop_stale_packets_ {true}; + + // Cumulative count of messages dropped by the stale-packet gate. + // Touched only on the socket-drain thread (decodeData); used for the + // throttled visibility log so the operator can see the gate working. + uint64_t stale_dropped_count_ {0}; + // Last publish-queue drop total observed by diagnosePublishQueue, so the // diagnostic can WARN on *recent* drops (increase since last tick) rather // than latching WARN forever after a single historical drop. Touched only diff --git a/udp_bridge/src/remote_node.cpp b/udp_bridge/src/remote_node.cpp index a9414b1..a080e50 100644 --- a/udp_bridge/src/remote_node.cpp +++ b/udp_bridge/src/remote_node.cpp @@ -92,6 +92,11 @@ void RemoteNode::update(const BridgeInfo& bridge_info, const SourceInfo& source_ received_packet_times_.clear(); resend_state_.clear(); given_up_packet_numbers_.clear(); + // The sender's packet_number space resets to 0 on restart, so + // the stale-packet high-water marks must reset too — otherwise + // every post-restart packet would be rejected as stale by + // admitForPublish. + highest_published_packet_number_.clear(); } } next_packet_number_ = bridge_info.next_packet_number; @@ -345,6 +350,27 @@ std::vector RemoteNode::unwrap(std::vector const &message, con return {}; } +bool RemoteNode::admitForPublish(const std::string& topic, uint64_t packet_number) +{ + std::lock_guard lock(state_mutex_); + auto it = highest_published_packet_number_.find(topic); + if(it != highest_published_packet_number_.end()) + { + // Strict: an equal-or-newer message for this topic was already + // admitted, so this is a superseded late resend. (Exact + // packet_number duplicates never reach here — RemoteNode::unwrap + // filters them via received_packet_times_ before decode.) + if(packet_number < it->second) + return false; + it->second = packet_number; + } + else + { + highest_published_packet_number_[topic] = packet_number; + } + return true; +} + Defragmenter& RemoteNode::defragmenter() { return defragmenter_; diff --git a/udp_bridge/src/udp_bridge.cpp b/udp_bridge/src/udp_bridge.cpp index a5361b4..3994f7e 100644 --- a/udp_bridge/src/udp_bridge.cpp +++ b/udp_bridge/src/udp_bridge.cpp @@ -141,6 +141,16 @@ UDPBridge::CallbackReturn UDPBridge::on_configure(const rclcpp_lifecycle::State } RCLCPP_INFO_STREAM(get_logger(), "maximum_packet_size: " << max_packet_size_); + // Stale-packet gate (drop late-arriving resends the resend protocol + // delivered out of order — see decodeData / RemoteNode::admitForPublish). + // Default true: a superseded old packet republished after a newer one + // makes a consumer see the topic's stamp jump backward. Disable per + // deployment (`-p drop_stale_packets:=false`) for topics that need every + // message regardless of order. + declareIfMissing("drop_stale_packets", drop_stale_packets_); + drop_stale_packets_ = get_parameter("drop_stale_packets").as_bool(); + RCLCPP_INFO_STREAM(get_logger(), "drop_stale_packets: " << std::boolalpha << drop_stale_packets_); + // Resend give-up rate thresholds (issue #22). Defaults calibrated // against the 2026-05-19 BizzyBoat storm (~3.9/s steady, ~700/s // burst): steady fires WARN, burst fires ERROR. Live-tunable: the @@ -802,9 +812,43 @@ void UDPBridge::decode(std::vector const &message, const SourceInfo& so void UDPBridge::decodeData(std::vector const &message, const SourceInfo& source_info) { - (void)source_info; auto outer_message = deserialize(message); + // destination_topic falls back to source_topic when unset. + std::string topic = outer_message.destination_topic; + if(topic.empty()) + topic = outer_message.source_topic; + + // Stale-packet gate: drop a decoded message whose wrapped packet_number + // is older than the newest already published for this destination topic + // — a late resend the resend protocol delivered out of order, which a + // newer message has already superseded (republishing it makes the + // consumer see the topic's stamp jump backward). Runs before the payload + // copy below so a dropped packet costs nothing. Un-sequenced packets + // (never wrapped) and the disabled case fall straight through. See + // RemoteNode::admitForPublish; the high-water marks live in the RemoteNode + // so they reset together with its sequence state on remote restart. + if(drop_stale_packets_ && source_info.sequenced) + { + std::shared_ptr remote_node; + { + std::lock_guard lock(remote_nodes_mutex_); + auto it = remote_nodes_.find(source_info.node_name); + if(it != remote_nodes_.end()) + remote_node = it->second; + } + if(remote_node && !remote_node->admitForPublish(topic, source_info.packet_number)) + { + ++stale_dropped_count_; + RCLCPP_INFO_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, + "drop_stale_packets: dropped stale message for topic '" << topic + << "' (packet_number " << source_info.packet_number + << " older than newest published); cumulative dropped: " + << stale_dropped_count_); + return; + } + } + // Socket-drain thread does only the CPU-bound deserialize + payload copy, // then hands everything rmw-touching to the publish worker. No publisher // create / graph query / publish runs here — see issue #10 and @@ -816,10 +860,7 @@ void UDPBridge::decodeData(std::vector const &message, const SourceInfo outer_message.data.data(), outer_message.data.size()); item.message.get_rcl_serialized_message().buffer_length = outer_message.data.size(); - // destination_topic falls back to source_topic when unset. - item.topic = outer_message.destination_topic; - if(item.topic.empty()) - item.topic = outer_message.source_topic; + item.topic = topic; item.datatype = outer_message.datatype; item.reliability = outer_message.reliability; item.durability = outer_message.durability; @@ -1154,6 +1195,13 @@ void UDPBridge::unwrap(const std::vector& message, const SourceInfo& so auto updated_source_info = source_info; updated_source_info.node_name = wrapped_packet->source_node; + // Carry the wrapped sequence number into the recursive decode so the + // stale-packet gate in decodeData can compare it against the per-topic + // high-water mark. Propagates through the Compressed and Fragment + // re-decode paths; for a reassembled message it is the completing + // fragment's number, which is sufficient to order whole messages. + updated_source_info.packet_number = wrapped_packet->packet_number; + updated_source_info.sequenced = true; std::shared_ptr remote; { diff --git a/udp_bridge/test/test_stale_packet_gate.cpp b/udp_bridge/test/test_stale_packet_gate.cpp new file mode 100644 index 0000000..d86eea1 --- /dev/null +++ b/udp_bridge/test/test_stale_packet_gate.cpp @@ -0,0 +1,154 @@ +// Unit tests for RemoteNode::admitForPublish — the stale-packet gate +// added on the 2026-06-29 BizzyBoat deployment (operator saw old packets +// republished after newer ones on heartbeat/video/costmap, making the +// topic stamp jump backward). The gate keeps a per-destination-topic +// high-water mark of the wrapped packet_number last admitted and drops a +// decoded message whose number is strictly below it. +// +// These cases pin the documented contract: +// - the first message for a topic is always admitted; +// - a strictly-older packet_number is dropped (the late-resend case); +// - an equal number is admitted (exact duplicates are filtered upstream +// by received_packet_times_ before decode, so == reaching the gate is +// not a duplicate — see admitForPublish's comment); +// - a newer number advances the high-water mark; +// - high-water marks are per-destination-topic and independent; and +// - the marks reset on remote-restart detection, so a post-restart +// packet (sender's numbers reset to 0) is NOT wrongly dropped. That +// last case is the load-bearing one: without the clear in +// update(BridgeInfo), every packet after a remote udp_bridge restart +// would be rejected as stale and the link would go silent. + +#include +#include +#include + +#include + +#include "rclcpp/rclcpp.hpp" + +#include "udp_bridge_interfaces/msg/bridge_info.hpp" +#include "udp_bridge_interfaces/msg/remote.hpp" +#include "udp_bridge/remote_node.h" + +namespace +{ + +// Owns a node so RemoteNode has the NodeInterfaces it needs at +// construction. local_name_ is "test-local"; the remote is "test-remote". +class StaleGateFixture : public ::testing::Test +{ +protected: + void SetUp() override + { + if(!rclcpp::ok()) + rclcpp::init(0, nullptr); + auto* info = ::testing::UnitTest::GetInstance()->current_test_info(); + std::string node_name = std::string("test_stale_packet_gate_") + info->name(); + node_ = std::make_shared(node_name); + remote_ = std::make_unique( + "test-remote", "test-local", + udp_bridge::RemoteNode::NodeInterfaces(*node_)); + } + + // Drive update() twice to trip the remote-restart detection in + // update(BridgeInfo): first a non-zero next_packet_number to seed + // next_packet_number_, then 0 (0 < seed) which clears the gate's + // high-water marks alongside the resend-tracking state. + void triggerRemoteRestart() + { + udp_bridge::SourceInfo src; + src.node_name = "test-remote"; + src.host = "127.0.0.1"; + src.port = 9999; + + udp_bridge_interfaces::msg::Remote remote_entry; + remote_entry.name = "test-local"; + + udp_bridge_interfaces::msg::BridgeInfo bi_seed; + bi_seed.next_packet_number = 100; + bi_seed.remotes.push_back(remote_entry); + remote_->update(bi_seed, src); + + udp_bridge_interfaces::msg::BridgeInfo bi_restart; + bi_restart.next_packet_number = 0; + bi_restart.remotes.push_back(remote_entry); + remote_->update(bi_restart, src); + } + + std::shared_ptr node_; + std::unique_ptr remote_; +}; + +TEST_F(StaleGateFixture, FirstMessageForTopicAdmitted) +{ + EXPECT_TRUE(remote_->admitForPublish("topicA", 5)); +} + +TEST_F(StaleGateFixture, StrictlyOlderDropped) +{ + ASSERT_TRUE(remote_->admitForPublish("topicA", 5)); + EXPECT_FALSE(remote_->admitForPublish("topicA", 4)); + EXPECT_FALSE(remote_->admitForPublish("topicA", 0)); +} + +TEST_F(StaleGateFixture, EqualNumberAdmitted) +{ + ASSERT_TRUE(remote_->admitForPublish("topicA", 5)); + // == is not < , so it is admitted (exact duplicates never reach the + // gate; they are filtered by received_packet_times_ before decode). + EXPECT_TRUE(remote_->admitForPublish("topicA", 5)); +} + +TEST_F(StaleGateFixture, NewerNumberAdvancesHighWater) +{ + ASSERT_TRUE(remote_->admitForPublish("topicA", 5)); + EXPECT_TRUE(remote_->admitForPublish("topicA", 7)); + // High-water is now 7, so 6 (between the two) is stale. + EXPECT_FALSE(remote_->admitForPublish("topicA", 6)); +} + +TEST_F(StaleGateFixture, HighWaterIsPerTopic) +{ + ASSERT_TRUE(remote_->admitForPublish("topicA", 10)); + // A different topic has its own high-water: first-seen 3 is admitted + // even though topicA is far ahead. + EXPECT_TRUE(remote_->admitForPublish("topicB", 3)); + // Each topic gates against only its own mark. + EXPECT_FALSE(remote_->admitForPublish("topicA", 9)); + EXPECT_FALSE(remote_->admitForPublish("topicB", 2)); + EXPECT_TRUE(remote_->admitForPublish("topicB", 4)); +} + +TEST_F(StaleGateFixture, RemoteRestartResetsHighWater) +{ + // Advance topicA's high-water, then simulate a remote udp_bridge + // restart (sender's packet numbers reset to 0). + ASSERT_TRUE(remote_->admitForPublish("topicA", 100)); + ASSERT_FALSE(remote_->admitForPublish("topicA", 1)) + << "Sanity: 1 < 100 should be stale before the restart"; + + triggerRemoteRestart(); + + // After the restart the mark is cleared, so the first post-restart + // packet (number 0) is admitted, not dropped as stale. + EXPECT_TRUE(remote_->admitForPublish("topicA", 0)) + << "post-restart packet_number 0 was dropped — update(BridgeInfo) " + "did not clear highest_published_packet_number_, so the gate " + "rejects every packet after a remote restart and the link goes " + "silent (remote_node.cpp restart-detection branch)."; + // And the gate resumes normally from the new baseline. + EXPECT_TRUE(remote_->admitForPublish("topicA", 1)); + EXPECT_FALSE(remote_->admitForPublish("topicA", 0)); +} + +} // namespace + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + int rc = RUN_ALL_TESTS(); + if(rclcpp::ok()) + rclcpp::shutdown(); + return rc; +}