ParkingRobot/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationKinematicChartBuilder.cs

using System;
using System.Collections.Generic;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using TrajectoryPlanningVisualization;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

public sealed class TrajectoryObservationKinematicChartBuilder
{
    public IReadOnlyList<VisualizationChart> Build(EmTrajectory trajectory, DirectionSegmentView segment,
        EmPlannerConfiguration configuration)
    {
        if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
        if (segment == null) throw new ArgumentNullException(nameof(segment));
        if (configuration == null) throw new ArgumentNullException(nameof(configuration));

        LsBuildResult ls = BuildLs(trajectory, segment, configuration.Frenet.MaximumProjectionDistanceMeters);
        return new[]
        {
            Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", ls.Points,
                "完整活动方向段投影；投影失败=" + ls.ProjectionFailureCount.ToString(CultureInfo.InvariantCulture)),
            Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points(trajectory, p => p.PathS), ""),
            WithSymmetricLimit("curvature-s", "曲率-距离", "s (m)", "κ (m⁻¹)",
                PointsBySegmentS(trajectory), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond),
            WithSymmetricLimit("curvature-t", "曲率-时间", "t (s)", "κ (m⁻¹)",
                Points(trajectory, p => p.VehicleCurvature), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond),
            Velocity(trajectory, configuration),
            Acceleration(trajectory, configuration),
            Jerk(trajectory, configuration),
            Chart("yaw-rate-t", "横摆角速度", "t (s)", "yaw rate (rad/s)",
                Points(trajectory, p => p.YawRate), "生效配置没有独立横摆角速度上限"),
        };
    }

    public int CountProjectionFailures(EmTrajectory trajectory, DirectionSegmentView segment,
        EmPlannerConfiguration configuration)
    {
        if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
        if (segment == null) throw new ArgumentNullException(nameof(segment));
        if (configuration == null) throw new ArgumentNullException(nameof(configuration));
        return BuildLs(trajectory, segment, configuration.Frenet.MaximumProjectionDistanceMeters).ProjectionFailureCount;
    }

    private static VisualizationChart Velocity(EmTrajectory trajectory, EmPlannerConfiguration configuration)
    {
        IReadOnlyList<VisualizationPoint> points = Points(trajectory, p => p.SignedLongitudinalVelocity);
        return new VisualizationChart("velocity-t", "速度", "t (s)", "v (m/s)", new[]
        {
            new VisualizationSeries("velocity-t-current", "当前轨迹", VisualizationLineStyle.Solid, points),
            Limit("velocity-t-forward-limit", "前进生效限制", points, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond),
            Limit("velocity-t-reverse-limit", "倒车生效限制", points, -configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond),
        });
    }

    private static VisualizationChart Acceleration(EmTrajectory trajectory, EmPlannerConfiguration configuration)
    {
        IReadOnlyList<VisualizationPoint> points = Points(trajectory, p => p.LongitudinalAcceleration);
        return new VisualizationChart("acceleration-t", "加速度", "t (s)", "a (m/s²)", new[]
        {
            new VisualizationSeries("acceleration-t-current", "当前轨迹", VisualizationLineStyle.Solid, points),
            Limit("acceleration-t-positive-limit", "加速生效限制", points, configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared),
            Limit("acceleration-t-negative-limit", "减速生效限制", points, -configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared),
        });
    }

    private static VisualizationChart Jerk(EmTrajectory trajectory, EmPlannerConfiguration configuration)
    {
        var intervals = new List<VisualizationPoint>();
        for (int index = 0; index + 1 < trajectory.Points.Count; index++)
            intervals.Add(new VisualizationPoint(trajectory.Points[index].TimeFromStart,
                trajectory.Points[index].LongitudinalJerk));
        return new VisualizationChart("jerk-t", "加加速度", "t (s)", "j (m/s³)", new[]
        {
            new VisualizationSeries("jerk-t-current", "当前轨迹", VisualizationLineStyle.Solid, intervals),
            Limit("jerk-t-positive-limit", "正向生效限制", intervals, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed),
            Limit("jerk-t-negative-limit", "负向生效限制", intervals, -configuration.Longitudinal.MaximumJerkMetersPerSecondCubed),
        }, "j[i] 表示 [tᵢ, tᵢ₊₁)；末点后无时间区间");
    }

    private static VisualizationChart WithSymmetricLimit(string id, string title, string xAxis, string yAxis,
        IReadOnlyList<VisualizationPoint> points, double limit)
    {
        return new VisualizationChart(id, title, xAxis, yAxis, new[]
        {
            new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points),
            Limit(id + "-positive-limit", "生效限制", points, limit),
            Limit(id + "-negative-limit", "生效限制", points, -limit),
        });
    }

    private static VisualizationSeries Limit(string id, string legend, IReadOnlyList<VisualizationPoint> points, double y)
    {
        double end = points.Count == 0 ? 0d : points[points.Count - 1].X;
        return new VisualizationSeries(id, legend, VisualizationLineStyle.Limit,
            new[] { new VisualizationPoint(0d, y), new VisualizationPoint(end, y) });
    }

    private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis,
        IReadOnlyList<VisualizationPoint> points, string note)
    {
        return new VisualizationChart(id, title, xAxis, yAxis,
            new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) }, note);
    }

    private static IReadOnlyList<VisualizationPoint> Points(EmTrajectory trajectory, Func<EmTrajectoryPoint, double> y)
    {
        var points = new List<VisualizationPoint>();
        foreach (EmTrajectoryPoint point in trajectory.Points)
            points.Add(new VisualizationPoint(point.TimeFromStart, y(point)));
        return points;
    }

    private static IReadOnlyList<VisualizationPoint> PointsBySegmentS(EmTrajectory trajectory)
    {
        var points = new List<VisualizationPoint>();
        foreach (EmTrajectoryPoint point in trajectory.Points)
            points.Add(new VisualizationPoint(point.SegmentLocalS, point.VehicleCurvature));
        return points;
    }

    private static LsBuildResult BuildLs(EmTrajectory trajectory, DirectionSegmentView segment,
        double maximumProjectionDistanceMeters)
    {
        var points = new List<VisualizationPoint>();
        var projector = new FrenetProjector();
        double seed = 0d;
        int failures = 0;
        foreach (EmTrajectoryPoint point in trajectory.Points)
        {
            if (!projector.TryProject(new Pose2D(point.X, point.Y, point.Yaw), segment, 0d, segment.LengthMeters,
                    maximumProjectionDistanceMeters, seed, out FrenetProjection projection))
            {
                failures++;
                continue;
            }
            points.Add(new VisualizationPoint(segment.SourceStartArcLength + projection.ReferenceS,
                projection.LateralOffset));
            seed = projection.ReferenceS;
        }
        return new LsBuildResult(points, failures);
    }

    private sealed class LsBuildResult
    {
        public LsBuildResult(IReadOnlyList<VisualizationPoint> points, int projectionFailureCount)
        {
            Points = points;
            ProjectionFailureCount = projectionFailureCount;
        }

        public IReadOnlyList<VisualizationPoint> Points { get; }
        public int ProjectionFailureCount { get; }
    }
}