ParkingRobot/Simulation/Core/SimulationVehicle.cs

using MyParking.Shared;
using MyParking.Simulation.Models;

namespace MyParking.Simulation.Core;

/// <summary>
/// 保存单辆四舵轮停车机器人的离线仿真状态。
/// </summary>
public sealed class SimulationVehicle
{
    public const double BodyLengthMeters = 1.472;
    public const double BodyWidthMeters = 0.948;

    // 当前MDCSToolBox.dll的MultiWheelChassisInitializer运行时轮位：
    // X=±750mm、Y=±500mm，舵角机械限位为±120°。
    private const double WheelX = 0.75;
    private const double WheelY = 0.5;
    private const double MaximumBodyAcceleration = 0.6;
    private const double MaximumAngularAcceleration = 0.8;

    private readonly List<VirtualSteerWheel> _wheels;
    private readonly double _initialX;
    private readonly double _initialY;
    private readonly double _initialYaw;

    private Twist2D _targetBodyTwist = Twist2D.Zero;
    private double _actualVx;
    private double _actualVy;
    private double _actualOmega;

    public SimulationVehicle(
        int vehicleId,
        double initialX,
        double initialY,
        double initialYaw)
    {
        VehicleId = vehicleId;
        _initialX = initialX;
        _initialY = initialY;
        _initialYaw = initialYaw;

        _wheels =
        [
            new VirtualSteerWheel("左前", WheelX, WheelY),
            new VirtualSteerWheel("右前", WheelX, -WheelY),
            new VirtualSteerWheel("左后", -WheelX, WheelY),
            new VirtualSteerWheel("右后", -WheelX, -WheelY)
        ];

        Reset();
    }

    public int VehicleId { get; }

    public string Mode { get; private set; } = "Normal";

    public double XMeters { get; private set; }

    public double YMeters { get; private set; }

    public double YawRadians { get; private set; }

    public bool ModeReady => _wheels.All(wheel => wheel.IsAligned);

    /// <summary>
    /// 停车并切换舵轮准备模式。
    /// </summary>
    public bool SetMode(string mode)
    {
        Stop();

        var success = mode switch
        {
            "Normal" => PrepareParallelDirection(0.0),
            "CrabLeft" => PrepareParallelDirection(90.0),
            "CrabRight" => PrepareParallelDirection(-90.0),
            "Spin" => PrepareSpinDirection(),
            _ => false
        };

        if (success)
            Mode = mode;

        return success;
    }

    /// <summary>
    /// 按当前模式发送前进或后退命令。
    /// </summary>
    public bool Move(double directionSign)
    {
        if (!ModeReady)
            return false;

        const double linearSpeed = 0.35;
        const double angularSpeed = 0.45;

        var command = Mode switch
        {
            "Normal" => new Twist2D(
                directionSign * linearSpeed, 0.0, 0.0),
            "CrabLeft" => new Twist2D(
                0.0, directionSign * linearSpeed, 0.0),
            "CrabRight" => new Twist2D(
                0.0, -directionSign * linearSpeed, 0.0),
            "Spin" => new Twist2D(
                0.0, 0.0, directionSign * angularSpeed),
            _ => Twist2D.Zero
        };

        return ApplyCommand(
            new ChassisCommand(VehicleId, command));
    }

    /// <summary>
    /// 在当前运动模式下增加逆时针或顺时针转动。
    /// </summary>
    public bool Turn(double directionSign)
    {
        if (!ModeReady || Mode == "Spin")
            return false;

        var command = new Twist2D(
            _targetBodyTwist.VxMetersPerSecond,
            _targetBodyTwist.VyMetersPerSecond,
            directionSign * 0.28);

        return ApplyCommand(
            new ChassisCommand(VehicleId, command));
    }

    /// <summary>
    /// 将网页虚拟遥控器的油门和转向组合为连续车体速度命令。
    /// </summary>
    public bool ManualDrive(
        double throttle,
        double steering,
        double speedScale,
        double steeringScale)
    {
        if (!AreFinite(
                throttle,
                steering,
                speedScale,
                steeringScale))
        {
            return false;
        }

        throttle = Math.Clamp(throttle, -1.0, 1.0);
        steering = Math.Clamp(steering, -1.0, 1.0);
        speedScale = Math.Clamp(speedScale, 0.0, 1.0);
        steeringScale = Math.Clamp(steeringScale, 0.0, 1.0);

        if (Math.Abs(throttle) < 0.001 &&
            Math.Abs(steering) < 0.001)
        {
            Stop();
            return true;
        }

        if (!ModeReady)
            return false;

        const double maximumLinearSpeed = 0.6;
        const double maximumAngularSpeed = 0.7;

        var linearSpeed =
            throttle * maximumLinearSpeed * speedScale;
        var angularSpeed =
            steering * maximumAngularSpeed * steeringScale;

        var twist = Mode switch
        {
            "Normal" => new Twist2D(
                linearSpeed,
                0.0,
                angularSpeed),
            "CrabLeft" => new Twist2D(
                0.0,
                linearSpeed,
                angularSpeed),
            "CrabRight" => new Twist2D(
                0.0,
                -linearSpeed,
                angularSpeed),
            "Spin" => new Twist2D(
                0.0,
                0.0,
                angularSpeed),
            _ => Twist2D.Zero
        };

        return ApplyCommand(
            new ChassisCommand(VehicleId, twist));
    }

    /// <summary>
    /// 应用统一车体速度命令并分解为四个舵轮速度向量。
    /// </summary>
    public bool ApplyCommand(ChassisCommand command)
    {
        if (command.VehicleId != VehicleId)
            return false;

        var twist = command.BodyTwist;
        var wheelCommands = _wheels.Select(wheel =>
        {
            var wheelVx =
                twist.VxMetersPerSecond -
                twist.OmegaRadiansPerSecond * wheel.YMeters;

            var wheelVy =
                twist.VyMetersPerSecond +
                twist.OmegaRadiansPerSecond * wheel.XMeters;

            return (Wheel: wheel, Vx: wheelVx, Vy: wheelVy);
        }).ToArray();

        foreach (var item in wheelCommands)
        {
            if (!item.Wheel.SetVelocityVector(
                    item.Vx,
                    item.Vy))
            {
                Stop();
                return false;
            }
        }

        _targetBodyTwist = twist;
        return true;
    }

    /// <summary>
    /// 将车辆目标速度设置为零并保持当前舵轮角度。
    /// </summary>
    public void Stop()
    {
        _targetBodyTwist = Twist2D.Zero;
        foreach (var wheel in _wheels)
            wheel.Stop();
    }

    /// <summary>
    /// 更新舵轮反馈和车辆世界位姿。
    /// </summary>
    public void Step(double deltaTimeSeconds)
    {
        foreach (var wheel in _wheels)
            wheel.Step(deltaTimeSeconds);

        var canMove = _wheels.All(wheel => wheel.IsAligned);
        var targetVx = canMove
            ? _targetBodyTwist.VxMetersPerSecond
            : 0.0;
        var targetVy = canMove
            ? _targetBodyTwist.VyMetersPerSecond
            : 0.0;
        var targetOmega = canMove
            ? _targetBodyTwist.OmegaRadiansPerSecond
            : 0.0;

        _actualVx = MoveTowards(
            _actualVx,
            targetVx,
            MaximumBodyAcceleration * deltaTimeSeconds);

        _actualVy = MoveTowards(
            _actualVy,
            targetVy,
            MaximumBodyAcceleration * deltaTimeSeconds);

        _actualOmega = MoveTowards(
            _actualOmega,
            targetOmega,
            MaximumAngularAcceleration * deltaTimeSeconds);

        var cos = Math.Cos(YawRadians);
        var sin = Math.Sin(YawRadians);

        var worldVx = cos * _actualVx - sin * _actualVy;
        var worldVy = sin * _actualVx + cos * _actualVy;

        XMeters += worldVx * deltaTimeSeconds;
        YMeters += worldVy * deltaTimeSeconds;
        YawRadians = FrameTransform2D.NormalizeAngle(
            YawRadians + _actualOmega * deltaTimeSeconds);
    }

    /// <summary>
    /// 恢复车辆初始位置和舵轮状态。
    /// </summary>
    public void Reset()
    {
        XMeters = _initialX;
        YMeters = _initialY;
        YawRadians = _initialYaw;
        Mode = "Normal";
        _targetBodyTwist = Twist2D.Zero;
        _actualVx = 0.0;
        _actualVy = 0.0;
        _actualOmega = 0.0;

        foreach (var wheel in _wheels)
            wheel.Reset();
    }

    /// <summary>
    /// 创建供网页读取的不可变状态快照。
    /// </summary>
    public VehicleStateDto GetSnapshot()
    {
        return new VehicleStateDto(
            VehicleId,
            XMeters,
            YMeters,
            YawRadians,
            BodyLengthMeters,
            BodyWidthMeters,
            Mode,
            ModeReady,
            new TwistStateDto(
                _targetBodyTwist.VxMetersPerSecond,
                _targetBodyTwist.VyMetersPerSecond,
                _targetBodyTwist.OmegaRadiansPerSecond),
            new TwistStateDto(
                _actualVx,
                _actualVy,
                _actualOmega),
            _wheels.Select(wheel =>
                new WheelStateDto(
                    wheel.Name,
                    wheel.XMeters,
                    wheel.YMeters,
                    wheel.TargetAngleDegrees,
                    wheel.ActualAngleDegrees,
                    wheel.TargetSpeedMetersPerSecond,
                    wheel.ActualSpeedMetersPerSecond,
                    wheel.IsAligned)).ToArray());
    }

    private bool PrepareParallelDirection(double targetAngleDegrees)
    {
        return _wheels.All(wheel =>
            wheel.PrepareDirection(targetAngleDegrees));
    }

    private bool PrepareSpinDirection()
    {
        var success = true;

        foreach (var wheel in _wheels)
        {
            var vx = -wheel.YMeters;
            var vy = wheel.XMeters;
            success &= wheel.SetVelocityVector(vx, vy);
            wheel.Stop();
        }

        return success;
    }

    private static double MoveTowards(
        double current,
        double target,
        double maximumChange)
    {
        var difference = target - current;
        if (Math.Abs(difference) <= maximumChange)
            return target;

        return current + Math.Sign(difference) * maximumChange;
    }

    private static bool AreFinite(params double[] values)
    {
        return values.All(value =>
            !double.IsNaN(value) &&
            !double.IsInfinity(value));
    }
}