nodo 0.18.5

A realtime framework for robotics
Documentation
// Copyright 2023 David Weikersdorfer

use crate::{
    app::NodeInfo,
    codelet::{
        Clocks, Codelet, CodeletInstance, CodeletStatus, Lifecycle, NodeId, RxBundle,
        SharedNodeCrumbs, TaskClocks, Transition, TxBundle,
    },
    config::{Config, ConfigAux, ConfigKind},
    core::{DefaultStatus, OutcomeKind},
    monitors::SharedAppMonitor,
    prelude::{Clock, ConfigSetParameterError, ParameterValue, ParameterWithPropertiesSet},
    signals::Signals,
};
use eyre::Result;
use std::sync::Arc;

/// Wrapper around a node which hides the codelet type
pub struct Vise(pub(crate) Box<dyn ViseTrait>);

impl Vise {
    pub fn new<C: Codelet + 'static>(mut instance: CodeletInstance<C>) -> Self {
        instance.is_scheduled = true; // TODO is this the right location?
        Self(Box::new(instance))
    }
}

pub trait ViseTrait: Send + Lifecycle {
    /// Unique nodelet ID assigned by the runtime
    fn id(&self) -> NodeId;

    /// Nodelet name assignd by the user
    fn name(&self) -> &str;

    /// The type name of the codelet as given by Rust compiler
    fn type_name(&self) -> &str;

    fn rx_names(&self) -> Vec<&str>;

    fn tx_names(&self) -> Vec<&str>;

    fn signal_names(&self) -> Vec<&str>;

    /// Gets the status as a string and the corresponding simplified status
    #[deprecated]
    fn status(&self) -> Option<(String, DefaultStatus)>;

    /// Called once at the beginning to setup the clock
    fn setup(&mut self, setup: NodeletSetup);

    /// Get all configuration parameters and their properties
    fn get_parameters_with_properties(&self) -> ParameterWithPropertiesSet<(), &'static str>;

    /// Change a configuration parameter
    fn configure(&mut self, key: &str, value: &ParameterValue) -> Result<(), ViseConfigureError>;
}

/// Helper type used internally to setup a nodelet
pub struct NodeletSetup {
    pub info: Arc<NodeInfo>,
    pub clocks: Clocks,
    pub node_id: NodeId,
    pub monitor: SharedAppMonitor,
    pub crumbs: SharedNodeCrumbs,
}

#[derive(thiserror::Error, Debug)]
pub enum ViseConfigureError {
    #[error("invalid parameter name: {0:?}")]
    InvalidParameterName(String),

    #[error("failed to set parameter '{0:?}': {1:?}")]
    SetParameterFailed(String, ConfigSetParameterError),
}

impl<C: Codelet> ViseTrait for CodeletInstance<C> {
    fn id(&self) -> NodeId {
        self.id.unwrap()
    }

    fn name(&self) -> &str {
        &self.name
    }

    fn type_name(&self) -> &str {
        self.type_name()
    }

    fn rx_names(&self) -> Vec<&str> {
        self.rx.iter_names().collect()
    }

    fn tx_names(&self) -> Vec<&str> {
        self.tx.iter_names().collect()
    }

    fn signal_names(&self) -> Vec<&str> {
        <C as Codelet>::Signals::names().collect()
    }

    fn status(&self) -> Option<(String, DefaultStatus)> {
        self.status
            .as_ref()
            .map(|s| (s.label().to_string(), s.as_default_status()))
    }

    fn setup(&mut self, setup: NodeletSetup) {
        self.id = Some(setup.node_id);
        self.crumbs = Some(setup.crumbs);
        self.clocks = Some(TaskClocks::from(setup.clocks));
        setup.monitor.setup_node::<C>(setup.info, self).unwrap();
    }

    fn get_parameters_with_properties(&self) -> ParameterWithPropertiesSet<(), &'static str> {
        let props = <C::Config as Config>::list_parameters();
        let values = self.config.get_parameters();

        ParameterWithPropertiesSet::from_iter(props.iter().zip(values.iter()).map(
            |((k, p), (k2, v))| {
                if k2 != k {
                    unreachable!()
                }
                ((), k.as_str(), (p.clone(), v.clone()))
            },
        ))
    }

    fn configure(&mut self, key: &str, value: &ParameterValue) -> Result<(), ViseConfigureError> {
        if let Some(kind) = <C::Config as Config>::Kind::from_str(key) {
            match self.config.set_parameter(kind, value.clone()) {
                Ok(()) => {
                    let pubtime = self
                        .clocks
                        .as_ref()
                        .expect("clocks must be initialized")
                        .app_mono
                        .now();
                    self.config_aux.on_set_parameter(kind, pubtime);
                    log::trace!("updated config parameter {:?}: {:?}", key, value);
                    Ok(())
                }
                Err(err) => Err(ViseConfigureError::SetParameterFailed(key.into(), err)),
            }
        } else {
            Err(ViseConfigureError::InvalidParameterName(key.into()))
        }
    }
}

impl ViseTrait for Vise {
    fn id(&self) -> NodeId {
        self.0.id()
    }

    fn name(&self) -> &str {
        self.0.name()
    }

    fn type_name(&self) -> &str {
        self.0.type_name()
    }

    fn rx_names(&self) -> Vec<&str> {
        self.0.rx_names()
    }

    fn tx_names(&self) -> Vec<&str> {
        self.0.tx_names()
    }

    fn signal_names(&self) -> Vec<&str> {
        self.0.signal_names()
    }

    fn status(&self) -> Option<(String, DefaultStatus)> {
        self.0.status()
    }

    fn setup(&mut self, setup: NodeletSetup) {
        self.0.setup(setup);
    }

    fn get_parameters_with_properties(&self) -> ParameterWithPropertiesSet<(), &'static str> {
        self.0.get_parameters_with_properties()
    }

    fn configure(&mut self, key: &str, value: &ParameterValue) -> Result<(), ViseConfigureError> {
        self.0.configure(key, value)
    }
}

impl Lifecycle for Vise {
    fn cycle(&mut self, transition: Transition) -> Result<OutcomeKind> {
        self.0.cycle(transition)
    }
}