mecha10_sim/sim/

profile.rs

//! Robot profile extraction from mecha10.json

use anyhow::{Context, Result};
use serde::{Deserialize, Serialize};
use std::collections::HashSet;
use std::fs;
use std::path::Path;

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct RobotProfile {
    pub platform: String,
    pub sensors: Vec<String>,
    pub sensor_configs: Vec<SensorConfig>,
    pub task_nodes: Vec<TaskNode>,
    pub capabilities: Vec<String>,
    pub difficulty_preference: String,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct SensorConfig {
    pub name: String,
    pub sensor_type: String,
    pub mount_point: Option<Vec<f32>>,
    pub orientation: Option<Vec<f32>>,
    pub rl_config: Option<RLConfig>,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct RLConfig {
    pub observation: Option<bool>,
    pub num_rays: Option<i32>,
    pub ray_length: Option<f32>,
    pub resolution: Option<Vec<i32>>,
    pub fov: Option<f32>,
    pub sensor_type: Option<String>,
    pub update_rate_hz: Option<f32>,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct TaskNode {
    pub name: String,
    pub description: String,
    pub tags: Vec<String>,
}

#[derive(Debug, Deserialize)]
struct Mecha10Config {
    robot: RobotConfig,
    #[serde(default)]
    drivers: Vec<DriverConfig>,
    #[serde(default)]
    nodes: NodesConfig,
    #[serde(default)]
    simulation: Option<SimulationConfig>,
}

#[derive(Debug, Deserialize)]
struct RobotConfig {
    #[serde(default = "default_platform")]
    platform: String,
}

fn default_platform() -> String {
    "rover".to_string()
}

#[derive(Debug, Deserialize)]
struct DriverConfig {
    #[serde(rename = "type")]
    driver_type: String,
    #[serde(default)]
    name: String,
    #[serde(default)]
    physics: Option<PhysicsConfig>,
    #[serde(default)]
    rl: Option<RLConfig>,
}

#[derive(Debug, Deserialize)]
struct PhysicsConfig {
    #[serde(default)]
    mount_point: Option<Vec<f32>>,
    #[serde(default)]
    orientation: Option<Vec<f32>>,
}

/// Nodes are now a simple list of identifiers:
/// - "@mecha10/<name>" - Framework nodes
/// - "@local/<name>" - Project nodes
/// - "@<org>/<name>" - Registry nodes (future)
#[derive(Debug, Deserialize, Default)]
#[serde(transparent)]
struct NodesConfig(Vec<String>);

impl NodesConfig {
    /// Extract project node names (@local/<name> format)
    fn project_node_names(&self) -> Vec<String> {
        self.0
            .iter()
            .filter_map(|id| {
                if id.starts_with("@local/") {
                    id.strip_prefix("@local/").map(|s| s.to_string())
                } else {
                    None
                }
            })
            .collect()
    }
}

#[derive(Debug, Deserialize)]
struct SimulationConfig {
    #[serde(default = "default_difficulty")]
    difficulty_preference: String,
}

fn default_difficulty() -> String {
    "medium".to_string()
}

impl RobotProfile {
    /// Extract robot profile from mecha10.json file
    pub fn from_config_file(path: &Path) -> Result<Self> {
        let content =
            fs::read_to_string(path).with_context(|| format!("Failed to read config file: {}", path.display()))?;

        let config: Mecha10Config = serde_json::from_str(&content).with_context(|| "Failed to parse mecha10.json")?;

        Ok(Self::from_config(config))
    }

    /// Extract robot profile from parsed config
    fn from_config(config: Mecha10Config) -> Self {
        let platform = config.robot.platform;

        // Extract sensor types (unique)
        let sensors: Vec<String> = config
            .drivers
            .iter()
            .map(|d| d.driver_type.clone())
            .collect::<HashSet<_>>()
            .into_iter()
            .collect();

        // Extract detailed sensor configurations (exclude motors - they're actuators, not sensors)
        let sensor_configs: Vec<SensorConfig> = config
            .drivers
            .iter()
            .filter(|d| d.driver_type != "motor") // Filter out motors
            .map(|d| {
                let mount_point = d.physics.as_ref().and_then(|p| p.mount_point.clone());
                let orientation = d.physics.as_ref().and_then(|p| p.orientation.clone());

                SensorConfig {
                    name: if d.name.is_empty() {
                        format!("{}_{}", d.driver_type, "default")
                    } else {
                        d.name.clone()
                    },
                    sensor_type: d.driver_type.clone(),
                    mount_point,
                    orientation,
                    rl_config: d.rl.clone(),
                }
            })
            .collect();

        // Extract task nodes with tags (from project nodes: nodes/<name>)
        let task_nodes: Vec<TaskNode> = config
            .nodes
            .project_node_names()
            .into_iter()
            .map(|name| {
                let tags = extract_tags_from_node(&name, "");
                TaskNode {
                    name,
                    description: String::new(), // Description no longer in config
                    tags,
                }
            })
            .collect();

        // Infer capabilities
        let capabilities = infer_capabilities(&task_nodes, &sensors);

        // Get difficulty preference
        let difficulty_preference = config
            .simulation
            .as_ref()
            .map(|s| s.difficulty_preference.clone())
            .unwrap_or_else(|| "medium".to_string());

        Self {
            platform,
            sensors,
            sensor_configs,
            task_nodes,
            capabilities,
            difficulty_preference,
        }
    }
}

/// Extract tags from node name and description
fn extract_tags_from_node(name: &str, description: &str) -> Vec<String> {
    let mut tags = HashSet::new();

    let name_lower = name.to_lowercase();
    let desc_lower = description.to_lowercase();

    // Extract from node name
    if name_lower.contains("task") || name_lower.contains("planner") {
        tags.insert("task".to_string());
        tags.insert("planner".to_string());
    }
    if name_lower.contains("delivery") {
        tags.insert("delivery".to_string());
        tags.insert("navigation".to_string());
    }
    if name_lower.contains("follow") {
        tags.insert("following".to_string());
        tags.insert("tracking".to_string());
        tags.insert("hri".to_string());
    }
    if name_lower.contains("object") && name_lower.contains("detector") {
        tags.insert("object".to_string());
        tags.insert("detection".to_string());
        tags.insert("vision".to_string());
    }
    if name_lower.contains("patrol") {
        tags.insert("patrol".to_string());
        tags.insert("security".to_string());
        tags.insert("monitoring".to_string());
    }
    if name_lower.contains("manipulation") || name_lower.contains("gripper") {
        tags.insert("manipulation".to_string());
        tags.insert("gripper".to_string());
        tags.insert("arm".to_string());
    }
    if name_lower.contains("person") || name_lower.contains("human") {
        tags.insert("hri".to_string());
        tags.insert("person".to_string());
    }
    if name_lower.contains("tray") {
        tags.insert("delivery".to_string());
        tags.insert("waiter".to_string());
    }

    // Extract from description
    if desc_lower.contains("delivery") || desc_lower.contains("waiter") {
        tags.insert("delivery".to_string());
        tags.insert("service".to_string());
    }
    if desc_lower.contains("navigation") || desc_lower.contains("navigate") {
        tags.insert("navigation".to_string());
    }
    if desc_lower.contains("avoid") {
        tags.insert("avoidance".to_string());
    }
    if desc_lower.contains("follow") || desc_lower.contains("tracking") {
        tags.insert("following".to_string());
        tags.insert("tracking".to_string());
    }
    if desc_lower.contains("person") || desc_lower.contains("human") {
        tags.insert("hri".to_string());
        tags.insert("person".to_string());
    }
    if desc_lower.contains("object") {
        tags.insert("object".to_string());
    }
    if desc_lower.contains("detection") || desc_lower.contains("detect") {
        tags.insert("detection".to_string());
    }
    if desc_lower.contains("workflow") {
        tags.insert("workflow".to_string());
    }

    tags.into_iter().collect()
}

/// Infer robot capabilities from task nodes and sensors
fn infer_capabilities(task_nodes: &[TaskNode], sensors: &[String]) -> Vec<String> {
    let mut capabilities = HashSet::new();

    // Infer from sensor combinations
    let has_camera = sensors.iter().any(|s| s == "camera");
    let has_lidar = sensors.iter().any(|s| s == "lidar");
    let has_gripper = sensors.iter().any(|s| s == "gripper");
    let has_imu = sensors.iter().any(|s| s == "imu");

    if has_camera && has_lidar {
        capabilities.insert("vision_navigation".to_string());
    }
    if has_camera {
        capabilities.insert("vision".to_string());
        capabilities.insert("object_detection".to_string());
    }
    if has_lidar {
        capabilities.insert("navigation".to_string());
        capabilities.insert("obstacle_avoidance".to_string());
    }
    if has_gripper {
        capabilities.insert("manipulation".to_string());
    }
    if has_imu {
        capabilities.insert("stabilization".to_string());
    }

    // Infer from task nodes
    for node in task_nodes {
        if node.name.contains("planner") {
            capabilities.insert("planning".to_string());
        }
        if node.name.contains("controller") {
            capabilities.insert("control".to_string());
        }
        if node.tags.contains(&"delivery".to_string()) {
            capabilities.insert("delivery".to_string());
        }
        if node.tags.contains(&"following".to_string()) {
            capabilities.insert("following".to_string());
        }
    }

    capabilities.into_iter().collect()
}