mecha10-behavior-runtime 0.6.5

//! 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};

/// WanderNode - Generates random movement commands
///
/// This node publishes random velocity commands to make a robot wander around.
/// It periodically changes direction to create natural wandering behavior.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "wander",
///   "config": {
///     "topic": "/motor/cmd_vel",
///     "linear_speed": 0.3,
///     "angular_speed": 0.5,
///     "change_interval_secs": 3.0
///   }
/// }
/// ```
#[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"
    }
}

/// MoveNode - Executes a velocity command
///
/// Publishes a fixed velocity command to control robot movement.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "move",
///   "config": {
///     "topic": "/motor/cmd_vel",
///     "linear": 0.5,
///     "angular": 0.0,
///     "duration_secs": 2.0
///   }
/// }
/// ```
#[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"
    }
}

/// StopNode - Publishes zero velocity to stop the robot
///
/// This node publishes a zero velocity command to stop the robot immediately.
/// Returns Success after publishing the stop command.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "stop",
///   "config": {
///     "topic": "/motor/cmd_vel"
///   }
/// }
/// ```
#[derive(Debug)]
pub struct StopNode {
    topic: String,
}

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

impl StopNode {
    // duplicate: false positive — matches `PlanPathNode::from_json`/`CheckArrivalNode::from_json`
    // (packages/behavior-runtime/src/actions/navigation.rs) on AST shape only: parse a
    // `serde_json::Value` into a node-specific `*Config` struct, then map its fields into
    // `Self`. Unlike `TimerNode`/`PauseNode`'s `from_json` (unified behind the
    // `from_json_with_duration!` macro in `elapsed_timer.rs` because both bodies were
    // byte-for-byte identical), each of these maps a different field set/count, so a shared
    // macro would just relocate the boilerplate rather than remove real duplication.
    /// 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"
    }
}