mecha10_behavior_runtime/actions/

movement.rs

//! 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> {
        // `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.
        if let Some(duration) = self.duration {
            if self.timer.elapsed_since_start(duration) {
                self.last_status = Some(NodeStatus::Success);
                return Ok(NodeStatus::Success);
            }
        }

        let mode = controller_mode();
        if wants_speed_limited_stepping(self.speed, &mode) {
            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 { .. })
}

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

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