ParkingRobot/MultiWheelC/Experiments/PathTrackingTests.cs

using System;
using System.Collections.Generic;
using System.Numerics;
using System.Threading;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MDCSToolBox.Clumsy.Tracks;
using MyParking.Shared;

namespace MultiWheelC
{
    [MovementTest(name = "SendMotion：连续前进4m")]
    public class TestForward4m : MovementTest
    {
        public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
        public float CruiseSpeed = 0.3f;  // 巡航速度上限，单位m/s。
        public int TrialNumber = 1; // 重复实验编号。
        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;
        // 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
        public override void Test()
        {
            if (!MovementTestPreparation.AreWheelsForward())
            {
                return;
            }

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消连续前进4m测试。");
                return;
            }
            var source = new Vector2((float)location.x, (float)location.y);
            // Detour航向单位是度，三角函数需要弧度。
            var headingRadians =
                AngleMath.DegreesToRadians(location.th);
            var destination = new Vector2(
                source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
                source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
            _recorder =
                        new TrackingExperimentRecorder(
                            controllerName: "LegacyGeometricController",
                            trajectoryName: "LegacyStraight4m",
                            trialNumber: TrialNumber,
                            referenceStart: source,
                            referenceEnd: destination,
                            referenceSpeed: CruiseSpeed);
            _recorder.Start();
            try
            {
                _task = new DriveTask(
                new DstTracker
                {
                    Src = source,
                    Dst = destination,
                    CarDirectionBias = 0f,
                    MaxSpeed = CruiseSpeed
                }.Get());
                _task.Wait();
                // 保留少量停车后数据，便于观察速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }
    }

    [MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
    public class TestArcMovement : MovementTest
    {
        public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
        public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
        public int TrialNumber = 1; // 重复实验编号。

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
        public override void Test()
        {
            if (float.IsNaN(RadiusMillimeters) ||
                float.IsInfinity(RadiusMillimeters) ||
                RadiusMillimeters <= 0f ||
                float.IsNaN(CruiseSpeed) ||
                float.IsInfinity(CruiseSpeed) ||
                CruiseSpeed <= 0f)
            {
                Console.WriteLine("圆弧运动测试参数无效。");
                return;
            }

            if (!MovementTestPreparation.AreWheelsForward())
            {
                return;
            }

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消圆弧运动测试。");
                return;
            }

            var source =
                new Vector2((float)location.x, (float)location.y);
            var headingRadians =
                AngleMath.DegreesToRadians(location.th);

            // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
            var center = new Vector2(
                source.X -
                    RadiusMillimeters *
                    (float)Math.Sin(headingRadians),
                source.Y +
                    RadiusMillimeters *
                    (float)Math.Cos(headingRadians));

            // 从圆心指向车辆起点的极角，比车辆切线航向小90°。
            var startRadialAngleDegrees =
                (float)location.th - 90f;

            var controller = new ChassisController
            {
                BaseSpeed = CruiseSpeed
            }.Get();
            controller.FinishSpeed = 0f;

            var arc = new CircularArcTrack(
                center,
                RadiusMillimeters,
                startRadialAngleDegrees,
                startRadialAngleDegrees + 90f,
                direction: 1)
            {
                Speed = CruiseSpeed,
                CarDirectionBias = 0f
            };

            // 左转90°后，圆心到终点的径向方向等于起始车头方向。
            var destination = center + new Vector2(
                RadiusMillimeters *
                    (float)Math.Cos(headingRadians),
                RadiusMillimeters *
                    (float)Math.Sin(headingRadians));

            if (!controller.AddTrack(arc, "LeftArc90Degrees"))
            {
                Console.WriteLine(
                    "左转90°圆弧轨迹添加失败，取消测试。");
                return;
            }

            _recorder = new TrackingExperimentRecorder(
                controllerName: "LegacyGeometricController",
                trajectoryName:
                    $"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: destination,
                referenceSpeed: CruiseSpeed);
            _recorder.Start();

            try
            {
                _task = new DriveTask(controller.Track());
                _task.Wait();

                // 保留少量停车后的样本，用于观察速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        // 停止圆弧运动并保存当前已经采集的实验数据。
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }
    }

    [MovementTest(name = "SendMotion：蟹行直线4m")]
    public class TestCrabForward4m : MovementTest
    {
        public float DistanceMillimeters = 4000f;
        public float CruiseSpeed = 0.2f;
        public int TrialNumber = 1;

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
        public override void Test()
        {
            if (!TryReadStartPose(
                    out var source,
                    out var bodyYawRadians))
                return;

            var motionYaw =
                bodyYawRadians + Math.PI / 2.0;
            var destination = new Vector2(
                source.X +
                    DistanceMillimeters *
                    (float)Math.Cos(motionYaw),
                source.Y +
                    DistanceMillimeters *
                    (float)Math.Sin(motionYaw));

            var tracker = new CrabMotionFrameTracker
            {
                CommandBackend =
                    CrabMotionFrameTracker
                        .ChassisCommandBackend
                        .SendMotion,
                PathKind =
                    CrabMotionFrameTracker
                        .ReferencePathKind.Straight,
                StartPosition = source,
                InitialBodyYawRadians =
                    bodyYawRadians,
                LengthMillimeters =
                    DistanceMillimeters,
                CruiseSpeed = CruiseSpeed
            };

            _recorder = new TrackingExperimentRecorder(
                controllerName:
                    "CrabSendMotionTracker",
                trajectoryName:
                    "CrabStraight4m",
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: destination,
                referenceSpeed: CruiseSpeed,
                referenceMotionFrameYawDegrees: 90f);
            tracker.CommandObserver =
                (vx, vy, omega) =>
                    _recorder?.UpdateBodyCommand(
                        vx,
                        vy,
                        omega);
            _recorder.Start();

            try
            {
                _task = new DriveTask(tracker.Get());
                _task.Wait();
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateBodyCommand(
                    0f,
                    0f,
                    0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateBodyCommand(
                0f,
                0f,
                0f);
            _recorder?.StopAndSave();
        }

        // 读取并校验测试开始时的Detour世界位姿。
        private static bool TryReadStartPose(
            out Vector2 source,
            out double bodyYawRadians)
        {
            var location =
                DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消蟹行直线测试。");
                source = Vector2.Zero;
                bodyYawRadians = 0.0;
                return false;
            }

            source = new Vector2(
                (float)location.x,
                (float)location.y);
            bodyYawRadians =
                AngleMath.DegreesToRadians(location.th);
            return true;
        }
    }

    [MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
    public class TestCrabLeftArc90 : MovementTest
    {
        public float RadiusMillimeters = 2000f;
        public float CruiseSpeed = 0.2f;
        public int TrialNumber = 1;

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
        public override void Test()
        {
            var location =
                DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消蟹行圆弧测试。");
                return;
            }

            var source = new Vector2(
                (float)location.x,
                (float)location.y);
            var bodyYawRadians =
                AngleMath.DegreesToRadians(location.th);
            var tracker = new CrabMotionFrameTracker
            {
                CommandBackend =
                    CrabMotionFrameTracker
                        .ChassisCommandBackend
                        .SendMotion,
                PathKind =
                    CrabMotionFrameTracker
                        .ReferencePathKind.LeftArc,
                StartPosition = source,
                InitialBodyYawRadians =
                    bodyYawRadians,
                RadiusMillimeters =
                    RadiusMillimeters,
                ArcSweepRadians = Math.PI / 2.0,
                CruiseSpeed = CruiseSpeed
            };
            var destination =
                tracker.GetArcDestination();

            _recorder = new TrackingExperimentRecorder(
                controllerName:
                    "CrabSendMotionTracker",
                trajectoryName:
                    $"CrabLeftArc90_R{RadiusMillimeters:0}mm",
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: destination,
                referenceSpeed: CruiseSpeed,
                referenceMotionFrameYawDegrees: 90f);
            tracker.CommandObserver =
                (vx, vy, omega) =>
                    _recorder?.UpdateBodyCommand(
                        vx,
                        vy,
                        omega);
            _recorder.Start();

            try
            {
                _task = new DriveTask(tracker.Get());
                _task.Wait();
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateBodyCommand(
                    0f,
                    0f,
                    0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateBodyCommand(
                0f,
                0f,
                0f);
            _recorder?.StopAndSave();
        }
    }

    [MovementTest(name = "SendMotion：4m S型曲线")]
    public class TestSCurve4m : MovementTest
    {
        public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
        public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
        public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
        public int TrialNumber = 1; // 重复实验编号。

        private DriveTask _task;
        private TrackingExperimentRecorder _recorder;

        // 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。
        public override void Test()
        {
            if (float.IsNaN(LengthMillimeters) ||
                float.IsInfinity(LengthMillimeters) ||
                LengthMillimeters <= 0f ||
                float.IsNaN(LateralOffsetMillimeters) ||
                float.IsInfinity(LateralOffsetMillimeters) ||
                LateralOffsetMillimeters <= 0f ||
                float.IsNaN(CruiseSpeed) ||
                float.IsInfinity(CruiseSpeed) ||
                CruiseSpeed <= 0f)
            {
                Console.WriteLine("S型曲线测试参数无效。");
                return;
            }

            if (!MovementTestPreparation.AreWheelsForward())
                return;

            var location = DetourInterface.getCartLocation();
            if (double.IsNaN(location.x) ||
                double.IsInfinity(location.x) ||
                double.IsNaN(location.y) ||
                double.IsInfinity(location.y) ||
                double.IsNaN(location.th) ||
                double.IsInfinity(location.th))
            {
                Console.WriteLine(
                    "Detour当前位姿无效，取消4m S型曲线测试。");
                return;
            }

            var source =
                new Vector2((float)location.x, (float)location.y);
            var headingRadians =
                AngleMath.DegreesToRadians(location.th);
            var length = LengthMillimeters;
            var offset = LateralOffsetMillimeters;

            // 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
            // 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
            var firstControlPoints = new List<Vector2>
            {
                LocalToWorld(source, headingRadians, 0f, 0f),
                LocalToWorld(
                    source, headingRadians,
                    length / 12f, 0f),
                LocalToWorld(
                    source, headingRadians,
                    length / 6f, offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 0.25f, offset)
            };
            var secondControlPoints = new List<Vector2>
            {
                LocalToWorld(
                    source, headingRadians,
                    length * 0.25f, offset),
                LocalToWorld(
                    source, headingRadians,
                    length / 3f, offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 2f / 3f, -offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 0.75f, -offset)
            };
            var thirdControlPoints = new List<Vector2>
            {
                LocalToWorld(
                    source, headingRadians,
                    length * 0.75f, -offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 5f / 6f, -offset),
                LocalToWorld(
                    source, headingRadians,
                    length * 11f / 12f, 0f),
                LocalToWorld(
                    source, headingRadians,
                    length, 0f)
            };

            var firstTrack = new BezierTrack(firstControlPoints)
            {
                Speed = CruiseSpeed,
                CarDirectionBias = 0f
            };
            var secondTrack = new BezierTrack(secondControlPoints)
            {
                Speed = CruiseSpeed,
                CarDirectionBias = 0f
            };
            var thirdTrack = new BezierTrack(thirdControlPoints)
            {
                Speed = CruiseSpeed,
                CarDirectionBias = 0f
            };

            var controller = new ChassisController
            {
                BaseSpeed = CruiseSpeed
            }.Get();
            controller.FinishSpeed = 0f;

            if (!controller.AddTrack(
                    firstTrack,
                    "SCurve4m-Part1") ||
                !controller.AddTrack(
                    secondTrack,
                    "SCurve4m-Part2") ||
                !controller.AddTrack(
                    thirdTrack,
                    "SCurve4m-Part3"))
            {
                Console.WriteLine(
                    "4m S型曲线轨迹添加失败，取消测试。");
                return;
            }

            var destination =
                LocalToWorld(
                    source,
                    headingRadians,
                    length,
                    0f);
            _recorder = new TrackingExperimentRecorder(
                controllerName: "LegacyGeometricController",
                trajectoryName:
                    $"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
                trialNumber: TrialNumber,
                referenceStart: source,
                referenceEnd: destination,
                referenceSpeed: CruiseSpeed);
            _recorder.Start();

            try
            {
                _task = new DriveTask(controller.Track());
                _task.Wait();

                // 保留少量停止后的数据，用于观察速度是否回到零。
                Thread.Sleep(300);
            }
            finally
            {
                _task?.Stop();
                _recorder?.UpdateCommand(0f, 0f);
                _recorder?.StopAndSave();
                _task = null;
                _recorder = null;
            }
        }

        // 停止S型曲线测试并保存当前已经采集的数据。
        public override void TestStop()
        {
            _task?.Stop();
            _recorder?.UpdateCommand(0f, 0f);
            _recorder?.StopAndSave();
        }

        // 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
        private static Vector2 LocalToWorld(
            Vector2 origin,
            double headingRadians,
            float localX,
            float localY)
        {
            var cos = (float)Math.Cos(headingRadians);
            var sin = (float)Math.Sin(headingRadians);

            return new Vector2(
                origin.X + localX * cos - localY * sin,
                origin.Y + localX * sin + localY * cos);
        }
    }
}