mecha10-planning 0.6.3

//! PathPlannerNode behavior implementation

use crate::types::Path;
use async_trait::async_trait;
use mecha10_behavior_runtime::{BehaviorNode, NodeStatus};
use mecha10_core::Context;
use serde::{Deserialize, Serialize};
use tracing::{debug, info};

/// Trait for path planning algorithms
pub trait PathPlanner: Send + Sync + std::fmt::Debug {
    /// Plan a path from start to goal
    fn plan(&self) -> Option<Path>;

    /// Get planner name
    fn name(&self) -> &str {
        "path_planner"
    }
}

impl PathPlanner for crate::astar::AStarPlanner {
    fn plan(&self) -> Option<Path> {
        self.plan()
    }

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

impl PathPlanner for crate::rrt::RRTPlanner {
    fn plan(&self) -> Option<Path> {
        self.plan()
    }

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

/// Path planner behavior node
///
/// This node wraps a path planning algorithm and executes it as a BehaviorNode.
/// It plans once and then returns Success or Failure.
#[derive(Debug)]
pub struct PathPlannerNode {
    planner: Box<dyn PathPlanner>,
    planned_path: Option<Path>,
    has_planned: bool,
}

impl PathPlannerNode {
    mecha10_core::new_with_defaults!(
        /// Create a new path planner node
        pub fn new(planner: Box<dyn PathPlanner>) -> Self {
            planned_path: None,
            has_planned: false
        }
    );

    /// Get the planned path (if planning succeeded)
    pub fn path(&self) -> Option<&Path> {
        self.planned_path.as_ref()
    }
}

#[async_trait]
impl BehaviorNode for PathPlannerNode {
    async fn tick(&mut self, _ctx: &Context) -> anyhow::Result<NodeStatus> {
        if self.has_planned {
            return Ok(if self.planned_path.is_some() {
                NodeStatus::Success
            } else {
                NodeStatus::Failure
            });
        }

        debug!("PathPlanner: planning with {}", self.planner.name());

        self.planned_path = self.planner.plan();
        self.has_planned = true;

        if let Some(ref path) = self.planned_path {
            info!(
                "PathPlanner: found path with {} waypoints, length: {:.2}",
                path.len(),
                path.length
            );
            Ok(NodeStatus::Success)
        } else {
            info!("PathPlanner: no path found");
            Ok(NodeStatus::Failure)
        }
    }

    async fn reset(&mut self) -> anyhow::Result<()> {
        self.planned_path = None;
        self.has_planned = false;
        Ok(())
    }

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

/// Configuration for path planner (for JSON deserialization)
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct PathPlannerConfig {
    /// Planner algorithm to use
    pub algorithm: String,

    /// Start position
    pub start: (f32, f32),

    /// Goal position
    pub goal: (f32, f32),

    /// Algorithm-specific parameters
    #[serde(default)]
    pub params: serde_json::Value,
}