cu-ros2-bridge 1.0.1

mod attachment;
mod error;
mod keyexpr;
mod liveliness;
mod node;
mod topic;

use attachment::encode_attachment;
use cdr::{CdrLe, Infinite};
use cu_ros2_payloads::RosBridgeAdapter;
use cu29::cubridge::{BridgeChannel, BridgeChannelConfig, BridgeChannelSet, CuBridge};
use cu29::prelude::*;
use liveliness::{node_liveliness, publisher_liveliness, subscriber_liveliness};
use node::Node;
use topic::Topic;
use zenoh::bytes::Encoding;
use zenoh::{Config, Error as ZenohError};

use std::any::{Any, TypeId, type_name};
use std::collections::HashMap;
use std::sync::{OnceLock, RwLock};

// One node per bridge session.
const NODE_ID: u32 = 0;

#[derive(Clone, Copy)]
struct RosPayloadCodec {
    namespace: &'static str,
    type_name: &'static str,
    type_hash: &'static str,
    encode_payload: fn(&dyn Any) -> CuResult<Vec<u8>>,
    decode_payload: fn(&[u8]) -> CuResult<Box<dyn Any>>,
}

struct RosPayloadRegistry {
    codecs: RwLock<HashMap<TypeId, RosPayloadCodec>>,
}

impl RosPayloadRegistry {
    fn new() -> Self {
        Self {
            codecs: RwLock::new(HashMap::new()),
        }
    }

    fn register<Payload>(&self)
    where
        Payload: CuMsgPayload + RosBridgeAdapter + 'static,
    {
        let codec = RosPayloadCodec {
            namespace: <Payload as RosBridgeAdapter>::namespace(),
            type_name: <Payload as RosBridgeAdapter>::type_name(),
            type_hash: <Payload as RosBridgeAdapter>::type_hash(),
            encode_payload: encode_payload_with::<Payload>,
            decode_payload: decode_payload_with::<Payload>,
        };
        self.codecs
            .write()
            .expect("RosPayloadRegistry lock poisoned")
            .insert(TypeId::of::<Payload>(), codec);
    }

    fn codec_for<Payload>(&self) -> CuResult<RosPayloadCodec>
    where
        Payload: CuMsgPayload + 'static,
    {
        self.codecs
            .read()
            .expect("RosPayloadRegistry lock poisoned")
            .get(&TypeId::of::<Payload>())
            .copied()
            .ok_or_else(|| {
                CuError::from(format!(
                    "Ros2Bridge: No ROS codec registered for payload type {}",
                    type_name::<Payload>()
                ))
            })
    }
}

fn encode_payload_with<Payload>(payload_any: &dyn Any) -> CuResult<Vec<u8>>
where
    Payload: CuMsgPayload + RosBridgeAdapter + 'static,
{
    let payload = payload_any.downcast_ref::<Payload>().ok_or_else(|| {
        CuError::from(format!(
            "Ros2Bridge: Registry encode payload mismatch for {}",
            type_name::<Payload>()
        ))
    })?;
    payload
        .validate_ros_message()
        .map_err(|e| CuError::from(format!("Ros2Bridge: Payload is not ROS-compatible: {e}")))?;
    let ros_payload = payload.to_ros_message();
    cdr::serialize::<_, _, CdrLe>(&ros_payload, Infinite)
        .map_err(|e| CuError::new_with_cause("Ros2Bridge: Failed to serialize payload", e))
}

fn decode_payload_with<Payload>(bytes: &[u8]) -> CuResult<Box<dyn Any>>
where
    Payload: CuMsgPayload + RosBridgeAdapter + 'static,
{
    let ros_payload: <Payload as RosBridgeAdapter>::RosMessage = cdr::deserialize(bytes)
        .map_err(|e| CuError::new_with_cause("Ros2Bridge: Failed to deserialize payload", e))?;
    let payload = Payload::from_ros_message(ros_payload).map_err(CuError::from)?;
    Ok(Box::new(payload))
}

fn payload_registry() -> &'static RosPayloadRegistry {
    static REGISTRY: OnceLock<RosPayloadRegistry> = OnceLock::new();
    REGISTRY.get_or_init(|| {
        let registry = RosPayloadRegistry::new();
        registry.register::<bool>();
        registry.register::<i8>();
        registry.register::<i16>();
        registry.register::<i32>();
        registry.register::<i64>();
        registry.register::<u8>();
        registry.register::<u16>();
        registry.register::<u32>();
        registry.register::<u64>();
        registry.register::<f32>();
        registry.register::<f64>();
        registry.register::<String>();
        registry
    })
}

/// Register a payload codec for ROS2 bridge transport.
///
/// Call this once at application startup for custom payload types that implement
/// [`cu_ros2_payloads::RosBridgeAdapter`].
pub fn register_ros2_payload<Payload>()
where
    Payload: CuMsgPayload + RosBridgeAdapter + 'static,
{
    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)]
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,
}

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,
    route: String,
    entity_id: u32,
    sequence_number: u64,
    publisher: Option<zenoh::pubsub::Publisher<'static>>,
    publisher_token: Option<zenoh::liveliness::LivelinessToken>,
}

struct Ros2RxChannel<Id: Copy> {
    id: Id,
    route: String,
    entity_id: u32,
    queue: RxQueueConfig,
    subscriber: Option<Ros2Subscriber>,
    subscriber_token: Option<zenoh::liveliness::LivelinessToken>,
}

struct Ros2Context<TxId: Copy, RxId: Copy> {
    session: zenoh::Session,
    #[allow(dead_code)]
    node_token: zenoh::liveliness::LivelinessToken,
    tx_channels: Vec<Ros2TxChannel<TxId>>,
    rx_channels: Vec<Ros2RxChannel<RxId>>,
}

#[derive(Reflect)]
#[reflect(from_reflect = false, no_field_bounds, type_path = false)]
pub struct Ros2Bridge<Tx, Rx>
where
    Tx: BridgeChannelSet + 'static,
    Rx: BridgeChannelSet + 'static,
    Tx::Id: Send + Sync + 'static,
    Rx::Id: Send + Sync + 'static,
{
    #[reflect(ignore)]
    session_config: Config,
    domain_id: u32,
    namespace: String,
    node: String,
    #[reflect(ignore)]
    tx_channels: Vec<Ros2TxChannelConfig<Tx::Id>>,
    #[reflect(ignore)]
    rx_channels: Vec<Ros2RxChannelConfig<Rx::Id>>,
    #[reflect(ignore)]
    ctx: Option<Ros2Context<Tx::Id, Rx::Id>>,
}

impl<Tx, Rx> Freezable for Ros2Bridge<Tx, Rx>
where
    Tx: BridgeChannelSet + 'static,
    Rx: BridgeChannelSet + 'static,
    Tx::Id: Send + Sync + 'static,
    Rx::Id: Send + Sync + 'static,
{
}

impl<Tx, Rx> cu29::reflect::TypePath for Ros2Bridge<Tx, Rx>
where
    Tx: BridgeChannelSet + 'static,
    Rx: BridgeChannelSet + 'static,
    Tx::Id: Send + Sync + 'static,
    Rx::Id: Send + Sync + 'static,
{
    fn type_path() -> &'static str {
        "cu_ros2_bridge::Ros2Bridge"
    }

    fn short_type_path() -> &'static str {
        "Ros2Bridge"
    }

    fn type_ident() -> Option<&'static str> {
        Some("Ros2Bridge")
    }

    fn crate_name() -> Option<&'static str> {
        Some("cu_ros2_bridge")
    }

    fn module_path() -> Option<&'static str> {
        Some("cu_ros2_bridge")
    }
}

impl<Tx, Rx> Ros2Bridge<Tx, Rx>
where
    Tx: BridgeChannelSet + 'static,
    Rx: BridgeChannelSet + 'static,
    Tx::Id: Send + Sync + 'static,
    Rx::Id: Send + Sync + 'static,
{
    fn parse_session_config(config: &ComponentConfig) -> CuResult<Config> {
        if let Some(path) = config.get::<String>("zenoh_config_file")? {
            return Config::from_file(&path).map_err(|e| {
                CuError::from(format!("Ros2Bridge: Failed to read config file: {e}"))
            });
        }
        if let Some(json) = config.get::<String>("zenoh_config_json")? {
            return Config::from_json5(&json).map_err(|e| {
                CuError::from(format!("Ros2Bridge: Failed to parse config json: {e}"))
            });
        }
        Ok(Config::default())
    }

    fn parse_domain_id(config: &ComponentConfig) -> CuResult<u32> {
        Ok(config.get::<u32>("domain_id")?.unwrap_or(0))
    }

    fn parse_namespace(config: &ComponentConfig) -> CuResult<String> {
        Ok(config
            .get::<String>("namespace")?
            .unwrap_or_else(|| "copper".to_string()))
    }

    fn parse_node_name(config: &ComponentConfig) -> CuResult<String> {
        Ok(config
            .get::<String>("node")?
            .unwrap_or_else(|| "node".to_string()))
    }

    fn channel_route<Id: Copy + core::fmt::Debug>(
        channel: &BridgeChannelConfig<Id>,
    ) -> CuResult<String> {
        channel
            .effective_route()
            .map(|route| route.into_owned())
            .ok_or_else(|| {
                let id = channel.channel.id;
                CuError::from(format!(
                    "Ros2Bridge: Missing route/topic for channel {:?}",
                    id
                ))
            })
    }

    fn make_node<'a>(
        domain_id: u32,
        namespace: &'a str,
        node_name: &'a str,
        session: &'a zenoh::Session,
    ) -> Node<'a> {
        Node {
            domain_id,
            zid: session.zid(),
            id: NODE_ID,
            namespace,
            name: node_name,
        }
    }

    fn find_tx_channel_index(channels: &[Ros2TxChannel<Tx::Id>], id: Tx::Id) -> Option<usize> {
        channels.iter().position(|channel| channel.id == id)
    }

    fn find_rx_channel_index(channels: &[Ros2RxChannel<Rx::Id>], id: Rx::Id) -> Option<usize> {
        channels.iter().position(|channel| channel.id == id)
    }

    fn codec_for_payload<Payload>() -> CuResult<RosPayloadCodec>
    where
        Payload: CuMsgPayload + 'static,
    {
        payload_registry().codec_for::<Payload>()
    }

    fn topic_for_codec<'a>(route: &'a str, codec: RosPayloadCodec) -> Topic<'a> {
        Topic::from_ros_type(route, codec.namespace, codec.type_name, codec.type_hash)
    }

    fn encode_payload<Payload>(msg: &CuMsg<Payload>, codec: RosPayloadCodec) -> CuResult<Vec<u8>>
    where
        Payload: CuMsgPayload + 'static,
    {
        let payload = msg
            .payload()
            .ok_or_else(|| CuError::from("Ros2Bridge: Cannot send empty payload through bridge"))?;
        (codec.encode_payload)(payload as &dyn Any)
    }

    fn decode_payload_into<Payload>(
        bytes: &[u8],
        msg: &mut CuMsg<Payload>,
        codec: RosPayloadCodec,
    ) -> CuResult<()>
    where
        Payload: CuMsgPayload + 'static,
    {
        let payload_any = (codec.decode_payload)(bytes)?;
        let payload = payload_any.downcast::<Payload>().map_err(|_| {
            CuError::from(format!(
                "Ros2Bridge: Codec produced wrong payload type for {}",
                type_name::<Payload>()
            ))
        })?;
        msg.set_payload(*payload);
        Ok(())
    }

    fn init_tx_channel(
        domain_id: u32,
        namespace: &str,
        node_name: &str,
        ctx: &mut Ros2Context<Tx::Id, Rx::Id>,
        tx_idx: usize,
        codec: RosPayloadCodec,
    ) -> CuResult<()> {
        if ctx.tx_channels[tx_idx].publisher.is_some() {
            return Ok(());
        }

        let route = ctx.tx_channels[tx_idx].route.clone();
        let entity_id = ctx.tx_channels[tx_idx].entity_id;
        let topic = Self::topic_for_codec(route.as_str(), codec);
        let node = Self::make_node(domain_id, namespace, node_name, &ctx.session);

        let publisher_token = zenoh::Wait::wait(
            ctx.session
                .liveliness()
                .declare_token(publisher_liveliness(&node, &topic, entity_id)?),
        )
        .map_err(cu_error_map(
            "Ros2Bridge: Failed to declare topic liveliness token",
        ))?;

        let publisher =
            zenoh::Wait::wait(ctx.session.declare_publisher(topic.pubsub_keyexpr(&node)?))
                .map_err(cu_error_map("Ros2Bridge: Failed to create publisher"))?;

        ctx.tx_channels[tx_idx].publisher = Some(publisher);
        ctx.tx_channels[tx_idx].publisher_token = Some(publisher_token);

        Ok(())
    }

    fn init_rx_channel(
        domain_id: u32,
        namespace: &str,
        node_name: &str,
        ctx: &mut Ros2Context<Tx::Id, Rx::Id>,
        rx_idx: usize,
        codec: RosPayloadCodec,
    ) -> CuResult<()> {
        if ctx.rx_channels[rx_idx].subscriber.is_some() {
            return Ok(());
        }

        let route = ctx.rx_channels[rx_idx].route.clone();
        let entity_id = ctx.rx_channels[rx_idx].entity_id;
        let topic = Self::topic_for_codec(route.as_str(), codec);
        let node = Self::make_node(domain_id, namespace, node_name, &ctx.session);

        let subscriber_token = zenoh::Wait::wait(
            ctx.session
                .liveliness()
                .declare_token(subscriber_liveliness(&node, &topic, entity_id)?),
        )
        .map_err(cu_error_map(
            "Ros2Bridge: Failed to declare subscriber liveliness token",
        ))?;

        let keyexpr = topic.pubsub_keyexpr(&node)?;
        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);
        Ok(())
    }

    fn ctx_mut(&mut self) -> CuResult<&mut Ros2Context<Tx::Id, Rx::Id>> {
        self.ctx
            .as_mut()
            .ok_or_else(|| CuError::from("Ros2Bridge: Context not initialized"))
    }
}

impl<Tx, Rx> CuBridge for Ros2Bridge<Tx, Rx>
where
    Tx: BridgeChannelSet + 'static,
    Rx: BridgeChannelSet + 'static,
    Tx::Id: core::fmt::Debug + Send + Sync + 'static,
    Rx::Id: core::fmt::Debug + Send + Sync + 'static,
{
    type Tx = Tx;
    type Rx = Rx;
    type Resources<'r> = ();

    fn new(
        config: Option<&ComponentConfig>,
        tx_channels: &[BridgeChannelConfig<<Self::Tx as BridgeChannelSet>::Id>],
        rx_channels: &[BridgeChannelConfig<<Self::Rx as BridgeChannelSet>::Id>],
        _resources: Self::Resources<'_>,
    ) -> CuResult<Self>
    where
        Self: Sized,
    {
        let default_config = ComponentConfig::default();
        let config = config.unwrap_or(&default_config);

        let session_config = Self::parse_session_config(config)?;
        let domain_id = Self::parse_domain_id(config)?;
        let namespace = Self::parse_namespace(config)?;
        let node = Self::parse_node_name(config)?;

        let mut tx_cfgs = Vec::with_capacity(tx_channels.len());
        for channel in tx_channels {
            let route = Self::channel_route(channel)?;
            tx_cfgs.push(Ros2TxChannelConfig {
                id: channel.channel.id,
                route,
            });
        }

        let mut rx_cfgs = Vec::with_capacity(rx_channels.len());
        for channel in rx_channels {
            let route = Self::channel_route(channel)?;
            let queue = RxQueueConfig::from_config(channel.config.as_ref())?;
            rx_cfgs.push(Ros2RxChannelConfig {
                id: channel.channel.id,
                route,
                queue,
            });
        }

        Ok(Self {
            session_config,
            domain_id,
            namespace,
            node,
            tx_channels: tx_cfgs,
            rx_channels: rx_cfgs,
            ctx: None,
        })
    }

    fn start(&mut self, _ctx: &CuContext) -> CuResult<()> {
        let session = zenoh::Wait::wait(zenoh::open(self.session_config.clone()))
            .map_err(cu_error_map("Ros2Bridge: Failed to open session"))?;

        let node = Self::make_node(self.domain_id, &self.namespace, &self.node, &session);
        let node_token =
            zenoh::Wait::wait(session.liveliness().declare_token(node_liveliness(&node)?))
                .map_err(cu_error_map(
                    "Ros2Bridge: Failed to declare node liveliness token",
                ))?;

        let tx_channels = self
            .tx_channels
            .iter()
            .enumerate()
            .map(|(index, channel)| Ros2TxChannel {
                id: channel.id,
                route: channel.route.clone(),
                entity_id: (index + 1) as u32,
                sequence_number: 0,
                publisher: None,
                publisher_token: None,
            })
            .collect();

        let rx_channels = self
            .rx_channels
            .iter()
            .enumerate()
            .map(|(index, channel)| Ros2RxChannel {
                id: channel.id,
                route: channel.route.clone(),
                entity_id: (index + 1) as u32,
                queue: channel.queue.clone(),
                subscriber: None,
                subscriber_token: None,
            })
            .collect();

        self.ctx = Some(Ros2Context {
            session,
            node_token,
            tx_channels,
            rx_channels,
        });

        Ok(())
    }

    fn send<'a, Payload>(
        &mut self,
        ctx: &CuContext,
        channel: &'static BridgeChannel<<Self::Tx as BridgeChannelSet>::Id, Payload>,
        msg: &CuMsg<Payload>,
    ) -> CuResult<()>
    where
        Payload: CuMsgPayload + 'a,
    {
        let codec = Self::codec_for_payload::<Payload>()?;
        let domain_id = self.domain_id;
        let namespace = self.namespace.clone();
        let node_name = self.node.clone();
        let channel_id = channel.id();
        let bridge_ctx = self.ctx_mut()?;

        let tx_idx =
            Self::find_tx_channel_index(&bridge_ctx.tx_channels, channel_id).ok_or_else(|| {
                CuError::from(format!("Ros2Bridge: Unknown Tx channel {:?}", channel_id))
            })?;

        Self::init_tx_channel(domain_id, &namespace, &node_name, bridge_ctx, tx_idx, codec)?;

        let encoded = Self::encode_payload(msg, codec)?;
        let session_zid = bridge_ctx.session.zid();

        let tx_channel = &mut bridge_ctx.tx_channels[tx_idx];
        let publisher = tx_channel
            .publisher
            .as_mut()
            .ok_or_else(|| CuError::from("Ros2Bridge: Tx publisher not initialized"))?;

        let attachment = encode_attachment(tx_channel.sequence_number, ctx, &session_zid);
        tx_channel.sequence_number += 1;

        zenoh::Wait::wait(
            publisher
                .put(encoded)
                .encoding(Encoding::APPLICATION_CDR)
                .attachment(attachment),
        )
        .map_err(cu_error_map("Ros2Bridge: Failed to put value"))?;

        Ok(())
    }

    fn receive<'a, Payload>(
        &mut self,
        ctx: &CuContext,
        channel: &'static BridgeChannel<<Self::Rx as BridgeChannelSet>::Id, Payload>,
        msg: &mut CuMsg<Payload>,
    ) -> CuResult<()>
    where
        Payload: CuMsgPayload + 'a,
    {
        let codec = Self::codec_for_payload::<Payload>()?;
        let domain_id = self.domain_id;
        let namespace = self.namespace.clone();
        let node_name = self.node.clone();
        let channel_id = channel.id();
        let bridge_ctx = self.ctx_mut()?;

        let rx_idx =
            Self::find_rx_channel_index(&bridge_ctx.rx_channels, channel_id).ok_or_else(|| {
                CuError::from(format!("Ros2Bridge: Unknown Rx channel {:?}", channel_id))
            })?;

        Self::init_rx_channel(domain_id, &namespace, &node_name, bridge_ctx, rx_idx, codec)?;

        msg.tov = Tov::Time(ctx.now());

        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();
            Self::decode_payload_into(payload.as_ref(), msg, codec)?;
        } else {
            msg.clear_payload();
        }

        Ok(())
    }

    fn stop(&mut self, _ctx: &CuContext) -> CuResult<()> {
        if let Some(Ros2Context {
            session,
            node_token: _,
            tx_channels,
            rx_channels,
        }) = self.ctx.take()
        {
            for channel in tx_channels {
                if let Some(publisher) = channel.publisher {
                    zenoh::Wait::wait(publisher.undeclare())
                        .map_err(cu_error_map("Ros2Bridge: Failed to undeclare publisher"))?;
                }
            }

            for channel in rx_channels {
                if let Some(subscriber) = channel.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"))?,
                    }
                }
            }

            zenoh::Wait::wait(session.close())
                .map_err(cu_error_map("Ros2Bridge: Failed to close session"))?;
        }

        Ok(())
    }
}

fn cu_error_map(msg: &str) -> impl FnOnce(ZenohError) -> CuError + '_ {
    move |e| CuError::from(format!("{msg}: {e}"))
}

#[cfg(test)]
mod tests {
    use super::*;
    use cu29::bincode::{Decode, Encode};
    use serde::{Deserialize, Serialize};

    #[derive(Clone, Debug, Default, Encode, Decode, Serialize, Deserialize, Reflect)]
    struct RejectingPayload;

    impl RosBridgeAdapter for RejectingPayload {
        type RosMessage = i32;

        fn validate_ros_message(&self) -> Result<(), String> {
            Err("test payload rejected".to_string())
        }

        fn namespace() -> &'static str {
            "test_msgs"
        }

        fn type_hash() -> &'static str {
            "test_hash"
        }

        fn to_ros_message(&self) -> Self::RosMessage {
            0
        }

        fn from_ros_message(_msg: Self::RosMessage) -> Result<Self, String> {
            Ok(Self)
        }
    }

    #[test]
    fn payload_validation_failure_stops_encoding() {
        let error = encode_payload_with::<RejectingPayload>(&RejectingPayload)
            .expect_err("validation failure should stop ROS encoding");

        assert!(error.to_string().contains("test payload rejected"));
    }

    #[test]
    fn scalar_payload_cdr_roundtrip_uses_little_endian_encapsulation() {
        let mut src = CuMsg::<i32>::default();
        src.set_payload(0x0102_0304);

        let codec =
            Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i32>()
                .expect("codec should be registered");
        let bytes =
            Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::encode_payload(&src, codec)
                .expect("encode should succeed");

        assert_eq!(&bytes[..4], &[0, 1, 0, 0], "CDR_LE encapsulation");
        assert_eq!(&bytes[4..8], &[4, 3, 2, 1], "little-endian i32 data");

        let mut dst = CuMsg::<i32>::default();
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::decode_payload_into(
            bytes.as_slice(),
            &mut dst,
            codec,
        )
        .expect("decode should succeed");

        assert_eq!(dst.payload(), Some(&0x0102_0304));
    }

    #[test]
    fn default_scalar_codecs_registered() {
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<bool>()
            .expect("bool codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i8>()
            .expect("i8 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i16>()
            .expect("i16 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i32>()
            .expect("i32 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i64>()
            .expect("i64 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<u8>()
            .expect("u8 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<u16>()
            .expect("u16 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<u32>()
            .expect("u32 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<u64>()
            .expect("u64 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<f32>()
            .expect("f32 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<f64>()
            .expect("f64 codec");
        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<String>()
            .expect("string codec");
    }

    tx_channels! {
        struct DummyTx : DummyTxId {
            out => i8,
        }
    }

    rx_channels! {
        struct DummyRx : DummyRxId {
            input => i8,
        }
    }
}