Add "ring" queue mode for ROS2 payload and serde_bytes (#914)

Author: NRdrgz

Cargo.toml

@@ -180,6 +180,9 @@ serde = { version = "1.0", default-features = false, features = [
   "derive",
   "alloc",
 ] }
+serde_bytes = { version = "0.11", default-features = false, features = [
+  "alloc",
+] }
 serde_json = { version = "1.0", default-features = false }
 erased-serde = { version = "0.4", default-features = false, features = [
   "alloc",

components/bridges/cu_ros2_bridge/README.md

@@ -14,6 +14,10 @@ Bridge-level config:
 Per-channel config (`bridges[].channels`):
 - `route`: ROS 2 topic path used by that channel.
 
+Rx-only options:
+- `queue_mode`: `"fifo"` (default) First In First Out mode. Delivers samples in order, one per cycle. `"ring"` creates a buffer and drops oldest sample when full.
+- `ring_size`: depth of the ring buffer when `queue_mode` is `"ring"` (default `1`).
+
 Example:
 
 ```ron
@@ -28,7 +32,10 @@ bridges: [
     },
     channels: [
       Tx(id: "outgoing", route: "/output"),
+      // fifo: ordered delivery, one sample per cycle (default)
       Rx(id: "incoming", route: "/input"),
+      // ring: always receive the freshest sample
+      Rx(id: "sensor",   route: "/sensor", config: {"queue_mode": "ring"}),
     ],
   ),
 ],

components/bridges/cu_ros2_bridge/src/lib.rs

@@ -134,14 +134,49 @@ where
     payload_registry().register::<Payload>();
 }
 
+/// Per-channel Rx queue strategy, configured via `queue_mode` in the channel config.
+/// - `"fifo"` (default): ordered delivery, one sample per cycle.
+/// - `"ring"`: creates a buffer and drops the oldest sample when full; set `ring_size` (default 1) for buffer depth.
 #[derive(Debug, Clone)]
-struct Ros2ChannelConfig<Id: Copy> {
+enum RxQueueConfig {
+    Fifo,
+    Ring { size: usize },
+}
+
+impl RxQueueConfig {
+    fn from_config(config: Option<&ComponentConfig>) -> CuResult<Self> {
+        let Some(cfg) = config else {
+            return Ok(Self::Fifo);
+        };
+        match cfg.get::<String>("queue_mode")?.as_deref() {
+            Some("ring") => Ok(Self::Ring {
+                size: cfg.get::<u32>("ring_size")?.unwrap_or(1) as usize,
+            }),
+            Some("fifo") | None => Ok(Self::Fifo),
+            Some(other) => Err(CuError::from(format!(
+                "Ros2Bridge: unknown queue_mode '{other}'"
+            ))),
+        }
+    }
+}
+
+#[derive(Debug, Clone)]
+struct Ros2TxChannelConfig<Id: Copy> {
+    id: Id,
+    route: String,
+}
+
+#[derive(Debug, Clone)]
+struct Ros2RxChannelConfig<Id: Copy> {
     id: Id,
     route: String,
+    queue: RxQueueConfig,
 }
 
-type Ros2Subscriber =
-    zenoh::pubsub::Subscriber<zenoh::handlers::FifoChannelHandler<zenoh::sample::Sample>>;
+enum Ros2Subscriber {
+    Fifo(zenoh::pubsub::Subscriber<zenoh::handlers::FifoChannelHandler<zenoh::sample::Sample>>),
+    Ring(zenoh::pubsub::Subscriber<zenoh::handlers::RingChannelHandler<zenoh::sample::Sample>>),
+}
 
 struct Ros2TxChannel<Id: Copy> {
     id: Id,
@@ -156,6 +191,7 @@ struct Ros2RxChannel<Id: Copy> {
     id: Id,
     route: String,
     entity_id: u32,
+    queue: RxQueueConfig,
     subscriber: Option<Ros2Subscriber>,
     subscriber_token: Option<zenoh::liveliness::LivelinessToken>,
 }
@@ -183,9 +219,9 @@ where
     namespace: String,
     node: String,
     #[reflect(ignore)]
-    tx_channels: Vec<Ros2ChannelConfig<Tx::Id>>,
+    tx_channels: Vec<Ros2TxChannelConfig<Tx::Id>>,
     #[reflect(ignore)]
-    rx_channels: Vec<Ros2ChannelConfig<Rx::Id>>,
+    rx_channels: Vec<Ros2RxChannelConfig<Rx::Id>>,
     #[reflect(ignore)]
     ctx: Option<Ros2Context<Tx::Id, Rx::Id>>,
 }
@@ -405,8 +441,20 @@ where
         ))?;
 
         let keyexpr = topic.pubsub_keyexpr(&node)?;
-        let subscriber = zenoh::Wait::wait(ctx.session.declare_subscriber(keyexpr))
-            .map_err(cu_error_map("Ros2Bridge: Failed to declare subscriber"))?;
+        let subscriber = match ctx.rx_channels[rx_idx].queue {
+            RxQueueConfig::Fifo => Ros2Subscriber::Fifo(
+                zenoh::Wait::wait(ctx.session.declare_subscriber(keyexpr))
+                    .map_err(cu_error_map("Ros2Bridge: Failed to declare subscriber"))?,
+            ),
+            RxQueueConfig::Ring { size } => Ros2Subscriber::Ring(
+                zenoh::Wait::wait(
+                    ctx.session
+                        .declare_subscriber(keyexpr)
+                        .with(zenoh::handlers::RingChannel::new(size)),
+                )
+                .map_err(cu_error_map("Ros2Bridge: Failed to declare subscriber"))?,
+            ),
+        };
 
         ctx.rx_channels[rx_idx].subscriber_token = Some(subscriber_token);
         ctx.rx_channels[rx_idx].subscriber = Some(subscriber);
@@ -451,7 +499,7 @@ where
         let mut tx_cfgs = Vec::with_capacity(tx_channels.len());
         for channel in tx_channels {
             let route = Self::channel_route(channel)?;
-            tx_cfgs.push(Ros2ChannelConfig {
+            tx_cfgs.push(Ros2TxChannelConfig {
                 id: channel.channel.id,
                 route,
             });
@@ -460,9 +508,11 @@ where
         let mut rx_cfgs = Vec::with_capacity(rx_channels.len());
         for channel in rx_channels {
             let route = Self::channel_route(channel)?;
-            rx_cfgs.push(Ros2ChannelConfig {
+            let queue = RxQueueConfig::from_config(channel.config.as_ref())?;
+            rx_cfgs.push(Ros2RxChannelConfig {
                 id: channel.channel.id,
                 route,
+                queue,
             });
         }
 
@@ -510,6 +560,7 @@ where
                 id: channel.id,
                 route: channel.route.clone(),
                 entity_id: (index + 1) as u32,
+                queue: channel.queue.clone(),
                 subscriber: None,
                 subscriber_token: None,
             })
@@ -596,15 +647,15 @@ where
 
         msg.tov = Tov::Time(ctx.now());
 
-        let sample = {
-            let subscriber = bridge_ctx.rx_channels[rx_idx]
-                .subscriber
-                .as_mut()
-                .ok_or_else(|| CuError::from("Ros2Bridge: Rx subscriber not initialized"))?;
-            subscriber
-                .try_recv()
-                .map_err(|e| CuError::from(format!("Ros2Bridge: receive failed: {e}")))?
-        };
+        let subscriber = bridge_ctx.rx_channels[rx_idx]
+            .subscriber
+            .as_mut()
+            .ok_or_else(|| CuError::from("Ros2Bridge: Rx subscriber not initialized"))?;
+        let sample = match subscriber {
+            Ros2Subscriber::Fifo(s) => s.try_recv(),
+            Ros2Subscriber::Ring(s) => s.try_recv(),
+        }
+        .map_err(|e| CuError::from(format!("Ros2Bridge: receive failed: {e}")))?;
 
         if let Some(sample) = sample {
             let payload = sample.payload().to_bytes();
@@ -633,8 +684,12 @@ where
 
             for channel in rx_channels {
                 if let Some(subscriber) = channel.subscriber {
-                    zenoh::Wait::wait(subscriber.undeclare())
-                        .map_err(cu_error_map("Ros2Bridge: Failed to undeclare subscriber"))?;
+                    match subscriber {
+                        Ros2Subscriber::Fifo(s) => zenoh::Wait::wait(s.undeclare())
+                            .map_err(cu_error_map("Ros2Bridge: Failed to undeclare subscriber"))?,
+                        Ros2Subscriber::Ring(s) => zenoh::Wait::wait(s.undeclare())
+                            .map_err(cu_error_map("Ros2Bridge: Failed to undeclare subscriber"))?,
+                    }
                 }
             }
 

components/payloads/cu_ros2_payloads/Cargo.toml

@@ -13,6 +13,7 @@ description = "ROS2 Payloads that can be constructed from/to Copper Payloads for
 [dependencies]
 compact_str = { workspace = true }
 serde = { workspace = true }
+serde_bytes = { workspace = true }
 cu-sensor-payloads = { workspace = true }
 cu29 = { workspace = true }
 

components/payloads/cu_ros2_payloads/src/sensor_msgs.rs

@@ -37,6 +37,7 @@ pub struct PointCloud2 {
     pub is_bigendian: bool,
     pub point_step: u32,
     pub row_step: u32,
+    #[serde(with = "serde_bytes")]
     pub data: Vec<u8>,
     pub is_dense: bool,
 }
@@ -50,6 +51,7 @@ pub struct Image {
     pub encoding: String,
     pub is_bigendian: u8,
     pub step: u32,
+    #[serde(with = "serde_bytes")]
     pub data: Vec<u8>,
 }