✨ feat(native-planning): 添加 Rust ICSP 原生规划核心及 C# 互操作集成

* 新增 Rust 原生 ICSP 规划库 (Cargo 项目, CubicSpline 算法, FFI 接口)
* 新增 Flyshot.Native.Planning C# 互操作项目 (P/Invoke 封装)
* 更新 ControllerClientCompat 适配新的规划接口
* 添加构建脚本 (Copy-NativePlanningArtifacts.ps1)
* 新增互操作测试及一致性对比测试
* 更新解决方案文件和设计文档
This commit is contained in:
2026-06-23 15:36:01 +08:00
parent bb21e0923d
commit 9c56ee825e
30 changed files with 2674 additions and 2 deletions

View File

@@ -0,0 +1,160 @@
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Native.Planning;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 Rust ICSP 与现有 C# 基准实现保持逐项数值一致。
/// </summary>
public sealed class RustIcspPlannerParityTests
{
/// <summary>
/// 验证基础六自由度兼容样本的时间轴、段时长、段缩放和迭代信息完全对齐。
/// </summary>
[Fact]
public void RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture()
{
TrajectoryRequest request = CreateCompatibilityRequest();
PlannedTrajectory managed = new ICspPlanner().Plan(request);
PlannedTrajectory native = new RustIcspPlanner().Plan(request);
Assert.Equal(PlanningMethod.Icsp, managed.Method);
Assert.Equal(PlanningMethod.IcspRust, native.Method);
AssertTrajectoryMatches(managed, native, tolerance: 1e-9);
}
/// <summary>
/// 验证关闭最终 scale 拒绝后,多自由度非均匀路点仍与托管算法逐项对齐。
/// </summary>
[Fact]
public void RustIcspPlanner_MatchesManagedIcspPlanner_ForNonUniformFixture()
{
TrajectoryRequest request = CreateNonUniformRequest();
var options = NativeIcspPlannerOptions.Default with { EnforceFinalScale = false };
PlannedTrajectory managed = new ICspPlanner(enforceFinalScale: false).Plan(request);
PlannedTrajectory native = new RustIcspPlanner(options).Plan(request);
Assert.Equal(PlanningMethod.Icsp, managed.Method);
Assert.Equal(PlanningMethod.IcspRust, native.Method);
AssertTrajectoryMatches(managed, native, tolerance: 1e-9);
}
/// <summary>
/// 创建与现有 PlanningCompatibilityTests 基础用例一致的六自由度请求。
/// </summary>
/// <returns>可由默认 ICSP 配置收敛的稳定规划请求。</returns>
private static TrajectoryRequest CreateCompatibilityRequest()
{
return CreateRequest(
velocityLimits: [1, 1, 1, 1, 1, 1],
accelerationLimits: [2, 2, 2, 2, 2, 2],
jerkLimits: [10, 10, 10, 10, 10, 10],
waypointRows:
[
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.4, 0.1, 0.0, 0.0, 0.0, 0.0],
[0.8, 0.3, 0.0, 0.0, 0.0, 0.0],
[1.0, 0.2, 0.0, 0.0, 0.0, 0.0]
]);
}
/// <summary>
/// 创建用于覆盖多自由度、非均匀段距离和不同约束的规划请求。
/// </summary>
/// <returns>关闭最终 scale 拒绝后可用于数值对拍的稳定请求。</returns>
private static TrajectoryRequest CreateNonUniformRequest()
{
return CreateRequest(
velocityLimits: [1.2, 0.9, 1.5],
accelerationLimits: [2.5, 1.8, 3.0],
jerkLimits: [12.0, 9.0, 15.0],
waypointRows:
[
[0.0, 0.0, 0.0],
[0.2, -0.1, 0.4],
[0.9, 0.3, 0.7],
[1.1, 0.8, 0.2],
[1.6, 0.4, -0.1]
]);
}
/// <summary>
/// 根据逐关节限制和路点矩阵创建完整领域请求。
/// </summary>
/// <param name="velocityLimits">逐关节速度上限。</param>
/// <param name="accelerationLimits">逐关节加速度上限。</param>
/// <param name="jerkLimits">逐关节 jerk 上限。</param>
/// <param name="waypointRows">按路点排列的关节位置矩阵。</param>
/// <returns>可同时传给 C# 与 Rust 规划器的请求。</returns>
private static TrajectoryRequest CreateRequest(
IReadOnlyList<double> velocityLimits,
IReadOnlyList<double> accelerationLimits,
IReadOnlyList<double> jerkLimits,
IReadOnlyList<double[]> waypointRows)
{
var robot = new RobotProfile(
name: "RustParityRobot",
modelPath: "Models/RustParity.robot",
degreesOfFreedom: velocityLimits.Count,
jointLimits: velocityLimits
.Select((velocity, index) => new JointLimit(
$"J{index + 1}",
velocity,
accelerationLimits[index],
jerkLimits[index]))
.ToArray(),
jointCouplings: [],
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
var program = new FlyshotProgram(
name: "rust-parity",
waypoints: waypointRows.Select(row => new JointWaypoint(row)).ToArray(),
shotFlags: Enumerable.Repeat(false, waypointRows.Count).ToArray(),
offsetValues: Enumerable.Repeat(0, waypointRows.Count).ToArray(),
addressGroups: Enumerable.Range(0, waypointRows.Count)
.Select(_ => new IoAddressGroup([]))
.ToArray());
return new TrajectoryRequest(robot, program, PlanningMethod.Icsp);
}
/// <summary>
/// 逐字段比较两套规划实现的数值结果。
/// </summary>
/// <param name="managed">现有 C# ICSP 基准结果。</param>
/// <param name="native">Rust ICSP 原生结果。</param>
/// <param name="tolerance">所有浮点数组和 threshold 使用的绝对误差。</param>
private static void AssertTrajectoryMatches(
PlannedTrajectory managed,
PlannedTrajectory native,
double tolerance)
{
Assert.Equal(managed.Iterations, native.Iterations);
Assert.Equal(managed.WaypointTimes.Count, native.WaypointTimes.Count);
Assert.Equal(managed.SegmentDurations.Count, native.SegmentDurations.Count);
Assert.Equal(managed.SegmentScales.Count, native.SegmentScales.Count);
Assert.InRange(Math.Abs(managed.Threshold - native.Threshold), 0.0, tolerance);
AssertSequenceMatches(managed.WaypointTimes, native.WaypointTimes, tolerance);
AssertSequenceMatches(managed.SegmentDurations, native.SegmentDurations, tolerance);
AssertSequenceMatches(managed.SegmentScales, native.SegmentScales, tolerance);
}
/// <summary>
/// 按相同索引比较两个浮点序列的绝对误差。
/// </summary>
/// <param name="expected">托管基准序列。</param>
/// <param name="actual">Rust 原生序列。</param>
/// <param name="tolerance">允许的最大绝对误差。</param>
private static void AssertSequenceMatches(
IReadOnlyList<double> expected,
IReadOnlyList<double> actual,
double tolerance)
{
for (int index = 0; index < expected.Count; index++)
{
Assert.InRange(Math.Abs(expected[index] - actual[index]), 0.0, tolerance);
}
}
}