Field import: udp_bridge stale-packet gate (2026-06-29) + tests

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()

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<uint64_t, rclcpp::Time> 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<std::string, uint64_t> 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

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

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

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<uint8_t> RemoteNode::unwrap(std::vector<uint8_t> const &message, con
   return {};
 }
 
+bool RemoteNode::admitForPublish(const std::string& topic, uint64_t packet_number)
+{
+  std::lock_guard<std::recursive_mutex> 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_;

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<uint8_t> const &message, const SourceInfo& so
 
 void UDPBridge::decodeData(std::vector<uint8_t> const &message, const SourceInfo& source_info)
 {
-  (void)source_info;
   auto outer_message = deserialize<MessageInternal>(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<RemoteNode> remote_node;
+    {
+      std::lock_guard<std::mutex> 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<uint8_t> 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<uint8_t>& 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<RemoteNode> remote;
     {

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 <cstdint>
+#include <memory>
+#include <string>
+
+#include <gtest/gtest.h>
+
+#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<rclcpp::Node>(node_name);
+    remote_ = std::make_unique<udp_bridge::RemoteNode>(
+      "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<rclcpp::Node> node_;
+  std::unique_ptr<udp_bridge::RemoteNode> 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;
+}