cu-zenoh-sink 0.10.0

Copper sink task for Zenoh.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114

use cu29::clock::RobotClock;
use cu29::{bincode, prelude::*};

use zenoh::key_expr::KeyExpr;
use zenoh::Config;
use zenoh::Error as ZenohError;

use std::marker::PhantomData;

/// This is a sink task that sends messages to a zenoh topic.
/// P is the payload type of the messages.
/// Copper messages and Zenoh payloads are compatible.
pub struct ZenohSink<P>
where
    P: CuMsgPayload,
{
    _marker: PhantomData<P>,
    config: ZenohConfig,
    ctx: Option<ZenohContext>,
}

pub struct ZenohConfig {
    config: Config,
    topic: String,
}

pub struct ZenohContext {
    session: zenoh::Session,
    publisher: zenoh::pubsub::Publisher<'static>,
}

fn cu_error(msg: &str, error: ZenohError) -> CuError {
    CuError::new_with_cause(msg, error.as_ref())
}

fn cu_error_map(msg: &str) -> impl FnOnce(ZenohError) -> CuError + '_ {
    |e| cu_error(msg, e)
}

impl<P> Freezable for ZenohSink<P> where P: CuMsgPayload {}

impl<P> CuSinkTask for ZenohSink<P>
where
    P: CuMsgPayload + 'static,
{
    type Input<'m> = input_msg!(P);

    fn new(config: Option<&ComponentConfig>) -> CuResult<Self>
    where
        Self: Sized,
    {
        let config = config.ok_or(CuError::from("ZenohSink: Missing configuration"))?;

        // Get json zenoh config
        let session_config = config.get::<String>("zenoh_config_file").map_or(
            // Or default zenoh config otherwise
            Ok(Config::default()),
            |s| -> CuResult<Config> {
                Config::from_file(&s)
                    .map_err(cu_error_map("ZenohSink: Failed to create zenoh config"))
            },
        )?;

        let topic = config.get::<String>("topic").unwrap_or("copper".to_owned());

        Ok(Self {
            _marker: Default::default(),
            config: ZenohConfig {
                config: session_config,
                topic,
            },
            ctx: None,
        })
    }

    fn start(&mut self, _clock: &RobotClock) -> CuResult<()> {
        let session = zenoh::Wait::wait(zenoh::open(self.config.config.clone()))
            .map_err(cu_error_map("ZenohSink: Failed to open session"))?;

        let key_expr = KeyExpr::<'static>::new(self.config.topic.clone())
            .map_err(cu_error_map("ZenohSink: Invalid topic string"))?;

        debug!("Zenoh session open");
        let publisher = zenoh::Wait::wait(session.declare_publisher(key_expr))
            .map_err(cu_error_map("ZenohSink: Failed to create publisher"))?;

        self.ctx = Some(ZenohContext { session, publisher });
        Ok(())
    }

    fn process(&mut self, _clock: &RobotClock, input: &Self::Input<'_>) -> CuResult<()> {
        let ctx = self
            .ctx
            .as_mut()
            .ok_or_else(|| CuError::from("ZenohSink: Context not found"))?;

        let encoded =
            bincode::encode_to_vec(input, bincode::config::standard()).expect("Encoding failed");
        zenoh::Wait::wait(ctx.publisher.put(encoded))
            .map_err(cu_error_map("ZenohSink: Failed to put value"))?;
        Ok(())
    }

    fn stop(&mut self, _clock: &RobotClock) -> CuResult<()> {
        if let Some(ZenohContext { session, publisher }) = self.ctx.take() {
            zenoh::Wait::wait(publisher.undeclare())
                .map_err(cu_error_map("ZenohSink: Failed to undeclare publisher"))?;
            zenoh::Wait::wait(session.close())
                .map_err(cu_error_map("ZenohSink: Failed to close session"))?;
        }
        debug!("ZenohSink: Stopped");
        Ok(())
    }
}