nodo 0.18.5

A realtime framework for robotics
Documentation
use crate::prelude::Pubtime;
use serde::{Deserialize, Serialize};
use std::{collections::HashMap, fmt::Debug, hash::Hash};

/// Trait for configuration types used by codelets.
pub trait Config {
    /// Enum auto-generated by nodo with one variant for each parameter
    type Kind: ConfigKind;

    /// Auxilary type which holds information about when parameters where last modified.
    type Aux: ConfigAux<Kind = Self::Kind>;

    /// List all parameters and their properties
    fn list_parameters() -> &'static [(Self::Kind, ParameterProperties)];

    /// Get values of all parameters
    fn get_parameters(&self) -> Vec<(Self::Kind, ParameterValue)>;

    /// Set a parameter
    fn set_parameter(
        &mut self,
        key: Self::Kind,
        value: ParameterValue,
    ) -> Result<(), ConfigSetParameterError>;

    /// Set multiple parameters
    fn set_parameters<I>(&mut self, items: I) -> Result<(), ConfigSetParameterError>
    where
        I: IntoIterator<Item = (Self::Kind, ParameterValue)>,
    {
        for (key, value) in items.into_iter() {
            self.set_parameter(key, value)?;
        }
        Ok(())
    }
}

/// Trait for auto-generated enums which enumerate parameters
pub trait ConfigKind: 'static + Copy + PartialEq + Eq + Hash {
    /// A string representation of an enum variant
    fn as_str(self) -> &'static str;

    /// Parses the enum variant from a string representation
    fn from_str(id: &str) -> Option<Self>;

    /// Like as_str but as an owned string
    fn to_string(self) -> String {
        self.as_str().into()
    }
}

/// An enum without variantes (which can thus not be instantiated)
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum EmptyEnum {}

impl ConfigKind for EmptyEnum {
    fn from_str(_id: &str) -> Option<Self> {
        None
    }

    fn as_str(self) -> &'static str {
        unreachable!()
    }
}

impl Config for () {
    type Kind = EmptyEnum;

    type Aux = ();

    fn list_parameters() -> &'static [(Self::Kind, ParameterProperties)] {
        &[]
    }

    fn get_parameters(&self) -> Vec<(Self::Kind, ParameterValue)> {
        Vec::new()
    }

    fn set_parameter<'a>(
        &mut self,
        _key: Self::Kind,
        _value: ParameterValue,
    ) -> Result<(), ConfigSetParameterError> {
        // EmptyEnum does not have variants
        unreachable!()
    }
}

/// Trait for auto-generated auxilary configy types which holds information about when parameters
/// were last modified.
pub trait ConfigAux: Send + Default {
    /// Config parameter identification enum
    type Kind: ConfigKind;

    /// Lists parameters which where modified since the last step
    fn dirty(&self) -> &[Self::Kind];

    /// Returns true if a parameter was modified since the last step
    fn is_dirty(&self) -> bool;

    /// Internal function called by the nodo runtime when a parameter is changed
    fn on_set_parameter(&mut self, key: Self::Kind, now: Pubtime);

    /// Internal function called by the nodo runtime after each step
    fn on_post_step(&mut self);
}

impl ConfigAux for () {
    type Kind = EmptyEnum;

    fn dirty(&self) -> &[Self::Kind] {
        &[]
    }

    fn is_dirty(&self) -> bool {
        false
    }

    fn on_set_parameter(&mut self, _key: Self::Kind, _now: Pubtime) {
        // EmptyEnum does not have variants
        unreachable!()
    }

    fn on_post_step(&mut self) {}
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)]
pub enum ParameterDataType {
    Bool,
    Int64,
    Usize,
    Float64,
    String,
    VecFloat64,
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)]
pub struct ParameterProperties {
    pub dtype: ParameterDataType,
    pub is_mutable: bool,
}

#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
pub enum ParameterValue {
    Bool(bool),
    Int64(i64),
    Usize(usize),
    Float64(f64),
    String(String),
    VecFloat64(Vec<f64>),
}

impl ParameterValue {
    pub fn dtype(&self) -> ParameterDataType {
        match self {
            ParameterValue::Bool(_) => ParameterDataType::Bool,
            ParameterValue::Int64(_) => ParameterDataType::Int64,
            ParameterValue::Usize(_) => ParameterDataType::Usize,
            ParameterValue::Float64(_) => ParameterDataType::Float64,
            ParameterValue::String(_) => ParameterDataType::String,
            ParameterValue::VecFloat64(_) => ParameterDataType::VecFloat64,
        }
    }
}

/// Macro to generate From implementations for ParameterValue
macro_rules! impl_into_config_value {
    ($($type:ty => $variant:ident),* $(,)?) => {
        $(
            impl From<$type> for ParameterValue {
                fn from(other: $type) -> Self {
                    ParameterValue::$variant(other)
                }
            }
        )*
    };
}

// Use the macro to implement From for all supported types
impl_into_config_value! {
    bool => Bool,
    i64 => Int64,
    usize => Usize,
    f64 => Float64,
    String => String,
    Vec<f64> => VecFloat64,
}

impl<const N: usize> From<[f64; N]> for ParameterValue {
    fn from(other: [f64; N]) -> Self {
        ParameterValue::VecFloat64(other.to_vec())
    }
}

pub trait ParameterAssignmentHelper {
    fn assign(self) -> Result<(), ParameterAssignmentError>;
}

impl<'a, T> ParameterAssignmentHelper for (&'a mut T, T) {
    fn assign(self) -> Result<(), ParameterAssignmentError> {
        *self.0 = self.1;
        Ok(())
    }
}

impl<'a, const N: usize> ParameterAssignmentHelper for (&'a mut [f64; N], Vec<f64>) {
    fn assign(self) -> Result<(), ParameterAssignmentError> {
        if self.1.len() != N {
            Err(ParameterAssignmentError::InvalidLength {
                expected: N,
                actual: self.1.len(),
            })
        } else {
            for i in 0..N {
                self.0[i] = self.1[i];
            }
            Ok(())
        }
    }
}

#[derive(thiserror::Error, Debug, PartialEq)]
pub enum ParameterAssignmentError {
    #[error("the value had an invalid length: expected={expected}, actual={actual}")]
    InvalidLength { expected: usize, actual: usize },
}

impl From<ParameterAssignmentError> for ConfigSetParameterError {
    fn from(other: ParameterAssignmentError) -> Self {
        ConfigSetParameterError::AssignmentFailed(other)
    }
}

#[derive(thiserror::Error, Debug, PartialEq)]
pub enum ConfigSetParameterError {
    #[error("cannot modify immutable parameter")]
    Immutable,

    #[error("assignment failed: {0:?}")]
    AssignmentFailed(ParameterAssignmentError),

    #[error("invalid type: expected {expected:?}, got {actual:?}")]
    InvalidType {
        expected: ParameterDataType,
        actual: ParameterDataType,
    },
}

#[derive(Default, Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)]
pub struct ParameterId<A, B>(pub A, pub B);

impl<A, B> ParameterId<A, B> {
    pub fn node(&self) -> &A {
        &self.0
    }

    pub fn param(&self) -> &B {
        &self.1
    }
}

pub type ParameterSet<A, B> = ParameterSetImpl<A, B, ParameterValue>;

pub type ParameterWithPropertiesSet<A, B> =
    ParameterSetImpl<A, B, (ParameterProperties, ParameterValue)>;

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct ParameterSetImpl<A, B, T>(pub HashMap<ParameterId<A, B>, T>)
where
    A: PartialEq + Eq + Hash,
    B: PartialEq + Eq + Hash;

impl<A, B, T> Default for ParameterSetImpl<A, B, T>
where
    A: PartialEq + Eq + Hash,
    B: PartialEq + Eq + Hash,
{
    fn default() -> Self {
        Self(HashMap::new())
    }
}

impl<A, B, T> ParameterSetImpl<A, B, T>
where
    A: PartialEq + Eq + Hash,
    B: PartialEq + Eq + Hash,
{
    pub fn new() -> Self {
        Self(HashMap::new())
    }

    pub fn from_array<const N: usize>(arr: [(A, B, T); N]) -> Self {
        Self(
            arr.into_iter()
                .map(|(a, b, v)| (ParameterId(a, b), v))
                .collect(),
        )
    }

    pub fn from_iter<I>(iter: I) -> Self
    where
        I: IntoIterator<Item = (A, B, T)>,
    {
        Self(
            iter.into_iter()
                .map(|(a, b, v)| (ParameterId(a, b), v))
                .collect(),
        )
    }

    pub fn get(&self, id: &ParameterId<A, B>) -> Option<&T> {
        self.0.get(id)
    }

    pub fn iter(&self) -> impl Iterator<Item = (&ParameterId<A, B>, &T)> {
        self.0.iter()
    }

    pub fn map_into<A2, B2, T2, F>(self, f: F) -> ParameterSetImpl<A2, B2, T2>
    where
        A2: PartialEq + Eq + Hash,
        B2: PartialEq + Eq + Hash,
        F: Fn((ParameterId<A, B>, T)) -> (ParameterId<A2, B2>, T2),
    {
        ParameterSetImpl(self.0.into_iter().map(|(k, v)| f((k, v))).collect())
    }

    pub fn insert(&mut self, id: ParameterId<A, B>, value: T) {
        self.0.insert(id, value);
    }

    pub fn extend(&mut self, other: Self) {
        for (k, v) in other.0.into_iter() {
            self.0.insert(k, v);
        }
    }
}

impl<'a, A1, T1, A2, T2> From<ParameterSetImpl<A1, &'a str, T1>>
    for ParameterSetImpl<A2, String, T2>
where
    A1: PartialEq + Eq + Hash,
    A2: PartialEq + Eq + Hash + From<A1>,
    T2: From<T1>,
{
    fn from(other: ParameterSetImpl<A1, &'a str, T1>) -> Self {
        other.map_into(|(ParameterId(a, b), v)| (ParameterId(a.into(), b.into()), v.into()))
    }
}

/// Auxilary type used to indicate when a parameter was last changed
#[derive(Default)]
pub struct ParameterAux {
    is_dirty: bool,
    changed_status: ChangedStatus,
}

impl ParameterAux {
    /// Returns true if a parameter was modified since the last step
    pub fn is_dirty(&self) -> bool {
        self.is_dirty
    }

    /// Gets information about when the parameter was last changed
    pub fn status(&self) -> &ChangedStatus {
        &self.changed_status
    }

    /// Internal function called by the nodo runtime when a parameter is changed
    pub fn on_set_parameter(&mut self, now: Pubtime) {
        self.is_dirty = true;
        self.changed_status = ChangedStatus::Changed(now);
    }

    /// Internal function called by the nodo runtime after each step
    pub fn on_post_step(&mut self) {
        self.is_dirty = false;
    }
}

/// Describes when a parameter was last changed
#[derive(Default)]
pub enum ChangedStatus {
    /// The parameter was never changed and has the value originally loaded from configuration.
    #[default]
    Original,

    /// The parameter was changed live at the given time
    Changed(Pubtime),
}