mecha10-behavior-runtime 4.2.3

//! Action nodes that publish velocity commands to move the robot.
//!
//! `WanderNode`/`MoveNode`/`StopNode` all drive the same "desired linear/angular velocity"
//! shape. Which message type and topic that actually becomes is resolved once per tick via
//! [`controller_mode`] - the same capability-based output-selection mechanism `mecha10-nodes`'
//! `teleop`/`rl`/`llm-command` controller nodes use to decide whether a project wants a
//! whole-robot [`Twist`] or a per-joint [`JointState`] (see [`resolve_movement_output`]).
//! Previously these nodes always published a `Twist` to whatever raw `topic` string the
//! behavior author configured, so a behavior tree using `wander`/`move` on a joints-only
//! platform (e.g. a pan-tilt-only `sentry`-style project, with no drivetrain at all) silently
//! did nothing - the `Twist` it published had no subscriber.

use crate::{BehaviorNode, Context, NodeStatus};
use async_trait::async_trait;
use mecha10_core::actuator::Twist;
use mecha10_core::config::{controller_mode, generate_joint_command, ControllerMode};
use mecha10_core::messages::JointState;
use mecha10_core::topics::actuator as actuator_topics;
use serde::{Deserialize, Serialize};
use serde_json::Value;
use std::time::{Duration, Instant};

use super::elapsed_timer::{reset_timer_field, ElapsedTimer};

/// The locomotion topic `wander`/`move`/`stop` configs default to when the behavior author
/// doesn't override `topic` - matches `navigation.rs`'s `FollowPathNode::default_motor_topic`
/// literal, the project-wide convention for "the" drivetrain command topic.
fn default_motor_topic() -> String {
    "/motor/cmd_vel".to_string()
}

/// What a `wander`/`move`/`stop` node actually publishes this tick, resolved by
/// [`resolve_movement_output`].
#[derive(Debug)]
enum MovementOutput {
    /// Locomotion-capable project: a whole-robot velocity [`Twist`], published to `topic`
    /// (today's exact behavior, unchanged).
    Twist { topic: String, twist: Twist },
    /// Joints-only project (e.g. pan-tilt): a per-joint [`JointState`], published to `topic`.
    Joints { topic: String, joint_state: JointState },
}

/// Resolve what a movement action node should publish for a desired `linear`/`angular`
/// velocity, given the behavior author's `configured_topic`.
///
/// - **Locomotion** (the default, and today's only behavior): builds a [`Twist`] exactly as
///   before, published to `configured_topic` unchanged - existing behavior JSONs for
///   locomotion robots keep working with no changes required.
/// - **Joints**: `linear`/`angular` map onto the first/second entry of the project's declared
///   joint names (clamped via [`generate_joint_command`]/joint limits), the same convention
///   `mecha10-nodes`' `rl` controller's `build_output_command` uses. The output topic defaults
///   to the project-wide joint command topic (`/actuator/joint_cmd`, same as
///   `teleop`/`rl`/`pan-tilt`) *unless* the author explicitly set `topic` to something other
///   than [`default_motor_topic`] - in that case we trust the author's explicit choice and keep
///   using it verbatim, on the theory that an author who bothered to override the topic away
///   from the locomotion default already picked one that matches whatever this project
///   actually publishes/subscribes to. This avoids the node silently publishing a `JointState`
///   to a topic the author pointed at a `Twist` subscriber (or vice versa) while still letting
///   an author who genuinely wants a non-default joint topic have one.
fn resolve_movement_output(configured_topic: &str, linear: f64, angular: f64) -> MovementOutput {
    match controller_mode() {
        ControllerMode::Locomotion => MovementOutput::Twist {
            topic: configured_topic.to_string(),
            twist: Twist {
                linear: linear as f32,
                angular: angular as f32,
            },
        },
        ControllerMode::Joints { joint_names } => {
            let topic = if configured_topic == default_motor_topic() {
                actuator_topics::JOINT_CMD.path().to_string()
            } else {
                configured_topic.to_string()
            };
            MovementOutput::Joints {
                topic,
                joint_state: joint_command(&joint_names, linear as f32, angular as f32),
            }
        }
    }
}

/// Build a [`JointState`] driving the first named joint to `linear` and the second to
/// `angular` (each clamped via [`generate_joint_command`]/the project's configured joint
/// limits) - the same linear-first/angular-second convention `mecha10-nodes`' `rl` controller
/// uses. If fewer than two joint names are configured, only the available ones are driven;
/// this never fails, it just produces a shorter `JointState`.
fn joint_command(joint_names: &[String], linear: f32, angular: f32) -> JointState {
    let mut names = Vec::new();
    let mut positions = Vec::new();

    if let Some(name) = joint_names.first() {
        let cmd = generate_joint_command(name, linear);
        names.extend(cmd.names);
        positions.extend(cmd.positions);
    }
    if let Some(name) = joint_names.get(1) {
        let cmd = generate_joint_command(name, angular);
        names.extend(cmd.names);
        positions.extend(cmd.positions);
    }

    JointState::with_positions(names, positions)
}

/// Publish `linear`/`angular` to whatever [`resolve_movement_output`] resolves to for this
/// project's [`controller_mode`].
async fn publish_movement(ctx: &Context, configured_topic: &str, linear: f64, angular: f64) -> anyhow::Result<()> {
    match resolve_movement_output(configured_topic, linear, angular) {
        MovementOutput::Twist { topic, twist } => {
            ctx.publish_to_path(&topic, &twist).await?;
        }
        MovementOutput::Joints { topic, joint_state } => {
            ctx.publish_to_path(&topic, &joint_state).await?;
        }
    }
    Ok(())
}

#[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 {
    #[serde(default = "default_motor_topic")]
    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);
        }

        publish_movement(ctx, &self.topic, self.linear_speed, self.current_angular).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 {
    #[serde(default = "default_motor_topic")]
    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);
            }
        }

        publish_movement(ctx, &self.topic, self.linear, self.angular).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 {
    #[serde(default = "default_motor_topic")]
    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> {
        publish_movement(ctx, &self.topic, 0.0, 0.0).await?;
        tracing::debug!("StopNode: published stop command");
        Ok(NodeStatus::Success)
    }

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

#[cfg(test)]
mod tests {
    use super::*;
    use mecha10_core::config::{ACTUATION_JOINT_NAMES_ENV_VAR, ACTUATOR_JOINT_LIMITS_ENV_VAR, CAPABILITIES_ENV_VAR};
    use std::env;
    use std::sync::Mutex;

    // `controller_mode()`/`generate_joint_command()` read process-global env vars; serialize
    // the tests that touch them so they can't observe each other's in-flight state (mirrors
    // `mecha10_core::config`'s own test convention).
    static CONTROLLER_ENV_LOCK: Mutex<()> = Mutex::new(());

    fn clear_controller_env() {
        env::remove_var(CAPABILITIES_ENV_VAR);
        env::remove_var(ACTUATION_JOINT_NAMES_ENV_VAR);
        env::remove_var(ACTUATOR_JOINT_LIMITS_ENV_VAR);
    }

    #[test]
    fn resolve_movement_output_is_a_twist_to_the_configured_topic_in_locomotion_mode() {
        let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        clear_controller_env();

        let output = resolve_movement_output("/motor/cmd_vel", 1.0, 0.5);

        let MovementOutput::Twist { topic, twist } = output else {
            panic!("expected Twist output");
        };
        assert_eq!(topic, "/motor/cmd_vel");
        assert_eq!(twist.linear, 1.0);
        assert_eq!(twist.angular, 0.5);

        clear_controller_env();
    }

    #[test]
    fn resolve_movement_output_ignores_declared_joints_when_locomotion_also_declared() {
        let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        clear_controller_env();
        env::set_var(CAPABILITIES_ENV_VAR, r#"["joints","locomotion"]"#);
        env::set_var(ACTUATION_JOINT_NAMES_ENV_VAR, r#"["pan","tilt"]"#);

        let output = resolve_movement_output("/motor/cmd_vel", 1.0, 0.5);

        let MovementOutput::Twist { topic, twist } = output else {
            panic!("expected Twist output");
        };
        assert_eq!(topic, "/motor/cmd_vel");
        assert_eq!(twist.linear, 1.0);
        assert_eq!(twist.angular, 0.5);

        clear_controller_env();
    }

    #[test]
    fn resolve_movement_output_is_a_joint_state_to_the_default_joint_topic_in_joints_mode() {
        let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        clear_controller_env();
        env::set_var(CAPABILITIES_ENV_VAR, r#"["joints"]"#);
        env::set_var(ACTUATION_JOINT_NAMES_ENV_VAR, r#"["pan","tilt"]"#);

        // The author left `topic` at its locomotion default - we should redirect to the
        // project-wide joint command topic rather than publish a JointState to "/motor/cmd_vel".
        let output = resolve_movement_output(&default_motor_topic(), 0.3, -0.2);

        let MovementOutput::Joints { topic, joint_state } = output else {
            panic!("expected Joints output");
        };
        assert_eq!(topic, actuator_topics::JOINT_CMD.path());
        assert_eq!(joint_state.names, vec!["pan".to_string(), "tilt".to_string()]);
        assert_eq!(joint_state.positions, vec![0.3, -0.2]);

        clear_controller_env();
    }

    #[test]
    fn resolve_movement_output_keeps_an_explicitly_overridden_topic_in_joints_mode() {
        let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        clear_controller_env();
        env::set_var(CAPABILITIES_ENV_VAR, r#"["joints"]"#);
        env::set_var(ACTUATION_JOINT_NAMES_ENV_VAR, r#"["pan","tilt"]"#);

        let output = resolve_movement_output("/custom/joint_topic", 0.3, -0.2);

        let MovementOutput::Joints { topic, .. } = output else {
            panic!("expected Joints output");
        };
        assert_eq!(topic, "/custom/joint_topic");

        clear_controller_env();
    }

    #[test]
    fn resolve_movement_output_clamps_to_configured_joint_limits_in_joints_mode() {
        let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        clear_controller_env();
        env::set_var(CAPABILITIES_ENV_VAR, r#"["joints"]"#);
        env::set_var(ACTUATION_JOINT_NAMES_ENV_VAR, r#"["pan"]"#);
        env::set_var(
            ACTUATOR_JOINT_LIMITS_ENV_VAR,
            r#"[{"joint_id":"pan","min_angle_rad":-1.0,"max_angle_rad":1.0}]"#,
        );

        let output = resolve_movement_output(&default_motor_topic(), 5.0, 0.0);

        let MovementOutput::Joints { joint_state, .. } = output else {
            panic!("expected Joints output");
        };
        assert_eq!(joint_state.names, vec!["pan".to_string()]);
        assert_eq!(joint_state.positions, vec![1.0]);

        clear_controller_env();
    }

    #[test]
    fn resolve_movement_output_drives_only_the_available_joint_when_fewer_than_two_are_configured() {
        let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        clear_controller_env();
        env::set_var(CAPABILITIES_ENV_VAR, r#"["joints"]"#);
        env::set_var(ACTUATION_JOINT_NAMES_ENV_VAR, r#"["pan"]"#);

        let output = resolve_movement_output(&default_motor_topic(), 0.7, -0.4);

        let MovementOutput::Joints { joint_state, .. } = output else {
            panic!("expected Joints output");
        };
        assert_eq!(joint_state.names, vec!["pan".to_string()]);
        assert_eq!(joint_state.positions, vec![0.7]);

        clear_controller_env();
    }

    #[test]
    fn wander_node_from_json_defaults_topic_to_the_locomotion_default() {
        let node = WanderNode::from_json(serde_json::json!({})).unwrap();
        assert_eq!(node.topic, default_motor_topic());
    }

    #[test]
    fn move_node_from_json_defaults_topic_to_the_locomotion_default() {
        let node = MoveNode::from_json(serde_json::json!({ "linear": 1.0, "angular": 0.0 })).unwrap();
        assert_eq!(node.topic, default_motor_topic());
    }

    #[test]
    fn stop_node_from_json_defaults_topic_to_the_locomotion_default() {
        let node = StopNode::from_json(serde_json::json!({})).unwrap();
        assert_eq!(node.topic, default_motor_topic());
    }
}