rosrustext_rosrs 0.4.1

rclrs adapter for rosrustext ROS 2 lifecycle + parameters parity
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
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137

use std::marker::PhantomData;
use std::sync::Arc;
use std::time::Duration;

use rclrs::{AnyTimerCallback, Clock, IntoNodeTimerRepeatingCallback, RclrsError, TimerClock, TimerOptions};

use super::{ActivationGate, ClockChoice, HasCallback, ManagedTimer, NoCallback};

/// Builder for lifecycle-managed repeating timers.
///
/// This owns all configuration data; no borrowed references are retained.
pub struct ManagedTimerBuilder<State> {
    node: Arc<rclrs::Node>,
    gate: Arc<ActivationGate>,
    period: Duration,
    clock: ClockChoice,
    callback: Option<AnyTimerCallback<rclrs::Node>>,
    _phantom: PhantomData<State>,
}

impl ManagedTimerBuilder<NoCallback> {
    pub(crate) fn new(node: Arc<rclrs::Node>, gate: Arc<ActivationGate>, period: Duration) -> Self {
        Self { node, gate, period, clock: ClockChoice::Steady, callback: None, _phantom: PhantomData }
    }
}

impl<State> ManagedTimerBuilder<State> {
    /// Replace period + clock from rclrs options.
    pub fn with_options<'a>(mut self, options: TimerOptions<'a>) -> Self {
        self.period = options.period;
        self.clock = match options.clock {
            TimerClock::SteadyTime => ClockChoice::Steady,
            TimerClock::SystemTime => ClockChoice::System,
            TimerClock::NodeTime => ClockChoice::Node,
            TimerClock::Clock(clock) => ClockChoice::Custom(clock.clone()),
        };
        self
    }

    /// Use steady time (default).
    pub fn steady_time(mut self) -> Self {
        self.clock = ClockChoice::Steady;
        self
    }

    /// Use system time.
    pub fn system_time(mut self) -> Self {
        self.clock = ClockChoice::System;
        self
    }

    /// Use node time.
    pub fn node_time(mut self) -> Self {
        self.clock = ClockChoice::Node;
        self
    }

    /// Use a specific clock.
    pub fn clock(mut self, clock: &Clock) -> Self {
        self.clock = ClockChoice::Custom(clock.clone());
        self
    }

    /// Set the repeating callback (erased and owned).
    pub fn callback<Args>(self, cb: impl IntoNodeTimerRepeatingCallback<Args>) -> ManagedTimerBuilder<HasCallback> {
        let erased = cb.into_node_timer_repeating_callback();
        ManagedTimerBuilder {
            node: self.node,
            gate: self.gate,
            period: self.period,
            clock: self.clock,
            callback: Some(erased),
            _phantom: PhantomData,
        }
    }
}

impl ManagedTimerBuilder<HasCallback> {
    /// Create the managed repeating timer.
    pub fn create(self) -> Result<ManagedTimer, RclrsError> {
        let ManagedTimerBuilder { node, gate, period, clock, callback, _phantom: _ } = self;

        let Some(callback) = callback else {
            unreachable!("callback must be set in HasCallback state");
        };

        let mut options = TimerOptions::new(period);
        options.clock = match &clock {
            ClockChoice::Steady => TimerClock::SteadyTime,
            ClockChoice::System => TimerClock::SystemTime,
            ClockChoice::Node => TimerClock::NodeTime,
            ClockChoice::Custom(clock) => TimerClock::Clock(clock),
        };

        let gated = wrap_gated_callback(callback, gate);
        let adapter = ErasedTimerCallback::new(gated);

        let timer = node.create_timer_repeating(options, adapter)?;
        Ok(ManagedTimer::new(timer))
    }
}

fn wrap_gated_callback(
    callback: AnyTimerCallback<rclrs::Node>, gate: Arc<ActivationGate>,
) -> AnyTimerCallback<rclrs::Node> {
    match callback {
        AnyTimerCallback::Repeating(mut cb) => AnyTimerCallback::Repeating(Box::new(move |payload, timer| {
            if gate.is_active() {
                cb(payload, timer);
            }
        })),
        AnyTimerCallback::OneShot(_) | AnyTimerCallback::Inert => {
            debug_assert!(false, "ManagedTimerBuilder expects repeating callbacks from IntoNodeTimerRepeatingCallback");
            AnyTimerCallback::Repeating(Box::new(move |_, _| {
                if gate.is_active() {
                    // No-op: repeating builder only supports repeating callbacks.
                }
            }))
        }
    }
}

struct ErasedTimerCallback {
    callback: AnyTimerCallback<rclrs::Node>,
}

impl ErasedTimerCallback {
    fn new(callback: AnyTimerCallback<rclrs::Node>) -> Self {
        Self { callback }
    }
}

impl IntoNodeTimerRepeatingCallback<()> for ErasedTimerCallback {
    fn into_node_timer_repeating_callback(self) -> AnyTimerCallback<rclrs::Node> {
        self.callback
    }
}