mecha10_behavior_runtime/actions/

navigation.rs

//! Stub navigation action nodes for path planning, following, and arrival checks.

use crate::{BehaviorNode, Context, NodeStatus};
use async_trait::async_trait;
use serde::{Deserialize, Serialize};
use serde_json::Value;

/// Body of the `tick` override for a stub node that has nothing to do yet: log a fixed debug
/// message and immediately report `Success`. `FollowPathNode`/`CheckArrivalNode` had
/// byte-for-byte identical `tick` bodies here (only the log message differed) - cargo-dupes
/// flagged them as an exact-duplicate trait-impl pair. This stays an expression-position macro
/// (rather than generating the whole `async fn tick`) so `#[async_trait]` still sees a literal
/// `async fn` signature to rewrite on each impl.
macro_rules! stub_tick_success {
    ($msg:literal) => {{
        tracing::debug!($msg);
        Ok(NodeStatus::Success)
    }};
}

/// PlanPathNode - Plans a path to a target position (STUB)
///
/// This is a stub implementation for path planning.
/// In a full implementation, this would calculate a collision-free path.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "plan_path",
///   "config": {
///     "target_position": [5.0, 3.0, 0.0],
///     "path_planning_algorithm": "a_star",
///     "max_speed": 1.0
///   }
/// }
/// ```
#[derive(Debug)]
pub struct PlanPathNode {
    #[allow(dead_code)]
    target_position: Vec<f64>,
    #[allow(dead_code)]
    algorithm: String,
}

#[derive(Debug, Serialize, Deserialize)]
struct PlanPathConfig {
    target_position: Vec<f64>,
    #[serde(default = "default_algorithm")]
    path_planning_algorithm: String,
}

fn default_algorithm() -> String {
    "a_star".to_string()
}

impl PlanPathNode {
    // duplicate: false positive — matches `StopNode::from_json`
    // (packages/behavior-runtime/src/actions/movement.rs) and `CheckArrivalNode::from_json`
    // below on AST shape only: parse a `serde_json::Value` into a node-specific `*Config`
    // struct, then map its fields into `Self`. See the note on `StopNode::from_json` for why
    // that's a coincidental structural match rather than real duplication (each maps a
    // different field set/count) and isn't unified the way `TimerNode`/`PauseNode` were.
    /// Create a new PlanPathNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: PlanPathConfig = serde_json::from_value(config)?;
        Ok(Self {
            target_position: cfg.target_position,
            algorithm: cfg.path_planning_algorithm,
        })
    }
}

#[async_trait]
impl BehaviorNode for PlanPathNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        // TODO: Implement actual path planning
        tracing::debug!("PlanPathNode: planning path to {:?} (stub)", self.target_position);
        Ok(NodeStatus::Success)
    }

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

/// FollowPathNode - Follows a planned path (STUB)
///
/// This is a stub implementation for path following.
/// In a full implementation, this would execute the planned path.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "follow_path",
///   "config": {
///     "max_speed": 1.0,
///     "turn_rate": 0.5,
///     "topic": "/motor/cmd_vel"
///   }
/// }
/// ```
#[derive(Debug)]
pub struct FollowPathNode {
    #[allow(dead_code)] // TODO: Use this when implementing path following
    topic: String,
    #[allow(dead_code)]
    max_speed: f64,
    #[allow(dead_code)]
    turn_rate: f64,
}

#[derive(Debug, Serialize, Deserialize)]
struct FollowPathConfig {
    #[serde(default = "default_motor_topic")]
    topic: String,
    #[serde(default = "default_max_speed")]
    max_speed: f64,
    #[serde(default = "default_turn_rate")]
    turn_rate: f64,
}

fn default_motor_topic() -> String {
    "/motor/cmd_vel".to_string()
}

fn default_max_speed() -> f64 {
    1.0
}

fn default_turn_rate() -> f64 {
    0.5
}

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

#[async_trait]
impl BehaviorNode for FollowPathNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        // TODO: Implement actual path following
        stub_tick_success!("FollowPathNode: following path (stub)")
    }

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

/// CheckArrivalNode - Checks if the robot has arrived at the target (STUB)
///
/// This is a stub implementation for arrival checking.
/// In a full implementation, this would check the robot's position against the target.
///
/// # Configuration
///
/// ```json
/// {
///   "type": "check_arrival",
///   "config": {
///     "position_tolerance": 0.1,
///     "angular_tolerance": 0.05
///   }
/// }
/// ```
#[derive(Debug)]
pub struct CheckArrivalNode {
    #[allow(dead_code)]
    position_tolerance: f64,
    #[allow(dead_code)]
    angular_tolerance: f64,
}

#[derive(Debug, Serialize, Deserialize)]
struct CheckArrivalConfig {
    position_tolerance: f64,
    angular_tolerance: f64,
}

impl CheckArrivalNode {
    // duplicate: false positive — matches `StopNode::from_json`
    // (packages/behavior-runtime/src/actions/movement.rs) and `PlanPathNode::from_json` above
    // on AST shape only; see the note on `StopNode::from_json` for why.
    /// Create a new CheckArrivalNode from JSON configuration
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let cfg: CheckArrivalConfig = serde_json::from_value(config)?;
        Ok(Self {
            position_tolerance: cfg.position_tolerance,
            angular_tolerance: cfg.angular_tolerance,
        })
    }
}

#[async_trait]
impl BehaviorNode for CheckArrivalNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        // TODO: Implement actual arrival checking
        stub_tick_success!("CheckArrivalNode: checking arrival (stub)")
    }

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