mecha10-behavior-runtime 0.8.2

//! Action nodes that publish velocity commands to move the robot.

use super::elapsed_timer::{reset_timer_field, ElapsedTimer};
use crate::{BehaviorNode, Context, NodeStatus};
use async_trait::async_trait;
use mecha10_core::actuator::Twist;
use mecha10_core::topics::Topic;
use serde::{Deserialize, Serialize};
use serde_json::Value;
use std::time::{Duration, Instant};

#[derive(Debug)]
pub struct WanderNode {
    topic: String,
    linear_speed: f64,
    angular_speed: f64,
    change_interval: Duration,
    last_change: Option<Instant>,
    current_angular: f64,
}

#[derive(Debug, Serialize, Deserialize)]
struct WanderConfig {
    topic: String,
    #[serde(default = "default_linear_speed")]
    linear_speed: f64,
    #[serde(default = "default_angular_speed")]
    angular_speed: f64,
    #[serde(default = "default_change_interval")]
    change_interval_secs: f64,
}

fn default_linear_speed() -> f64 {
    0.3
}

fn default_angular_speed() -> f64 {
    0.5
}

fn default_change_interval() -> f64 {
    3.0
}

impl WanderNode {
    /// Create a new WanderNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: WanderConfig = serde_json::from_value(config)?;
        Ok(Self {
            topic: cfg.topic,
            linear_speed: cfg.linear_speed,
            angular_speed: cfg.angular_speed,
            change_interval: Duration::from_secs_f64(cfg.change_interval_secs),
            last_change: None,
            current_angular: 0.0,
        })
    }
}

#[async_trait]
impl BehaviorNode for WanderNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        let now = Instant::now();

        if self.last_change.is_none() || now.duration_since(self.last_change.unwrap()) >= self.change_interval {
            self.current_angular = (rand::random::<f64>() * 2.0 - 1.0) * self.angular_speed;
            self.last_change = Some(now);
        }

        let twist = Twist {
            linear: self.linear_speed as f32,
            angular: self.current_angular as f32,
        };

        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
        let topic = Topic::<Twist>::new(topic_static);

        ctx.publish_to(topic, &twist).await?;
        Ok(NodeStatus::Running)
    }

    fn name(&self) -> &str {
        "WanderNode"
    }
}

#[derive(Debug, Default)]
pub struct MoveNode {
    topic: String,
    linear: f64,
    angular: f64,
    duration: Option<Duration>,
    timer: ElapsedTimer,
}

#[derive(Debug, Serialize, Deserialize)]
struct MoveConfig {
    topic: String,
    linear: f64,
    angular: f64,
    duration_secs: Option<f64>,
}

impl MoveNode {
    /// Create a new MoveNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: MoveConfig = serde_json::from_value(config)?;
        Ok(Self {
            topic: cfg.topic,
            linear: cfg.linear,
            angular: cfg.angular,
            duration: cfg.duration_secs.map(Duration::from_secs_f64),
            timer: ElapsedTimer::default(),
        })
    }
}

#[async_trait]
impl BehaviorNode for MoveNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        if let Some(duration) = self.duration {
            if self.timer.elapsed_since_start(duration) {
                return Ok(NodeStatus::Success);
            }
        }

        let twist = Twist {
            linear: self.linear as f32,
            angular: self.angular as f32,
        };

        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
        let topic = Topic::<Twist>::new(topic_static);

        ctx.publish_to(topic, &twist).await?;

        Ok(NodeStatus::Running)
    }

    reset_timer_field!(timer);

    fn name(&self) -> &str {
        "MoveNode"
    }
}

#[derive(Debug)]
pub struct StopNode {
    topic: String,
}

#[derive(Debug, Serialize, Deserialize)]
struct StopConfig {
    topic: String,
}

impl StopNode {
    /// Create a new StopNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: StopConfig = serde_json::from_value(config)?;
        Ok(Self { topic: cfg.topic })
    }
}

#[async_trait]
impl BehaviorNode for StopNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        let twist = Twist {
            linear: 0.0,
            angular: 0.0,
        };

        let topic_static: &'static str = Box::leak(self.topic.clone().into_boxed_str());
        let topic = Topic::<Twist>::new(topic_static);

        ctx.publish_to(topic, &twist).await?;
        tracing::debug!("StopNode: published stop command");
        Ok(NodeStatus::Success)
    }

    fn name(&self) -> &str {
        "StopNode"
    }
}