cu_ros2_bridge/
lib.rs

1mod attachment;
2mod error;
3mod keyexpr;
4mod liveliness;
5mod node;
6mod topic;
7
8use attachment::encode_attachment;
9use cdr::{CdrLe, Infinite};
10use cu_ros2_payloads::RosBridgeAdapter;
11use cu29::cubridge::{BridgeChannel, BridgeChannelConfig, BridgeChannelSet, CuBridge};
12use cu29::prelude::*;
13use liveliness::{node_liveliness, publisher_liveliness, subscriber_liveliness};
14use node::Node;
15use topic::Topic;
16use zenoh::bytes::Encoding;
17use zenoh::{Config, Error as ZenohError};
18
19use std::any::{Any, TypeId, type_name};
20use std::collections::HashMap;
21use std::sync::{OnceLock, RwLock};
22
23// One node per bridge session.
24const NODE_ID: u32 = 0;
25
26#[derive(Clone, Copy)]
27struct RosPayloadCodec {
28    namespace: &'static str,
29    type_name: &'static str,
30    type_hash: &'static str,
31    encode_payload: fn(&dyn Any) -> CuResult<Vec<u8>>,
32    decode_payload: fn(&[u8]) -> CuResult<Box<dyn Any>>,
33}
34
35struct RosPayloadRegistry {
36    codecs: RwLock<HashMap<TypeId, RosPayloadCodec>>,
37}
38
39impl RosPayloadRegistry {
40    fn new() -> Self {
41        Self {
42            codecs: RwLock::new(HashMap::new()),
43        }
44    }
45
46    fn register<Payload>(&self)
47    where
48        Payload: CuMsgPayload + RosBridgeAdapter + 'static,
49    {
50        let codec = RosPayloadCodec {
51            namespace: <Payload as RosBridgeAdapter>::namespace(),
52            type_name: <Payload as RosBridgeAdapter>::type_name(),
53            type_hash: <Payload as RosBridgeAdapter>::type_hash(),
54            encode_payload: encode_payload_with::<Payload>,
55            decode_payload: decode_payload_with::<Payload>,
56        };
57        self.codecs
58            .write()
59            .expect("RosPayloadRegistry lock poisoned")
60            .insert(TypeId::of::<Payload>(), codec);
61    }
62
63    fn codec_for<Payload>(&self) -> CuResult<RosPayloadCodec>
64    where
65        Payload: CuMsgPayload + 'static,
66    {
67        self.codecs
68            .read()
69            .expect("RosPayloadRegistry lock poisoned")
70            .get(&TypeId::of::<Payload>())
71            .copied()
72            .ok_or_else(|| {
73                CuError::from(format!(
74                    "Ros2Bridge: No ROS codec registered for payload type {}",
75                    type_name::<Payload>()
76                ))
77            })
78    }
79}
80
81fn encode_payload_with<Payload>(payload_any: &dyn Any) -> CuResult<Vec<u8>>
82where
83    Payload: CuMsgPayload + RosBridgeAdapter + 'static,
84{
85    let payload = payload_any.downcast_ref::<Payload>().ok_or_else(|| {
86        CuError::from(format!(
87            "Ros2Bridge: Registry encode payload mismatch for {}",
88            type_name::<Payload>()
89        ))
90    })?;
91    payload
92        .validate_ros_message()
93        .map_err(|e| CuError::from(format!("Ros2Bridge: Payload is not ROS-compatible: {e}")))?;
94    let ros_payload = payload.to_ros_message();
95    cdr::serialize::<_, _, CdrLe>(&ros_payload, Infinite)
96        .map_err(|e| CuError::new_with_cause("Ros2Bridge: Failed to serialize payload", e))
97}
98
99fn decode_payload_with<Payload>(bytes: &[u8]) -> CuResult<Box<dyn Any>>
100where
101    Payload: CuMsgPayload + RosBridgeAdapter + 'static,
102{
103    let ros_payload: <Payload as RosBridgeAdapter>::RosMessage = cdr::deserialize(bytes)
104        .map_err(|e| CuError::new_with_cause("Ros2Bridge: Failed to deserialize payload", e))?;
105    let payload = Payload::from_ros_message(ros_payload).map_err(CuError::from)?;
106    Ok(Box::new(payload))
107}
108
109fn payload_registry() -> &'static RosPayloadRegistry {
110    static REGISTRY: OnceLock<RosPayloadRegistry> = OnceLock::new();
111    REGISTRY.get_or_init(|| {
112        let registry = RosPayloadRegistry::new();
113        registry.register::<bool>();
114        registry.register::<i8>();
115        registry.register::<i16>();
116        registry.register::<i32>();
117        registry.register::<i64>();
118        registry.register::<u8>();
119        registry.register::<u16>();
120        registry.register::<u32>();
121        registry.register::<u64>();
122        registry.register::<f32>();
123        registry.register::<f64>();
124        registry.register::<String>();
125        registry
126    })
127}
128
129/// Register a payload codec for ROS2 bridge transport.
130///
131/// Call this once at application startup for custom payload types that implement
132/// [`cu_ros2_payloads::RosBridgeAdapter`].
133pub fn register_ros2_payload<Payload>()
134where
135    Payload: CuMsgPayload + RosBridgeAdapter + 'static,
136{
137    payload_registry().register::<Payload>();
138}
139
140/// Per-channel Rx queue strategy, configured via `queue_mode` in the channel config.
141/// - `"fifo"` (default): ordered delivery, one sample per cycle.
142/// - `"ring"`: creates a buffer and drops the oldest sample when full; set `ring_size` (default 1) for buffer depth.
143#[derive(Debug, Clone)]
144enum RxQueueConfig {
145    Fifo,
146    Ring { size: usize },
147}
148
149impl RxQueueConfig {
150    fn from_config(config: Option<&ComponentConfig>) -> CuResult<Self> {
151        let Some(cfg) = config else {
152            return Ok(Self::Fifo);
153        };
154        match cfg.get::<String>("queue_mode")?.as_deref() {
155            Some("ring") => Ok(Self::Ring {
156                size: cfg.get::<u32>("ring_size")?.unwrap_or(1) as usize,
157            }),
158            Some("fifo") | None => Ok(Self::Fifo),
159            Some(other) => Err(CuError::from(format!(
160                "Ros2Bridge: unknown queue_mode '{other}'"
161            ))),
162        }
163    }
164}
165
166#[derive(Debug, Clone)]
167struct Ros2TxChannelConfig<Id: Copy> {
168    id: Id,
169    route: String,
170}
171
172#[derive(Debug, Clone)]
173struct Ros2RxChannelConfig<Id: Copy> {
174    id: Id,
175    route: String,
176    queue: RxQueueConfig,
177}
178
179enum Ros2Subscriber {
180    Fifo(zenoh::pubsub::Subscriber<zenoh::handlers::FifoChannelHandler<zenoh::sample::Sample>>),
181    Ring(zenoh::pubsub::Subscriber<zenoh::handlers::RingChannelHandler<zenoh::sample::Sample>>),
182}
183
184struct Ros2TxChannel<Id: Copy> {
185    id: Id,
186    route: String,
187    entity_id: u32,
188    sequence_number: u64,
189    publisher: Option<zenoh::pubsub::Publisher<'static>>,
190    publisher_token: Option<zenoh::liveliness::LivelinessToken>,
191}
192
193struct Ros2RxChannel<Id: Copy> {
194    id: Id,
195    route: String,
196    entity_id: u32,
197    queue: RxQueueConfig,
198    subscriber: Option<Ros2Subscriber>,
199    subscriber_token: Option<zenoh::liveliness::LivelinessToken>,
200}
201
202struct Ros2Context<TxId: Copy, RxId: Copy> {
203    session: zenoh::Session,
204    #[allow(dead_code)]
205    node_token: zenoh::liveliness::LivelinessToken,
206    tx_channels: Vec<Ros2TxChannel<TxId>>,
207    rx_channels: Vec<Ros2RxChannel<RxId>>,
208}
209
210#[derive(Reflect)]
211#[reflect(from_reflect = false, no_field_bounds, type_path = false)]
212pub struct Ros2Bridge<Tx, Rx>
213where
214    Tx: BridgeChannelSet + 'static,
215    Rx: BridgeChannelSet + 'static,
216    Tx::Id: Send + Sync + 'static,
217    Rx::Id: Send + Sync + 'static,
218{
219    #[reflect(ignore)]
220    session_config: Config,
221    domain_id: u32,
222    namespace: String,
223    node: String,
224    #[reflect(ignore)]
225    tx_channels: Vec<Ros2TxChannelConfig<Tx::Id>>,
226    #[reflect(ignore)]
227    rx_channels: Vec<Ros2RxChannelConfig<Rx::Id>>,
228    #[reflect(ignore)]
229    ctx: Option<Ros2Context<Tx::Id, Rx::Id>>,
230}
231
232impl<Tx, Rx> Freezable for Ros2Bridge<Tx, Rx>
233where
234    Tx: BridgeChannelSet + 'static,
235    Rx: BridgeChannelSet + 'static,
236    Tx::Id: Send + Sync + 'static,
237    Rx::Id: Send + Sync + 'static,
238{
239}
240
241impl<Tx, Rx> cu29::reflect::TypePath for Ros2Bridge<Tx, Rx>
242where
243    Tx: BridgeChannelSet + 'static,
244    Rx: BridgeChannelSet + 'static,
245    Tx::Id: Send + Sync + 'static,
246    Rx::Id: Send + Sync + 'static,
247{
248    fn type_path() -> &'static str {
249        "cu_ros2_bridge::Ros2Bridge"
250    }
251
252    fn short_type_path() -> &'static str {
253        "Ros2Bridge"
254    }
255
256    fn type_ident() -> Option<&'static str> {
257        Some("Ros2Bridge")
258    }
259
260    fn crate_name() -> Option<&'static str> {
261        Some("cu_ros2_bridge")
262    }
263
264    fn module_path() -> Option<&'static str> {
265        Some("cu_ros2_bridge")
266    }
267}
268
269impl<Tx, Rx> Ros2Bridge<Tx, Rx>
270where
271    Tx: BridgeChannelSet + 'static,
272    Rx: BridgeChannelSet + 'static,
273    Tx::Id: Send + Sync + 'static,
274    Rx::Id: Send + Sync + 'static,
275{
276    fn parse_session_config(config: &ComponentConfig) -> CuResult<Config> {
277        if let Some(path) = config.get::<String>("zenoh_config_file")? {
278            return Config::from_file(&path).map_err(|e| {
279                CuError::from(format!("Ros2Bridge: Failed to read config file: {e}"))
280            });
281        }
282        if let Some(json) = config.get::<String>("zenoh_config_json")? {
283            return Config::from_json5(&json).map_err(|e| {
284                CuError::from(format!("Ros2Bridge: Failed to parse config json: {e}"))
285            });
286        }
287        Ok(Config::default())
288    }
289
290    fn parse_domain_id(config: &ComponentConfig) -> CuResult<u32> {
291        Ok(config.get::<u32>("domain_id")?.unwrap_or(0))
292    }
293
294    fn parse_namespace(config: &ComponentConfig) -> CuResult<String> {
295        Ok(config
296            .get::<String>("namespace")?
297            .unwrap_or_else(|| "copper".to_string()))
298    }
299
300    fn parse_node_name(config: &ComponentConfig) -> CuResult<String> {
301        Ok(config
302            .get::<String>("node")?
303            .unwrap_or_else(|| "node".to_string()))
304    }
305
306    fn channel_route<Id: Copy + core::fmt::Debug>(
307        channel: &BridgeChannelConfig<Id>,
308    ) -> CuResult<String> {
309        channel
310            .effective_route()
311            .map(|route| route.into_owned())
312            .ok_or_else(|| {
313                let id = channel.channel.id;
314                CuError::from(format!(
315                    "Ros2Bridge: Missing route/topic for channel {:?}",
316                    id
317                ))
318            })
319    }
320
321    fn make_node<'a>(
322        domain_id: u32,
323        namespace: &'a str,
324        node_name: &'a str,
325        session: &'a zenoh::Session,
326    ) -> Node<'a> {
327        Node {
328            domain_id,
329            zid: session.zid(),
330            id: NODE_ID,
331            namespace,
332            name: node_name,
333        }
334    }
335
336    fn find_tx_channel_index(channels: &[Ros2TxChannel<Tx::Id>], id: Tx::Id) -> Option<usize> {
337        channels.iter().position(|channel| channel.id == id)
338    }
339
340    fn find_rx_channel_index(channels: &[Ros2RxChannel<Rx::Id>], id: Rx::Id) -> Option<usize> {
341        channels.iter().position(|channel| channel.id == id)
342    }
343
344    fn codec_for_payload<Payload>() -> CuResult<RosPayloadCodec>
345    where
346        Payload: CuMsgPayload + 'static,
347    {
348        payload_registry().codec_for::<Payload>()
349    }
350
351    fn topic_for_codec<'a>(route: &'a str, codec: RosPayloadCodec) -> Topic<'a> {
352        Topic::from_ros_type(route, codec.namespace, codec.type_name, codec.type_hash)
353    }
354
355    fn encode_payload<Payload>(msg: &CuMsg<Payload>, codec: RosPayloadCodec) -> CuResult<Vec<u8>>
356    where
357        Payload: CuMsgPayload + 'static,
358    {
359        let payload = msg
360            .payload()
361            .ok_or_else(|| CuError::from("Ros2Bridge: Cannot send empty payload through bridge"))?;
362        (codec.encode_payload)(payload as &dyn Any)
363    }
364
365    fn decode_payload_into<Payload>(
366        bytes: &[u8],
367        msg: &mut CuMsg<Payload>,
368        codec: RosPayloadCodec,
369    ) -> CuResult<()>
370    where
371        Payload: CuMsgPayload + 'static,
372    {
373        let payload_any = (codec.decode_payload)(bytes)?;
374        let payload = payload_any.downcast::<Payload>().map_err(|_| {
375            CuError::from(format!(
376                "Ros2Bridge: Codec produced wrong payload type for {}",
377                type_name::<Payload>()
378            ))
379        })?;
380        msg.set_payload(*payload);
381        Ok(())
382    }
383
384    fn init_tx_channel(
385        domain_id: u32,
386        namespace: &str,
387        node_name: &str,
388        ctx: &mut Ros2Context<Tx::Id, Rx::Id>,
389        tx_idx: usize,
390        codec: RosPayloadCodec,
391    ) -> CuResult<()> {
392        if ctx.tx_channels[tx_idx].publisher.is_some() {
393            return Ok(());
394        }
395
396        let route = ctx.tx_channels[tx_idx].route.clone();
397        let entity_id = ctx.tx_channels[tx_idx].entity_id;
398        let topic = Self::topic_for_codec(route.as_str(), codec);
399        let node = Self::make_node(domain_id, namespace, node_name, &ctx.session);
400
401        let publisher_token = zenoh::Wait::wait(
402            ctx.session
403                .liveliness()
404                .declare_token(publisher_liveliness(&node, &topic, entity_id)?),
405        )
406        .map_err(cu_error_map(
407            "Ros2Bridge: Failed to declare topic liveliness token",
408        ))?;
409
410        let publisher =
411            zenoh::Wait::wait(ctx.session.declare_publisher(topic.pubsub_keyexpr(&node)?))
412                .map_err(cu_error_map("Ros2Bridge: Failed to create publisher"))?;
413
414        ctx.tx_channels[tx_idx].publisher = Some(publisher);
415        ctx.tx_channels[tx_idx].publisher_token = Some(publisher_token);
416
417        Ok(())
418    }
419
420    fn init_rx_channel(
421        domain_id: u32,
422        namespace: &str,
423        node_name: &str,
424        ctx: &mut Ros2Context<Tx::Id, Rx::Id>,
425        rx_idx: usize,
426        codec: RosPayloadCodec,
427    ) -> CuResult<()> {
428        if ctx.rx_channels[rx_idx].subscriber.is_some() {
429            return Ok(());
430        }
431
432        let route = ctx.rx_channels[rx_idx].route.clone();
433        let entity_id = ctx.rx_channels[rx_idx].entity_id;
434        let topic = Self::topic_for_codec(route.as_str(), codec);
435        let node = Self::make_node(domain_id, namespace, node_name, &ctx.session);
436
437        let subscriber_token = zenoh::Wait::wait(
438            ctx.session
439                .liveliness()
440                .declare_token(subscriber_liveliness(&node, &topic, entity_id)?),
441        )
442        .map_err(cu_error_map(
443            "Ros2Bridge: Failed to declare subscriber liveliness token",
444        ))?;
445
446        let keyexpr = topic.pubsub_keyexpr(&node)?;
447        let subscriber = match ctx.rx_channels[rx_idx].queue {
448            RxQueueConfig::Fifo => Ros2Subscriber::Fifo(
449                zenoh::Wait::wait(ctx.session.declare_subscriber(keyexpr))
450                    .map_err(cu_error_map("Ros2Bridge: Failed to declare subscriber"))?,
451            ),
452            RxQueueConfig::Ring { size } => Ros2Subscriber::Ring(
453                zenoh::Wait::wait(
454                    ctx.session
455                        .declare_subscriber(keyexpr)
456                        .with(zenoh::handlers::RingChannel::new(size)),
457                )
458                .map_err(cu_error_map("Ros2Bridge: Failed to declare subscriber"))?,
459            ),
460        };
461
462        ctx.rx_channels[rx_idx].subscriber_token = Some(subscriber_token);
463        ctx.rx_channels[rx_idx].subscriber = Some(subscriber);
464        Ok(())
465    }
466
467    fn ctx_mut(&mut self) -> CuResult<&mut Ros2Context<Tx::Id, Rx::Id>> {
468        self.ctx
469            .as_mut()
470            .ok_or_else(|| CuError::from("Ros2Bridge: Context not initialized"))
471    }
472}
473
474impl<Tx, Rx> CuBridge for Ros2Bridge<Tx, Rx>
475where
476    Tx: BridgeChannelSet + 'static,
477    Rx: BridgeChannelSet + 'static,
478    Tx::Id: core::fmt::Debug + Send + Sync + 'static,
479    Rx::Id: core::fmt::Debug + Send + Sync + 'static,
480{
481    type Tx = Tx;
482    type Rx = Rx;
483    type Resources<'r> = ();
484
485    fn new(
486        config: Option<&ComponentConfig>,
487        tx_channels: &[BridgeChannelConfig<<Self::Tx as BridgeChannelSet>::Id>],
488        rx_channels: &[BridgeChannelConfig<<Self::Rx as BridgeChannelSet>::Id>],
489        _resources: Self::Resources<'_>,
490    ) -> CuResult<Self>
491    where
492        Self: Sized,
493    {
494        let default_config = ComponentConfig::default();
495        let config = config.unwrap_or(&default_config);
496
497        let session_config = Self::parse_session_config(config)?;
498        let domain_id = Self::parse_domain_id(config)?;
499        let namespace = Self::parse_namespace(config)?;
500        let node = Self::parse_node_name(config)?;
501
502        let mut tx_cfgs = Vec::with_capacity(tx_channels.len());
503        for channel in tx_channels {
504            let route = Self::channel_route(channel)?;
505            tx_cfgs.push(Ros2TxChannelConfig {
506                id: channel.channel.id,
507                route,
508            });
509        }
510
511        let mut rx_cfgs = Vec::with_capacity(rx_channels.len());
512        for channel in rx_channels {
513            let route = Self::channel_route(channel)?;
514            let queue = RxQueueConfig::from_config(channel.config.as_ref())?;
515            rx_cfgs.push(Ros2RxChannelConfig {
516                id: channel.channel.id,
517                route,
518                queue,
519            });
520        }
521
522        Ok(Self {
523            session_config,
524            domain_id,
525            namespace,
526            node,
527            tx_channels: tx_cfgs,
528            rx_channels: rx_cfgs,
529            ctx: None,
530        })
531    }
532
533    fn start(&mut self, _ctx: &CuContext) -> CuResult<()> {
534        let session = zenoh::Wait::wait(zenoh::open(self.session_config.clone()))
535            .map_err(cu_error_map("Ros2Bridge: Failed to open session"))?;
536
537        let node = Self::make_node(self.domain_id, &self.namespace, &self.node, &session);
538        let node_token =
539            zenoh::Wait::wait(session.liveliness().declare_token(node_liveliness(&node)?))
540                .map_err(cu_error_map(
541                    "Ros2Bridge: Failed to declare node liveliness token",
542                ))?;
543
544        let tx_channels = self
545            .tx_channels
546            .iter()
547            .enumerate()
548            .map(|(index, channel)| Ros2TxChannel {
549                id: channel.id,
550                route: channel.route.clone(),
551                entity_id: (index + 1) as u32,
552                sequence_number: 0,
553                publisher: None,
554                publisher_token: None,
555            })
556            .collect();
557
558        let rx_channels = self
559            .rx_channels
560            .iter()
561            .enumerate()
562            .map(|(index, channel)| Ros2RxChannel {
563                id: channel.id,
564                route: channel.route.clone(),
565                entity_id: (index + 1) as u32,
566                queue: channel.queue.clone(),
567                subscriber: None,
568                subscriber_token: None,
569            })
570            .collect();
571
572        self.ctx = Some(Ros2Context {
573            session,
574            node_token,
575            tx_channels,
576            rx_channels,
577        });
578
579        Ok(())
580    }
581
582    fn send<'a, Payload>(
583        &mut self,
584        ctx: &CuContext,
585        channel: &'static BridgeChannel<<Self::Tx as BridgeChannelSet>::Id, Payload>,
586        msg: &CuMsg<Payload>,
587    ) -> CuResult<()>
588    where
589        Payload: CuMsgPayload + 'a,
590    {
591        let codec = Self::codec_for_payload::<Payload>()?;
592        let domain_id = self.domain_id;
593        let namespace = self.namespace.clone();
594        let node_name = self.node.clone();
595        let channel_id = channel.id();
596        let bridge_ctx = self.ctx_mut()?;
597
598        let tx_idx =
599            Self::find_tx_channel_index(&bridge_ctx.tx_channels, channel_id).ok_or_else(|| {
600                CuError::from(format!("Ros2Bridge: Unknown Tx channel {:?}", channel_id))
601            })?;
602
603        Self::init_tx_channel(domain_id, &namespace, &node_name, bridge_ctx, tx_idx, codec)?;
604
605        let encoded = Self::encode_payload(msg, codec)?;
606        let session_zid = bridge_ctx.session.zid();
607
608        let tx_channel = &mut bridge_ctx.tx_channels[tx_idx];
609        let publisher = tx_channel
610            .publisher
611            .as_mut()
612            .ok_or_else(|| CuError::from("Ros2Bridge: Tx publisher not initialized"))?;
613
614        let attachment = encode_attachment(tx_channel.sequence_number, ctx, &session_zid);
615        tx_channel.sequence_number += 1;
616
617        zenoh::Wait::wait(
618            publisher
619                .put(encoded)
620                .encoding(Encoding::APPLICATION_CDR)
621                .attachment(attachment),
622        )
623        .map_err(cu_error_map("Ros2Bridge: Failed to put value"))?;
624
625        Ok(())
626    }
627
628    fn receive<'a, Payload>(
629        &mut self,
630        ctx: &CuContext,
631        channel: &'static BridgeChannel<<Self::Rx as BridgeChannelSet>::Id, Payload>,
632        msg: &mut CuMsg<Payload>,
633    ) -> CuResult<()>
634    where
635        Payload: CuMsgPayload + 'a,
636    {
637        let codec = Self::codec_for_payload::<Payload>()?;
638        let domain_id = self.domain_id;
639        let namespace = self.namespace.clone();
640        let node_name = self.node.clone();
641        let channel_id = channel.id();
642        let bridge_ctx = self.ctx_mut()?;
643
644        let rx_idx =
645            Self::find_rx_channel_index(&bridge_ctx.rx_channels, channel_id).ok_or_else(|| {
646                CuError::from(format!("Ros2Bridge: Unknown Rx channel {:?}", channel_id))
647            })?;
648
649        Self::init_rx_channel(domain_id, &namespace, &node_name, bridge_ctx, rx_idx, codec)?;
650
651        msg.tov = Tov::Time(ctx.now());
652
653        let subscriber = bridge_ctx.rx_channels[rx_idx]
654            .subscriber
655            .as_mut()
656            .ok_or_else(|| CuError::from("Ros2Bridge: Rx subscriber not initialized"))?;
657        let sample = match subscriber {
658            Ros2Subscriber::Fifo(s) => s.try_recv(),
659            Ros2Subscriber::Ring(s) => s.try_recv(),
660        }
661        .map_err(|e| CuError::from(format!("Ros2Bridge: receive failed: {e}")))?;
662
663        if let Some(sample) = sample {
664            let payload = sample.payload().to_bytes();
665            Self::decode_payload_into(payload.as_ref(), msg, codec)?;
666        } else {
667            msg.clear_payload();
668        }
669
670        Ok(())
671    }
672
673    fn stop(&mut self, _ctx: &CuContext) -> CuResult<()> {
674        if let Some(Ros2Context {
675            session,
676            node_token: _,
677            tx_channels,
678            rx_channels,
679        }) = self.ctx.take()
680        {
681            for channel in tx_channels {
682                if let Some(publisher) = channel.publisher {
683                    zenoh::Wait::wait(publisher.undeclare())
684                        .map_err(cu_error_map("Ros2Bridge: Failed to undeclare publisher"))?;
685                }
686            }
687
688            for channel in rx_channels {
689                if let Some(subscriber) = channel.subscriber {
690                    match subscriber {
691                        Ros2Subscriber::Fifo(s) => zenoh::Wait::wait(s.undeclare())
692                            .map_err(cu_error_map("Ros2Bridge: Failed to undeclare subscriber"))?,
693                        Ros2Subscriber::Ring(s) => zenoh::Wait::wait(s.undeclare())
694                            .map_err(cu_error_map("Ros2Bridge: Failed to undeclare subscriber"))?,
695                    }
696                }
697            }
698
699            zenoh::Wait::wait(session.close())
700                .map_err(cu_error_map("Ros2Bridge: Failed to close session"))?;
701        }
702
703        Ok(())
704    }
705}
706
707fn cu_error_map(msg: &str) -> impl FnOnce(ZenohError) -> CuError + '_ {
708    move |e| CuError::from(format!("{msg}: {e}"))
709}
710
711#[cfg(test)]
712mod tests {
713    use super::*;
714    use cu29::bincode::{Decode, Encode};
715    use serde::{Deserialize, Serialize};
716
717    #[derive(Clone, Debug, Default, Encode, Decode, Serialize, Deserialize, Reflect)]
718    struct RejectingPayload;
719
720    impl RosBridgeAdapter for RejectingPayload {
721        type RosMessage = i32;
722
723        fn validate_ros_message(&self) -> Result<(), String> {
724            Err("test payload rejected".to_string())
725        }
726
727        fn namespace() -> &'static str {
728            "test_msgs"
729        }
730
731        fn type_hash() -> &'static str {
732            "test_hash"
733        }
734
735        fn to_ros_message(&self) -> Self::RosMessage {
736            0
737        }
738
739        fn from_ros_message(_msg: Self::RosMessage) -> Result<Self, String> {
740            Ok(Self)
741        }
742    }
743
744    #[test]
745    fn payload_validation_failure_stops_encoding() {
746        let error = encode_payload_with::<RejectingPayload>(&RejectingPayload)
747            .expect_err("validation failure should stop ROS encoding");
748
749        assert!(error.to_string().contains("test payload rejected"));
750    }
751
752    #[test]
753    fn scalar_payload_cdr_roundtrip_uses_little_endian_encapsulation() {
754        let mut src = CuMsg::<i32>::default();
755        src.set_payload(0x0102_0304);
756
757        let codec =
758            Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i32>()
759                .expect("codec should be registered");
760        let bytes =
761            Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::encode_payload(&src, codec)
762                .expect("encode should succeed");
763
764        assert_eq!(&bytes[..4], &[0, 1, 0, 0], "CDR_LE encapsulation");
765        assert_eq!(&bytes[4..8], &[4, 3, 2, 1], "little-endian i32 data");
766
767        let mut dst = CuMsg::<i32>::default();
768        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::decode_payload_into(
769            bytes.as_slice(),
770            &mut dst,
771            codec,
772        )
773        .expect("decode should succeed");
774
775        assert_eq!(dst.payload(), Some(&0x0102_0304));
776    }
777
778    #[test]
779    fn default_scalar_codecs_registered() {
780        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<bool>()
781            .expect("bool codec");
782        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i8>()
783            .expect("i8 codec");
784        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i16>()
785            .expect("i16 codec");
786        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i32>()
787            .expect("i32 codec");
788        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<i64>()
789            .expect("i64 codec");
790        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<u8>()
791            .expect("u8 codec");
792        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<u16>()
793            .expect("u16 codec");
794        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<u32>()
795            .expect("u32 codec");
796        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<u64>()
797            .expect("u64 codec");
798        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<f32>()
799            .expect("f32 codec");
800        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<f64>()
801            .expect("f64 codec");
802        Ros2Bridge::<crate::tests::DummyTx, crate::tests::DummyRx>::codec_for_payload::<String>()
803            .expect("string codec");
804    }
805
806    tx_channels! {
807        struct DummyTx : DummyTxId {
808            out => i8,
809        }
810    }
811
812    rx_channels! {
813        struct DummyRx : DummyRxId {
814            input => i8,
815        }
816    }
817}
cu_ros2_bridge/lib.rs

cu_ros2_bridge/
lib.rs
