FlyShotHost/tests/Flyshot.Core.Tests/OfflinePlanTests.cs

using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Export;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
using Xunit.Abstractions;

namespace Flyshot.Core.Tests;

/// <summary>
/// 离线轨迹生成入口，与 Python demo 的 CLI 对齐。
///
/// 为什么放在测试项目里？
/// ---
/// 测试项目已经引用了所有需要的模块（Config、Domain、Planning、Triggering），
/// 不需要额外创建 Console 项目。通过 dotnet test --filter 可以直接调用，
/// 也能利用 xUnit 的断言做结果验证。
/// </summary>
public sealed class OfflinePlanTests
{
    /// <summary>
    /// 使用预设参数生成离线轨迹，输出到 analysis/output/dotnet/。
    /// </summary>
    [Theory]
    [InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "EOL9_EAU_90", false, 1.0)]
    [InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "EOL9_EAU_90", true, 0.9)]
    [InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "EOL10_EAU_0", false, 1.0)]
    public void GenerateTrajectory_MatchesPythonDemo(
        string configPath,
        string trajName,
        bool useSelfAdapt,
        double speedRatio)
    {
        var workspaceRoot = GetWorkspaceRoot();
        var resolvedConfigPath = Path.Combine(workspaceRoot, configPath);
        var outputDir = Path.Combine(workspaceRoot, "analysis", "output", "dotnet", $"{trajName}_sr{speedRatio:F2}_{(useSelfAdapt ? "adapt" : "icsp")}");
        Directory.CreateDirectory(outputDir);

        // 1. 加载配置和模型。
        var loadedConfig = new RobotConfigLoader().Load(resolvedConfigPath, repoRoot: workspaceRoot);
        var program = loadedConfig.Programs[trajName];
        var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "LR_Mate_200iD_7L.json");
        var loadedModel = new RobotModelLoader().LoadProfileAndKinematics(
            resolvedRobotModelPath,
            loadedConfig.Robot.AccLimitScale,
            loadedConfig.Robot.JerkLimitScale);
        var baseProfile = loadedModel.Profile;
        var kinematicsModel = loadedModel.KinematicsModel;

        // 2. 应用 speed_ratio 缩放。
        var scaledProfile = ScaleRobotProfile(baseProfile, speedRatio);

        // 3. 规划轨迹。
        var request = new TrajectoryRequest(
            robot: scaledProfile,
            program: program,
            method: useSelfAdapt ? PlanningMethod.SelfAdaptIcsp : PlanningMethod.Icsp);

        PlannedTrajectory trajectory;
        if (useSelfAdapt)
        {
            trajectory = new SelfAdaptIcspPlanner().Plan(request, loadedConfig.Robot.AdaptIcspTryNum);
        }
        else
        {
            trajectory = new ICspPlanner().Plan(request);
        }

        // 4. 生成触发时间轴。
        var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
            .Build(trajectory, holdCycles: loadedConfig.Robot.IoKeepCycles, samplePeriod: TimeSpan.FromMilliseconds(16));

        // 5. 稠密采样。
        double samplePeriod = 0.016;
        var jointDense = TrajectorySampler.SampleJointTrajectory(trajectory, samplePeriod);
        var cartDense = TrajectorySampler.SampleCartesianTrajectory(trajectory, kinematicsModel, samplePeriod);

        // 6. 导出文件。
        TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.demo.txt"), jointDense);
        TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.demo.txt"), cartDense);
        TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.demo.json"), timeline.ShotEvents);

        // 7. 打印统计信息到测试输出。
        _testOutputHelper.WriteLine($"traj_name={trajName}");
        _testOutputHelper.WriteLine($"speed_ratio={speedRatio:F6}");
        _testOutputHelper.WriteLine($"duration={trajectory.WaypointTimes[^1]:F6}");
        _testOutputHelper.WriteLine($"joint_dense_rows={jointDense.Count}");
        _testOutputHelper.WriteLine($"cart_dense_rows={cartDense.Count}");
        _testOutputHelper.WriteLine($"shot_events={timeline.ShotEvents.Count}");
        _testOutputHelper.WriteLine($"output_dir={outputDir}");

        // 最小验证：输出文件存在且非空。
        Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.demo.txt")));
        Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.demo.txt")));
        Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.demo.json")));
        Assert.True(jointDense.Count > 0);
        Assert.True(cartDense.Count > 0);
    }

    private readonly ITestOutputHelper _testOutputHelper;

    public OfflinePlanTests(ITestOutputHelper testOutputHelper)
    {
        _testOutputHelper = testOutputHelper;
    }

    /// <summary>
    /// 按 speed_ratio 缩放机器人关节限制。
    ///
    /// 缩放律：
    /// - 速度按一次方：speedRatio^1
    /// - 加速度按平方：speedRatio^2
    /// - jerk 按立方：speedRatio^3
    ///
    /// 为什么加速度是平方？
    /// ---
    /// 如果把时间轴整体缩放 1/speedRatio 倍，速度按 speedRatio 缩放，加速度按 speedRatio^2 缩放。
    /// 所以降低 speed_ratio 意味着降低速度、更大幅度地降低加速度和 jerk，轨迹更平滑。
    /// </summary>
    private static RobotProfile ScaleRobotProfile(RobotProfile source, double speedRatio)
    {
        return new RobotProfile(
            name: source.Name,
            modelPath: source.ModelPath,
            degreesOfFreedom: source.DegreesOfFreedom,
            jointLimits: source.JointLimits
                .Select(limit => new JointLimit(
                    limit.JointName,
                    limit.VelocityLimit * speedRatio,
                    limit.AccelerationLimit * speedRatio * speedRatio,
                    limit.JerkLimit * speedRatio * speedRatio * speedRatio))
                .ToArray(),
            jointCouplings: source.JointCouplings,
            servoPeriod: source.ServoPeriod,
            triggerPeriod: source.TriggerPeriod);
    }

    /// <summary>
    /// 定位父工作区根目录。
    /// </summary>
    private static string GetWorkspaceRoot()
    {
        var current = new DirectoryInfo(AppContext.BaseDirectory);
        while (current is not null)
        {
            if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
            {
                return Path.GetFullPath(Path.Combine(current.FullName, ".."));
            }

            current = current.Parent;
        }

        throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
    }
}