mecha10-behavior-runtime 4.2.11

//! 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, TreeStatusSnapshot};
use async_trait::async_trait;
use mecha10_core::actuator::Twist;
use mecha10_core::config::{controller_mode, generate_joint_command, joint_limits, ControllerMode};
use mecha10_core::messages::JointState;
use mecha10_core::topics::actuator as actuator_topics;
use mecha10_core::topics::sensor as sensor_topics;
use mecha10_core::Receiver;
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 } => MovementOutput::Joints {
            topic: joint_command_topic(configured_topic),
            joint_state: joint_command(&joint_names, linear as f32, angular as f32),
        },
    }
}

/// Resolve the topic a joints-mode `JointState` should publish to: the project-wide joint
/// command topic (`/actuator/joint_cmd`) when the behavior author left `topic` at its
/// locomotion default, or the author's explicit override verbatim otherwise. Shared by
/// [`resolve_movement_output`] and [`MoveNode`]'s speed-limited stepping path so both pick the
/// same topic for the same configured `topic` value.
fn joint_command_topic(configured_topic: &str) -> String {
    if configured_topic == default_motor_topic() {
        actuator_topics::JOINT_CMD.path().to_string()
    } else {
        configured_topic.to_string()
    }
}

/// 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,
    last_status: Option<NodeStatus>,
}

#[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,
            last_status: None,
        })
    }
}

#[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?;
        self.last_status = Some(NodeStatus::Running);
        Ok(NodeStatus::Running)
    }

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

    fn status_snapshot(&self) -> TreeStatusSnapshot {
        TreeStatusSnapshot {
            name: self.name().to_string(),
            status: self.last_status,
            active_capability: self.active_capability().map(String::from),
            children: Vec::new(),
        }
    }
}

/// Within this many radians of `target`, a `speed`-limited joint is considered "arrived" for
/// [`MoveNode`]'s completion check - real joint feedback never settles to an exact float, so an
/// exact-equality check would never report `Success`.
const JOINT_POSITION_TOLERANCE_RAD: f32 = 0.01;

#[derive(Default)]
pub struct MoveNode {
    topic: String,
    linear: f64,
    angular: f64,
    duration: Option<Duration>,
    /// Maximum rate of change, in rad/s, [`MoveNode::tick_speed_limited`] steps each joint's
    /// published position by - unset means "unchanged": publish the static `linear`/`angular`
    /// target every tick, exactly as before this field existed (see
    /// [`wants_speed_limited_stepping`]).
    speed: Option<f64>,
    timer: ElapsedTimer,
    /// Subscribed lazily on the first speed-limited tick, same pattern as `SensorCheckNode`'s
    /// `receiver`/`GoToLocationNode`'s `odometry_receiver`.
    joint_feedback: Option<Receiver<JointState>>,
    latest_joint_state: Option<JointState>,
    /// Wall-clock time of the last speed-limited tick, used to compute `dt` for `speed * dt`
    /// stepping - same pattern as `GoToLocationNode::last_tick`.
    last_step_tick: Option<Instant>,
    last_status: Option<NodeStatus>,
}

impl std::fmt::Debug for MoveNode {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.debug_struct("MoveNode")
            .field("topic", &self.topic)
            .field("linear", &self.linear)
            .field("angular", &self.angular)
            .field("duration", &self.duration)
            .field("speed", &self.speed)
            .field("timer", &self.timer)
            .field("joint_feedback_subscribed", &self.joint_feedback.is_some())
            .field("latest_joint_state", &self.latest_joint_state)
            .field("last_step_tick", &self.last_step_tick)
            .field("last_status", &self.last_status)
            .finish()
    }
}

#[derive(Debug, Serialize, Deserialize)]
struct MoveConfig {
    #[serde(default = "default_motor_topic")]
    topic: String,
    linear: f64,
    angular: f64,
    duration_secs: Option<f64>,
    speed: 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),
            speed: cfg.speed,
            timer: ElapsedTimer::default(),
            joint_feedback: None,
            latest_joint_state: None,
            last_step_tick: None,
            last_status: None,
        })
    }

    /// Speed-limited, real-feedback-driven joint stepping (see module-level design notes above
    /// [`resolve_movement_output`]): step each configured joint's published position toward its
    /// clamped `linear`/`angular` target by at most `speed * dt` this tick, using the latest
    /// known *real* joint position (from [`sensor_topics::JOINT_STATES`]) as the starting point
    /// rather than re-publishing the static target. Reaches `Success` once every configured
    /// joint's real position is within [`JOINT_POSITION_TOLERANCE_RAD`] of its target.
    async fn tick_speed_limited(
        &mut self,
        ctx: &Context,
        joint_names: &[String],
        speed: f64,
    ) -> anyhow::Result<NodeStatus> {
        if self.joint_feedback.is_none() {
            self.joint_feedback = Some(ctx.subscribe(sensor_topics::JOINT_STATES).await?);
        }
        while let Ok(state) = self.joint_feedback.as_mut().expect("just subscribed above").try_recv() {
            self.latest_joint_state = Some(state);
        }

        let Some(latest) = self.latest_joint_state.as_ref() else {
            // No real feedback has arrived yet. Stepping from an assumed starting position
            // (e.g. 0.0) could snap the servo just as badly as the bug this feature fixes -
            // hold instead, the same "wait for the first reading" precedent
            // `SensorCheckNode`/`GoToLocationNode` already establish for a required feedback
            // topic that hasn't produced a message yet.
            tracing::debug!(
                "MoveNode: waiting for first {} reading before speed-limited joint stepping",
                sensor_topics::JOINT_STATES.path()
            );
            self.last_status = Some(NodeStatus::Running);
            return Ok(NodeStatus::Running);
        };

        let now = Instant::now();
        let dt = self
            .last_step_tick
            .map(|last| now.duration_since(last).as_secs_f64())
            .unwrap_or(0.0);
        self.last_step_tick = Some(now);
        let max_step = (speed * dt) as f32;

        let mut names = Vec::new();
        let mut positions = Vec::new();
        let mut all_arrived = true;

        for (name, raw_target) in [
            (joint_names.first(), self.linear as f32),
            (joint_names.get(1), self.angular as f32),
        ] {
            let Some(name) = name else { continue };
            let Some(current) = latest.get_position(name) else {
                // This joint hasn't shown up in a feedback message yet - same "don't step from
                // an assumed position" reasoning as the no-feedback-at-all case above.
                tracing::debug!(joint = %name, "MoveNode: waiting for feedback on this joint before speed-limited stepping");
                self.last_status = Some(NodeStatus::Running);
                return Ok(NodeStatus::Running);
            };
            let target = clamped_joint_target(name, raw_target);
            let (stepped, arrived) = step_joint_position(current, target, max_step, JOINT_POSITION_TOLERANCE_RAD);
            names.push(name.clone());
            positions.push(stepped);
            all_arrived &= arrived;
        }

        ctx.publish_to_path(
            &joint_command_topic(&self.topic),
            &JointState::with_positions(names.clone(), positions),
        )
        .await?;

        if !names.is_empty() && all_arrived {
            self.last_status = Some(NodeStatus::Success);
            return Ok(NodeStatus::Success);
        }

        self.last_status = Some(NodeStatus::Running);
        Ok(NodeStatus::Running)
    }
}

#[async_trait]
impl BehaviorNode for MoveNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        let mode = controller_mode();
        let speed_limited = wants_speed_limited_stepping(self.speed, &mode);

        // `duration_secs`, when configured, is always a hard safety timeout - even when
        // `speed` is set and completion is otherwise driven by real-feedback tolerance below.
        //
        // For the speed-limited joint-stepping path, `duration_secs` alone can't be trusted as
        // the ceiling: it's a single static number a behavior author hand-tunes for whatever
        // joint range *their* robot happens to have, with no way to know the right value ahead
        // of time (see module docs). So here the *effective* timeout is widened - never
        // shortened - to `max(duration_secs, dynamic_safety_timeout)`, where the dynamic value
        // is derived from the robot's own real `mecha10.json`-resolved joint limits and the
        // configured `speed`. This makes `duration_secs` a floor/sanity-minimum rather than a
        // ceiling that can silently truncate a legitimate slow sweep.
        if let Some(duration) = self.duration {
            let effective_duration = if speed_limited {
                let ControllerMode::Joints { joint_names } = &mode else {
                    unreachable!("wants_speed_limited_stepping only returns true for ControllerMode::Joints");
                };
                let speed = self
                    .speed
                    .expect("wants_speed_limited_stepping only returns true when speed is set");
                dynamic_safety_timeout(joint_names, speed, duration)
            } else {
                duration
            };

            if self.timer.elapsed_since_start(effective_duration) {
                self.last_status = Some(NodeStatus::Success);
                return Ok(NodeStatus::Success);
            }
        }

        if speed_limited {
            let ControllerMode::Joints { joint_names } = mode else {
                unreachable!("wants_speed_limited_stepping only returns true for ControllerMode::Joints");
            };
            let speed = self
                .speed
                .expect("wants_speed_limited_stepping only returns true when speed is set");
            return self.tick_speed_limited(ctx, &joint_names, speed).await;
        }

        publish_movement(ctx, &self.topic, self.linear, self.angular).await?;

        self.last_status = Some(NodeStatus::Running);
        Ok(NodeStatus::Running)
    }

    reset_timer_field!(timer);

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

    fn status_snapshot(&self) -> TreeStatusSnapshot {
        TreeStatusSnapshot {
            name: self.name().to_string(),
            status: self.last_status,
            active_capability: self.active_capability().map(String::from),
            children: Vec::new(),
        }
    }
}

/// Whether this tick should use `speed`-limited, feedback-driven joint stepping rather than
/// publishing the static `linear`/`angular` target every tick: only once the behavior author
/// configured a `speed` AND the project is in joints mode. Locomotion's `Twist` is already a
/// velocity command the robot integrates into position over time - naturally smooth without any
/// rate limiting here - so `speed` is ignored outside joints mode.
fn wants_speed_limited_stepping(speed: Option<f64>, mode: &ControllerMode) -> bool {
    speed.is_some() && matches!(mode, ControllerMode::Joints { .. })
}

/// Multiplier applied to the theoretical minimum travel time (`range / speed`) when deriving a
/// speed-limited move's dynamic safety timeout (see [`dynamic_safety_timeout`]) - leaves
/// headroom for real-world settling/overshoot-correction time beyond the theoretical minimum,
/// rather than cutting a move off the instant it crosses the bare-minimum travel time.
const DYNAMIC_SAFETY_MARGIN: f64 = 1.5;

/// Derive the effective hard-safety-timeout [`Duration`] for a speed-limited joints-mode
/// [`MoveNode`] move: `max(static_duration, required_secs * DYNAMIC_SAFETY_MARGIN)`, where
/// `required_secs` is the worst-case time to cross a driven joint's *full configured range*
/// (`max_angle_rad - min_angle_rad`) at `speed` rad/s, resolved via the same [`joint_limits`]
/// helper [`clamped_joint_target`] already uses.
///
/// The full configured range (rather than the distance from wherever the joint happens to be
/// at move-start) is the safe worst-case choice, since the real starting position isn't known
/// yet when this safety timeout needs to be decided - the move might start at either extreme.
/// Only the first two `joint_names` are considered (linear, then angular), matching the
/// linear-first/angular-second convention [`MoveNode::tick_speed_limited`] steps; when more
/// than one of those joints has a resolvable limit, the slower (larger-required-time) one
/// wins, since the move only completes once every driven joint has arrived.
///
/// Falls back to `static_duration` unchanged (never *shortens* it) when `speed` is zero or
/// negative (would divide by zero/invert direction) or when none of the driven joints have a
/// resolvable configured limit (e.g. the project doesn't declare joint limits for this robot) -
/// in both cases there's no real joint-range data to derive a dynamic duration from, so
/// `duration_secs` alone decides, exactly as before this feature existed.
fn dynamic_safety_timeout(joint_names: &[String], speed: f64, static_duration: Duration) -> Duration {
    if speed <= 0.0 {
        return static_duration;
    }

    let required_secs = joint_names
        .iter()
        .take(2)
        .filter_map(|name| joint_limits(name))
        .map(|limits| (limits.max_angle_rad - limits.min_angle_rad).abs() as f64 / speed)
        .reduce(f64::max);

    let Some(required_secs) = required_secs else {
        return static_duration;
    };

    static_duration.max(Duration::from_secs_f64(required_secs * DYNAMIC_SAFETY_MARGIN))
}

/// Clamp `raw_target` to `joint_name`'s configured joint limits (if the project declares any) -
/// the same clamp [`generate_joint_command`] applies to a joints-mode `MoveNode`'s final
/// published target, reused here so intermediate stepped positions never aim past a joint's
/// limits either.
fn clamped_joint_target(joint_name: &str, raw_target: f32) -> f32 {
    match joint_limits(joint_name) {
        Some(limits) => raw_target.clamp(limits.min_angle_rad, limits.max_angle_rad),
        None => raw_target,
    }
}

/// Step `current` toward `target` by at most `max_step.abs()` (the caller supplies `speed *
/// dt`, always non-negative in practice), without ever overshooting `target`. Returns the
/// stepped position and whether it's already within `tolerance` of `target`.
fn step_joint_position(current: f32, target: f32, max_step: f32, tolerance: f32) -> (f32, bool) {
    let max_step = max_step.abs();
    let delta = target - current;
    let stepped = current + delta.clamp(-max_step, max_step);
    let arrived = (target - stepped).abs() <= tolerance;
    (stepped, arrived)
}

#[derive(Debug)]
pub struct StopNode {
    topic: String,
    last_status: Option<NodeStatus>,
}

#[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,
            last_status: None,
        })
    }
}

#[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");
        self.last_status = Some(NodeStatus::Success);
        Ok(NodeStatus::Success)
    }

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

    fn status_snapshot(&self) -> TreeStatusSnapshot {
        TreeStatusSnapshot {
            name: self.name().to_string(),
            status: self.last_status,
            active_capability: self.active_capability().map(String::from),
            children: Vec::new(),
        }
    }
}

#[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 move_node_from_json_defaults_speed_to_unset() {
        let node = MoveNode::from_json(serde_json::json!({ "linear": 1.0, "angular": 0.0 })).unwrap();
        assert_eq!(node.speed, None);
    }

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

    #[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());
    }

    // --- `speed`-limited joint stepping (LAB sentry pan-sweep jitter fix) ---
    //
    // `wants_speed_limited_stepping`/`step_joint_position`/`clamped_joint_target` are plain,
    // `Context`-free functions precisely so the velocity-limiting math can be unit tested
    // without needing a live `Context`/Redis - the same reason `GoToLocationNode` factors
    // `compute_control`/`world_to_anchor_frame` etc. out of its `tick()` instead of testing
    // through a real `Context`.

    #[test]
    fn wants_speed_limited_stepping_is_false_when_speed_is_unset_regardless_of_mode() {
        // (a) Regression: with no `speed` configured, `MoveNode` must never take the
        // speed-limited path - `tick()` always falls through to the unchanged
        // static-publish/duration-based-success behavior, in both controller modes.
        assert!(!wants_speed_limited_stepping(None, &ControllerMode::Locomotion));
        assert!(!wants_speed_limited_stepping(
            None,
            &ControllerMode::Joints {
                joint_names: vec!["pan".to_string()]
            }
        ));
    }

    #[test]
    fn wants_speed_limited_stepping_is_false_in_locomotion_mode_even_with_speed_set() {
        // Locomotion's `Twist` is already a velocity command, naturally smooth - `speed` is a
        // joints-mode-only concept.
        assert!(!wants_speed_limited_stepping(Some(0.5), &ControllerMode::Locomotion));
    }

    #[test]
    fn wants_speed_limited_stepping_is_true_in_joints_mode_with_speed_set() {
        assert!(wants_speed_limited_stepping(
            Some(0.5),
            &ControllerMode::Joints {
                joint_names: vec!["pan".to_string()]
            }
        ));
    }

    #[test]
    fn step_joint_position_moves_toward_target_by_at_most_max_step() {
        // (b) A single tick moves the published position by at most `speed * dt` toward the
        // target, starting from a given simulated current-feedback value.
        let (stepped, arrived) = step_joint_position(0.0, 1.0, 0.1, JOINT_POSITION_TOLERANCE_RAD);
        assert!((stepped - 0.1).abs() < f32::EPSILON);
        assert!(!arrived);
    }

    #[test]
    fn step_joint_position_steps_toward_target_from_either_side() {
        let (stepped, _) = step_joint_position(1.0, 0.0, 0.1, JOINT_POSITION_TOLERANCE_RAD);
        assert!((stepped - 0.9).abs() < f32::EPSILON);
    }

    #[test]
    fn step_joint_position_never_overshoots_the_target() {
        // (c) Overshoot protection: stepping never publishes past the target even if
        // `speed * dt` would exceed the remaining distance.
        let (stepped, arrived) = step_joint_position(0.95, 1.0, 10.0, JOINT_POSITION_TOLERANCE_RAD);
        assert_eq!(stepped, 1.0);
        assert!(arrived);
    }

    #[test]
    fn step_joint_position_never_overshoots_the_target_stepping_downward() {
        let (stepped, arrived) = step_joint_position(-0.95, -1.0, 10.0, JOINT_POSITION_TOLERANCE_RAD);
        assert_eq!(stepped, -1.0);
        assert!(arrived);
    }

    #[test]
    fn step_joint_position_reports_arrived_once_within_tolerance() {
        // (d) The node reaches `Success` once the stepped position is within tolerance of the
        // target - exercised here at the pure-function level `tick_speed_limited` builds on.
        let (_, arrived) = step_joint_position(0.995, 1.0, 0.1, JOINT_POSITION_TOLERANCE_RAD);
        assert!(arrived);

        let (_, not_yet_arrived) = step_joint_position(0.9, 1.0, 0.0, JOINT_POSITION_TOLERANCE_RAD);
        assert!(!not_yet_arrived);
    }

    #[test]
    fn step_joint_position_treats_max_step_as_a_magnitude() {
        // The caller always supplies a non-negative `speed * dt`, but guard against a negative
        // `max_step` still working sanely (it's clamped via `.abs()`) rather than inverting the
        // step direction.
        let (stepped, _) = step_joint_position(0.0, 1.0, -0.1, JOINT_POSITION_TOLERANCE_RAD);
        assert!((stepped - 0.1).abs() < f32::EPSILON);
    }

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

        assert_eq!(clamped_joint_target("pan", 5.0), 5.0);

        clear_controller_env();
    }

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

        assert_eq!(clamped_joint_target("pan", 1000.0), 1.0);
        assert_eq!(clamped_joint_target("pan", -1000.0), -1.0);

        clear_controller_env();
    }

    // --- `dynamic_safety_timeout` (sentry pan-sweep truncation fix) ---
    //
    // The production bug this corrects: a static `duration_secs` left over from before
    // `speed` existed cut off a slower, now-correctly-smooth sweep before it reached the real
    // joint extremes. These tests exercise the dynamic-extension logic directly, the same
    // `Context`-free pattern the rest of this module's pure-function tests use.

    #[test]
    fn dynamic_safety_timeout_extends_a_too_short_static_duration_when_the_real_range_needs_more_time() {
        let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        clear_controller_env();
        // A full sweep across this range at this speed takes 10s minimum - well past the
        // leftover 3s static `duration_secs` that caused the production truncation.
        env::set_var(
            ACTUATOR_JOINT_LIMITS_ENV_VAR,
            r#"[{"joint_id":"pan","min_angle_rad":-5.0,"max_angle_rad":5.0}]"#,
        );

        let effective = dynamic_safety_timeout(&["pan".to_string()], 1.0, Duration::from_secs_f64(3.0));

        // required_secs = 10.0 / 1.0 = 10.0, * 1.5 margin = 15.0 - well above the static 3.0.
        assert_eq!(effective, Duration::from_secs_f64(15.0));

        clear_controller_env();
    }

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

        // required_secs = 2.0 / 1.0 = 2.0, * 1.5 margin = 3.0 - the generous static duration
        // (60s) stays in effect since it's already larger.
        let effective = dynamic_safety_timeout(&["pan".to_string()], 1.0, Duration::from_secs_f64(60.0));

        assert_eq!(effective, Duration::from_secs_f64(60.0));

        clear_controller_env();
    }

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

        // pan: 2.0 / 1.0 = 2.0s, tilt: 10.0 / 1.0 = 10.0s - the slower joint (tilt) decides.
        let effective = dynamic_safety_timeout(
            &["pan".to_string(), "tilt".to_string()],
            1.0,
            Duration::from_secs_f64(0.1),
        );

        assert_eq!(effective, Duration::from_secs_f64(15.0));

        clear_controller_env();
    }

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

        let effective = dynamic_safety_timeout(&["pan".to_string()], 0.0, Duration::from_secs_f64(3.0));

        assert_eq!(effective, Duration::from_secs_f64(3.0));

        clear_controller_env();
    }

    #[test]
    fn dynamic_safety_timeout_falls_back_to_static_when_no_joint_limits_are_resolvable() {
        let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        clear_controller_env();
        // No `ACTUATOR_JOINT_LIMITS_ENV_VAR` configured at all - nothing to compute a dynamic
        // requirement from, so this must not crash/panic and must preserve the static value.
        let effective = dynamic_safety_timeout(&["pan".to_string()], 1.0, Duration::from_secs_f64(3.0));

        assert_eq!(effective, Duration::from_secs_f64(3.0));

        clear_controller_env();
    }

    #[tokio::test]
    #[ignore = "Requires Redis infrastructure"]
    async fn move_node_without_speed_publishes_the_static_target_and_succeeds_on_duration() {
        // (a) Regression, at the full `tick()` level: with no `speed` configured, behavior is
        // unchanged from before this feature existed - static publish every tick, `Success`
        // only once `duration_secs` elapses. Locomotion mode needs no joint feedback at all,
        // so this exercises the real `tick()` without requiring a live Redis-backed `Context`.
        //
        // Clears the controller env vars (so this runs in the `Locomotion` default) and drops
        // the guard immediately rather than holding it across the `await`s below - clippy
        // (`await_holding_lock`) flags a `MutexGuard` held across an await point, and this test
        // is `#[ignore]`d/run serially via Redis-gated CI anyway, not concurrently with the
        // env-mutating `resolve_movement_output_*` tests above.
        {
            let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
            clear_controller_env();
        }

        let mut node = MoveNode::from_json(serde_json::json!({
            "linear": 1.0,
            "angular": 0.5,
            "duration_secs": 0.05
        }))
        .unwrap();

        let ctx = crate::test_helpers::create_test_context("move_node_without_speed").await;
        assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Running);

        tokio::time::sleep(Duration::from_millis(80)).await;
        assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Success);

        let _guard = CONTROLLER_ENV_LOCK.lock().unwrap_or_else(|e| e.into_inner());
        clear_controller_env();
    }
}